File proper_quaternion.hpp

File List > data_format > proper_quaternion.hpp

Go to the documentation of this file

#pragma once
#ifdef __ANDROID__
#    include 
#else
#    include 
#endif
#include 

namespace ILLIXR::data_format {

template<typename Scalar_, int Options_ = Eigen::AutoAlign>
class proper_quaternion : public Eigen::Quaternion<Scalar_, Options_> {
public:
    typedef typename Eigen::Quaternion<Scalar_>::Base Base;

    using Base::operator=;

    inline proper_quaternion& operator=(const proper_quaternion& other) {
        Base::operator=(other);
        return *this;
    }

    using Base::operator*=;

    // constructors
    proper_quaternion()                               = default;
    proper_quaternion(const proper_quaternion& other) = default;

    [[maybe_unused]] explicit proper_quaternion(const Eigen::Quaternion<Scalar_, Options_>& other)
        : Eigen::Quaternion<Scalar_, Options_>(other) { }

    [[maybe_unused]] proper_quaternion(const Scalar_& w, const Scalar_& x, const Scalar_& y, const Scalar_& z)
        : Eigen::Quaternion<Scalar_, Options_>(w, x, y, z) { }

    template<typename Derived>
    [[maybe_unused]] proper_quaternion(const Scalar_& w, const Eigen::MatrixBase<Derived>& vec)
        : Eigen::Quaternion<Scalar_, Options_>(w, vec) { }

    [[maybe_unused]] explicit proper_quaternion(const Scalar_* data)
        : Eigen::Quaternion<Scalar_, Options_>(data) { }

    [[maybe_unused]] explicit proper_quaternion(const Eigen::Matrix<Scalar_, 4, 1>& vec)
        : Eigen::Quaternion<Scalar_, Options_>(vec[3], vec[0], vec[1], vec[2]) { }

    template<typename Derived>
    [[maybe_unused]] explicit proper_quaternion(const Eigen::QuaternionBase<Derived>& other)
        : Eigen::Quaternion<Scalar_>(other) { }

    proper_quaternion& operator*=(const Scalar_& a) {
        this->x() *= a;
        this->y() *= a;
        this->z() *= a;
        this->w() *= a;
        return *this;
    }

    proper_quaternion operator*(const int a) const {
        return proper_quaternion<Scalar_, Options_>(this->w() * a, this->x() * a, this->y() * a, this->z() * a);
    }

    proper_quaternion operator*(const float a) const {
        return proper_quaternion<Scalar_, Options_>(this->w() * a, this->x() * a, this->y() * a, this->z() * a);
    }

    proper_quaternion operator*(const double a) const {
        return proper_quaternion<Scalar_, Options_>(this->w() * a, this->x() * a, this->y() * a, this->z() * a);
    }

    proper_quaternion<Scalar_> operator*(const Eigen::Quaternion<Scalar_>& other) const {
        return proper_quaternion<Scalar_>(Eigen::Quaternion<Scalar_>::operator*(other));
    }

    Eigen::Matrix<Scalar_, 4, 1> asVector() const {
        return Eigen::Matrix<Scalar_, 4, 1>(this->x(), this->y(), this->z(), this->w());
    }

    proper_quaternion<Scalar_>& operator+=(const proper_quaternion<Scalar_>& other) {
        this->w() += other.w();
        this->x() += other.x();
        this->y() += other.y();
        this->z() += other.z();
        return *this;
    }

    proper_quaternion<Scalar_> operator+(const proper_quaternion<Scalar_>& other) const {
        proper_quaternion<Scalar_> result(*this);
        result += other;
        return result;
    }

    template<typename T>
    proper_quaternion<T> cast() {
        return proper_quaternion<T>(static_cast<T>(this->w()), static_cast<T>(this->x()), static_cast<T>(this->y()),
                                    static_cast<T>(this->z()));
    }

    inline void normalize() {
        if (this->w() < 0.)
            (*this) *= -1.;

        Eigen::Quaternion<Scalar_, Options_>::normalize();
    }
};

template<typename T>
std::ostream& operator<<(std::ostream& os, const proper_quaternion<T>& pq) {
    os << "X " << pq.x() << std::endl << "Y " << pq.y() << std::endl << "Z " << pq.z() << std::endl << "W " << pq.w();
    return os;
}

[[maybe_unused]] typedef proper_quaternion<double> proper_quaterniond;
[[maybe_unused]] typedef proper_quaternion<float>  proper_quaternionf;

template<typename Scalar_>
inline proper_quaternion<Scalar_> operator*(Scalar_ x, const proper_quaternion<Scalar_>& pq) {
    return proper_quaternion<Scalar_>(x * pq.w(), x * pq.x(), x * pq.y(), x * pq.z());
}

template<typename Scalar_>
inline proper_quaternion<Scalar_> operator/(const proper_quaternion<Scalar_>& pq, Scalar_ x) {
    return proper_quaternion<Scalar_>(pq.w() / x, pq.x() / x, pq.y() / x, pq.z() / x);
}

} // namespace ILLIXR::data_format