Accuracy3D.java

/*
 * Copyright (C) 2018 Alberto Irurueta Carro (alberto@irurueta.com)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *         http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

package com.irurueta.geometry;

import com.irurueta.algebra.AlgebraException;
import com.irurueta.algebra.ArrayUtils;
import com.irurueta.algebra.Matrix;
import com.irurueta.algebra.NonSymmetricPositiveDefiniteMatrixException;

/**
 * Contains methods to convert covariance matrices into ellipsoids representing accuracy
 * with requested confidence.
 */
public class Accuracy3D extends Accuracy {

    /**
     * Constructor.
     */
    public Accuracy3D() {
        super();
    }

    /**
     * Constructor.
     *
     * @param covarianceMatrix covariance matrix to be set. Must be 3x3 and positive
     *                         definite.
     * @throws IllegalArgumentException                    if provided matrix is not square (it must also be
     *                                                     positive definite to be properly converted to an ellipsoid).
     * @throws NonSymmetricPositiveDefiniteMatrixException if provided matrix is not symmetric and
     *                                                     positive definite.
     */
    public Accuracy3D(final Matrix covarianceMatrix) throws NonSymmetricPositiveDefiniteMatrixException {
        super(covarianceMatrix);
    }

    /**
     * Constructor.
     *
     * @param confidence confidence of provided accuracy of an estimated position.
     * @throws IllegalArgumentException if provided value is not within 0 and 1.
     */
    public Accuracy3D(final double confidence) {
        super(confidence);
    }

    /**
     * Constructor.
     *
     * @param covarianceMatrix covariance matrix to be set. Must be 3x3 and positive
     *                         definite.
     * @param confidence       confidence of provided accuracy of an estimated position.
     * @throws IllegalArgumentException                    if provided matrix is not square (it must also be
     *                                                     positive definite to be properly converted to an ellipsoid),
     *                                                     or if provided confidence value is not within 0 and 1.
     * @throws NonSymmetricPositiveDefiniteMatrixException if provided matrix is not symmetric and
     *                                                     positive definite.
     */
    public Accuracy3D(final Matrix covarianceMatrix, final double confidence)
            throws NonSymmetricPositiveDefiniteMatrixException {
        super(covarianceMatrix, confidence);
    }

    /**
     * Gets number of dimensions.
     *
     * @return always returns 3.
     */
    @Override
    public int getNumberOfDimensions() {
        return Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH;
    }

    /**
     * Converts provided covariance matrix into a 3D ellipsoid taking into account current
     * confidence and standard deviation factor.
     *
     * @return ellipsoid representing accuracy of covariance matrix with current confidence and
     * standard deviation factor.
     * @throws NullPointerException           if covariance matrix has not been provided yet.
     * @throws InvalidRotationMatrixException if rotation cannot be properly determined.
     */
    public Ellipsoid toEllipsoid() throws InvalidRotationMatrixException {
        return toEllipsoid(standardDeviationFactor);
    }

    /**
     * Flattens accuracy representation to 2D by taking into account only x and y coordinates and
     * ignoring variance related to z coordinates.
     *
     * @return flattened accuracy representation in 2D.
     * @throws NullPointerException if covariance matrix is not defined.
     * @throws GeometryException    if intersection cannot be computed.
     */
    public Accuracy2D flattenTo2D() throws GeometryException {
        // get intersected ellipse for unitary standard deviation
        final var ellipse = intersectWithPlane(1.0);

        final var semiMajorAxis = ellipse.getSemiMajorAxis();
        final var semiMinorAxis = ellipse.getSemiMinorAxis();
        final var rotation = ellipse.getRotation();

        final var u = rotation.asInhomogeneousMatrix();
        final var s2 = Matrix.diagonal(new double[]{
                semiMajorAxis * semiMajorAxis,
                semiMinorAxis * semiMinorAxis});

        try {
            // compute covariance as the squared matrix M = U*S*V'
            // Hence: M*M' = U*S*V'*(U*S*V')' = U*S*V'*V'*S*U' = U*S^2*U'

            s2.multiply(u);
            u.multiply(s2);

            return new Accuracy2D(u, confidence);
        } catch (final AlgebraException e) {
            throw new GeometryException(e);
        }
    }

    /**
     * Intersects ellipsoid representing this accuracy with horizontal xy plane.
     *
     * @return intersected ellipse.
     * @throws NullPointerException if covariance matrix is not defined.
     * @throws GeometryException    if intersection cannot be computed.
     */
    public Ellipse intersectWithPlane() throws GeometryException {
        return intersectWithPlane(standardDeviationFactor);
    }

    /**
     * Converts provided covariance matrix into a 3D ellipsoid taking into account current
     * confidence and standard deviation factor.
     *
     * @param standardDeviationFactor standard deviation factor.
     * @return ellipsoid representing accuracy of covariance matrix with provided standard
     * deviation factor.
     * @throws NullPointerException           if covariance matrix has not been provided yet.
     * @throws InvalidRotationMatrixException if rotation cannot be properly determined.
     */
    private Ellipsoid toEllipsoid(final double standardDeviationFactor)
            throws InvalidRotationMatrixException {
        final var semiAxesLengths = ArrayUtils.multiplyByScalarAndReturnNew(
                sqrtSingularValues, standardDeviationFactor);
        final var rotation = new MatrixRotation3D(u);
        return new Ellipsoid(Point3D.create(), semiAxesLengths, rotation);
    }

    /**
     * Intersects ellipsoid representing this accuracy with provided standard
     * deviation factor and with horizontal xy plane.
     *
     * @param standardDeviationFactor standard deviation factor.
     * @return intersected ellipse.
     * @throws NullPointerException if covariance matrix is not defined.
     * @throws GeometryException    if intersection cannot be computed.
     */
    private Ellipse intersectWithPlane(final double standardDeviationFactor) throws GeometryException {
        final var ellipsoid = toEllipsoid(standardDeviationFactor);
        final var quadric = ellipsoid.toQuadric();

        // create horizontal xy plane located at ellipsoid center
        final var center = ellipsoid.getCenter();
        final var directorVector = new double[]{0.0, 0.0, 1.0};
        final var plane = new Plane(center, directorVector);

        final var conic = quadric.intersectWith(plane);

        return new Ellipse(conic);
    }
}