EulerAngles.h

#pragma once
#define EIGEN_MPL2_ONLY
 
#include <Eigen/Geometry>
 
#include "DenseMatrix.h"
#include "Quaternion.h"
#include "Vector.h"
 
namespace isce3 { namespace core {

class EulerAngles {
public:
    EulerAngles(double y, double p, double r) : _yaw(y), _pitch(p), _roll(r) {}

    explicit EulerAngles(const Mat3& rotmat);

    explicit EulerAngles(const Quaternion& quat);

    Mat3 toRotationMatrix() const
    {
        return (Eigen::AngleAxisd(yaw(), Eigen::Vector3d::UnitZ()) *
                Eigen::AngleAxisd(pitch(), Eigen::Vector3d::UnitY()) *
                Eigen::AngleAxisd(roll(), Eigen::Vector3d::UnitX()))
                .toRotationMatrix();
    }

    double yaw() const { return _yaw; }

    double pitch() const { return _pitch; }

    double roll() const { return _roll; }

    Quaternion toQuaternion() const;

    bool isApprox(const EulerAngles& other, double prec = 1e-7) const;

    Vec3 rotate(const Eigen::Vector3d& vec) const;

    EulerAngles& operator+=(const EulerAngles& rhs)
    {
        _yaw += rhs._yaw;
        _pitch += rhs._pitch;
        _roll += rhs._roll;
        return *this;
    }

    EulerAngles& operator-=(const EulerAngles& rhs)
    {
        _yaw -= rhs._yaw;
        _pitch -= rhs._pitch;
        _roll -= rhs._roll;
        return *this;
    }

    EulerAngles& operator*=(const EulerAngles& rhs)
    {
        *this = EulerAngles(this->toRotationMatrix() * rhs.toRotationMatrix());
        return *this;
    }
 
private:
    double _yaw, _pitch, _roll;
};
 
// Regular/non-member functions

inline EulerAngles operator+(const EulerAngles& lhs, const EulerAngles& rhs)
{
    auto sum {lhs};
    sum += rhs;
    return sum;
}

inline EulerAngles operator-(const EulerAngles& lhs, const EulerAngles& rhs)
{
    auto sub {lhs};
    sub -= rhs;
    return sub;
}

inline EulerAngles operator*(const EulerAngles& lhs, const EulerAngles& rhs)
{
    auto mul {lhs};
    mul *= rhs;
    return mul;
}
 
}} // namespace isce3::core