rotation_lib.h

Go to the documentation of this file.

 
#pragma once
 
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Geometry>
 
#include <array>
#include <cmath>
#include <iostream>
#include <stdexcept>
#include <string>
#include <vector>
 
// Additional comments to clarify the behavior of intrinsic and extrinsic rotations
 
namespace MoveG
{
 
class Rotation
{
public:
    Rotation();
 
    Rotation(const Rotation &other);
 
    Rotation(Rotation &&other) noexcept;
 
    Rotation(const Eigen::Matrix3d &rotation_matrix);
 
    Rotation(const Eigen::Quaterniond &quaternion);
 
    Rotation(const Eigen::AngleAxisd &angle_axis);
 
    Rotation(double angle1,
             double angle2,
             double angle3,
             bool intrinsic = true,
             const std::string &sequence = "ZYX",
             bool degree = false);
 
    ~Rotation() = default;
 
    static Rotation fromRotationMatrix(const Eigen::Matrix3d &rotation_matrix);
 
    static Rotation fromQuaternion(const Eigen::Quaterniond &quaternion);
 
    static Rotation fromAngleAxis(const Eigen::AngleAxisd &angle_axis);
 
    static Rotation fromEulerAngles(double angle1,
                                    double angle2,
                                    double angle3,
                                    bool intrinsic = true,
                                    const std::string &sequence = "XYZ",
                                    bool degree = false);
 
    // Converters
 
    Eigen::Matrix3d toRotationMatrix() const;
 
    Eigen::Quaterniond toQuaternion() const;
 
    Eigen::AngleAxisd toAngleAxis() const;
 
    Eigen::Vector3d toEulerAngles(bool intrinsic = true, const std::string &sequence = "ZYX") const;
 
    // Overloading of operators * and =
 
    Rotation operator*(const Rotation &other) const;
 
    Rotation &operator=(const Rotation &other);
 
    Rotation &operator=(Rotation &&other) noexcept;
 
    // Angle normalization methods
    static double normalizeAngle(double angle);
 
    static double normalizeAngleRange(double angle, double minVal, double maxVal);
 
    static Eigen::Vector3d normalizeEulerAngles(const Eigen::Vector3d &angles);
 
    // Static methods for quaternion operations
    static Eigen::Quaterniond quaternion_difference(const Eigen::Quaterniond &a,
                                                    const Eigen::Quaterniond &b);
 
    static Eigen::Quaterniond quaternion_negation(const Eigen::Quaterniond &v);
 
    static Eigen::Quaterniond scalar_product(const Eigen::Quaterniond &v, double t);
 
    static Eigen::Quaterniond quaternion_minus(const Eigen::Quaterniond &v1,
                                               const Eigen::Quaterniond &v0);
 
    static Eigen::Quaterniond quaternion_plus(const Eigen::Quaterniond &v1,
                                              const Eigen::Quaterniond &v0);
 
    static double deg2rad(double degree);
 
    static double rad2deg(double radian);
 
    // Elementary Rotations Matrices
 
    static Eigen::Matrix3d rotationX(double angle);
 
    static Eigen::Matrix3d rotationY(double angle);
 
    static Eigen::Matrix3d rotationZ(double angle);
 
    // Definition of Matrix S and R_dot
 
    static Eigen::Matrix3d matrixS(const Eigen::Vector3d &omega);
 
    static Eigen::Matrix3d matrixR_dot(const Eigen::Matrix3d &R, const Eigen::Matrix3d &S);
 
    static Eigen::Matrix3d matrixR_dot(const Eigen::Matrix3d &R, const Eigen::Vector3d &omega);
 
    // Matrix T Mapping Between Body Angular Velocity Vector and the Euler Angle Rates
 
    static Eigen::Matrix3d matrixT(const Eigen::Vector3d &angles, const std::string &sequence);
 
    // Output stream operator
    friend std::ostream &operator<<(std::ostream &os, const Rotation &rotation);
 
private:
    Eigen::Quaterniond q;
 
    // Auxiliary functions
 
    static Eigen::AngleAxisd angleAxis(char axis, double angle);
 
    static int axisToIndex(char axis);
 
    static int checkSequence(const std::string &sequence);
};
 
} // namespace MoveG