Quaternion.h

#pragma once
#define EIGEN_MPL2_ONLY
 
#include <cmath>
 
#include <Eigen/Dense>
#include <Eigen/Geometry>
 
#include <isce3/except/Error.h>
 
#include "DenseMatrix.h"
#include "EulerAngles.h"
#include "Vector.h"
 
namespace isce3 { namespace core {

class Quaternion : public Eigen::Quaterniond {
 
private:
    using super_t = Eigen::Quaterniond;
    using AngleAxis_t = Eigen::AngleAxisd;
 
public:
    Quaternion() : super_t(1, 0, 0, 0) {}

    Quaternion(double w, double x, double y, double z) : super_t(w, x, y, z)
    {
        normalize();
    }

    explicit Quaternion(const Eigen::Vector4d& qvec)
        : Quaternion(qvec(0), qvec(1), qvec(2), qvec(3))
    {}

    explicit Quaternion(const Eigen::Vector3d& vec)
        : Quaternion(0.0, vec(0), vec(1), vec(2))
    {}

    Quaternion(const Eigen::Matrix3d& rotmat) : super_t(rotmat)
    {
        if (!rotmat.isUnitary(1e-6))
            throw isce3::except::InvalidArgument(
                    ISCE_SRCINFO(), "Requires unitary/rotation Matrix!");
    }

    Quaternion(const Eigen::AngleAxisd& aa)
        : super_t(AngleAxis_t(aa.angle(), aa.axis().normalized()))
    {}

    Quaternion(double angle, const Eigen::Vector3d& axis)
        : Quaternion(AngleAxis_t(angle, axis))
    {}

    Quaternion(double yaw, double pitch, double roll)
        : super_t((AngleAxis_t(yaw, Vec3::UnitZ()) *
                   AngleAxis_t(pitch, Vec3::UnitY()) *
                   AngleAxis_t(roll, Vec3::UnitX()))
                          .toRotationMatrix())
    {}

    explicit Quaternion(const EulerAngles& ypr);

    Quaternion(const Eigen::Quaternion<double>& other) : super_t(other)
    {
        normalize();
    }

    Eigen::Vector3d rotate(const Eigen::Vector3d& vec) const
    {
        // Note in case method "_tansformVector" of Eigen Base Quaternion
        // becomes private or deprecated, one can replace it with the
        // following code snippet :
        // return ((*this)*(Quaternion(vec)*(this->conjugate()))).vec();
        return this->_transformVector(vec);
    }

    Vec3 toYPR() const;

    EulerAngles toEulerAngles() const;

    Eigen::AngleAxisd toAngleAxis() const
    {
        double angh {std::acos(this->w())};
        return AngleAxis_t(2.0 * angh, (1.0 / std::sin(angh)) * (this->vec()));
    }
};
 
}} // namespace isce3::core