Program Listing for File RotationMatrix.h

↰ Return to documentation for file (include/Karana/Math/RotationMatrix.h)

/*
 * Copyright (c) 2024-2026 Karana Dynamics Pty Ltd. All rights reserved.
 *
 * NOTICE TO USER:
 *
 * This source code and/or documentation (the "Licensed Materials") is
 * the confidential and proprietary information of Karana Dynamics Inc.
 * Use of these Licensed Materials is governed by the terms and conditions
 * of a separate software license agreement between Karana Dynamics and the
 * Licensee ("License Agreement"). Unless expressly permitted under that
 * agreement, any reproduction, modification, distribution, or disclosure
 * of the Licensed Materials, in whole or in part, to any third party
 * without the prior written consent of Karana Dynamics is strictly prohibited.
 *
 * THE LICENSED MATERIALS ARE PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND.
 * KARANA DYNAMICS DISCLAIMS ALL WARRANTIES, EXPRESS OR IMPLIED, INCLUDING
 * BUT NOT LIMITED TO WARRANTIES OF MERCHANTABILITY, NON-INFRINGEMENT, AND
 * FITNESS FOR A PARTICULAR PURPOSE.
 *
 * IN NO EVENT SHALL KARANA DYNAMICS BE LIABLE FOR ANY DAMAGES WHATSOEVER,
 * INCLUDING BUT NOT LIMITED TO LOSS OF PROFITS, DATA, OR USE, EVEN IF
 * ADVISED OF THE POSSIBILITY OF SUCH DAMAGES, WHETHER IN CONTRACT, TORT,
 * OR OTHERWISE ARISING OUT OF OR IN CONNECTION WITH THE LICENSED MATERIALS.
 *
 * U.S. Government End Users: The Licensed Materials are a "commercial item"
 * as defined at 48 C.F.R. 2.101, and are provided to the U.S. Government
 * only as a commercial end item under the terms of this license.
 *
 * Any use of the Licensed Materials in individual or commercial software must
 * include, in the user documentation and internal source code comments,
 * this Notice, Disclaimer, and U.S. Government Use Provision.
 */

/**
 * @file
 * @brief Contains the declarations for the RotationMatrix class.
 */

#pragma once

#include "Karana/Math/Defs.h"
#include <csignal>

namespace Karana::Math {

    using namespace Eigen;

    class RotationVector;

    /**
     * @class RotationMatrix
     *
     * @brief The direction-cosine 3x3 attitude representation matrix
     * that is orthogonal and has +1 as one of its eigenvalues.
     *
     * See \sref{attitude_sec} section for more discussion on attitude
     * representations.
     */
    class RotationMatrix : public Mat33 {

        // Constructors
      public:
        //! Default constructor
        RotationMatrix();

        /**
         * @brief Constructor from 3x3 matrix.
         *
         * @param mat The 3x3 matrix to create the RotationMatrix from.
         * @param orthogonalize If true, orthogonalize the input values.
         * @param skip_values_check If true, skip check that the matrix is orthogonal.
         */
        RotationMatrix(const Mat33 &mat,
                       bool orthogonalize = false,
                       bool skip_values_check = false);

        /**
         * @brief Constructor from a RotationVector.
         *
         * @param rotvec The RotationVector to create this RotationMatrix from.
         */
        RotationMatrix(const RotationVector &rotvec);

        // Copy constructor, move constructor, and assignment operators
      public:
        /**
         * @brief Copy constructor.
         * @param rv Other RotationMatrix to copy from.
         */
        RotationMatrix(const RotationMatrix &rv);

        /**
         * @brief Copy assignment operator.
         * @param other Other RotationMatrix to copy from.
         * @return Reference to this object.
         */
        RotationMatrix &operator=(const RotationMatrix &other);

        /**
         * @brief Move constructor.
         * @param rv RotationMatrix to move from.
         */
        RotationMatrix(RotationMatrix &&rv) noexcept;

        /**
         * @brief Move assignment operator.
         * @param other RotationMatrix to move from.
         * @return Reference to this object.
         */
        RotationMatrix &operator=(RotationMatrix &&other) noexcept;

      public:
        /**
         * @brief Equality operator.
         * @param other RotationMatrix to compare with.
         * @return True if equal, false otherwise.
         */
        bool operator==(const RotationMatrix &other);

        /**
         * @brief Approximate equality check.
         * @param other RotationMatrix to compare with.
         * @param prec Precision threshold.
         * @return True if approximately equal, false otherwise.
         */
        bool isApprox(const RotationMatrix &other, double prec = MATH_EPSILON);

        /**
         * @brief Get the type string of this class.
         * @return "RotationMatrix"
         */
        std::string typeString() const;

        /**
         * @brief Assign external values from a 3x3 matrix
         *
         * Since there is no guarantee on the values of the input matrix, we
         * need to be careful to enable appropriate checks and
         * orthogonalization as needed.
         *
         * @param mat Input matrix.
         * @param orthogonalize If true, orthogonalize the input values.
         * @param skip_values_check If true, skip check that the matrix is orthogonal.
         */
        void
        setValues(const Mat33 &mat, bool orthogonalize = false, bool skip_values_check = false);

      private:
        /**
         * @brief Constructor from 4 scalar coefficients
         *
         * If orthogonalize is true, then a QR decomposition is used to
         * extract an orthogonal matrix from the 3x3 matrix. Else, if
         * skip_value_check is false, then the method checks that the input
         * matrix is orthogonal, and throws and exception if that is not the
         * case.
         *
         * @param orthogonalize If true, orthogonalize the RotationMatrix
         * @param skip_values_check If true, skip check that the RotationMatrix
         *                          is orthogonal.
         *
         * @throw an exception if the input matrix is not an orthogonal
         * matrix with determinant 1.
         */
        void _setValues(bool orthogonalize = false, bool skip_values_check = false);

        /**
         * @brief Check whether the matrix is orthogonal with determinant 1
         *
         * @param epsilon  tolerance to use
         *
         * @throw an exception if the input matrix is not an orthogonal
         * matrix with determinant 1.
         */
        void _isOrthogonal(double epsilon = MATH_EPSILON) const;

        /**
         * @brief Orthogonalize the matrix and return one with determinant 1
         *
         * A QR decomposition is used to extract an orthogonal matrix.
         */
        void _orthogonalize();
    };

} // namespace Karana::Math