UnitQuaternion.h

Go to the documentation of this file.

#pragma once

 
#include <concepts>
#include <type_traits>
 
#include <Eigen/Core>
#include <Eigen/Geometry>
 
#include <EmrysFilter/Core/Forwarding.h>
#include <EmrysFilter/Core/Macros.h>
#include <EmrysFilter/Spaces/Coordinates.h>
#include <EmrysFilter/Spaces/Space.h>
 
namespace Emrys
{

struct UnitQuaternion
{
    static constexpr int FixedValueDimension = 4;  
    static constexpr int ValueDimensionStep = 0;   
    static constexpr int MaxValueDimension = 4;    

    [[nodiscard]] static constexpr Eigen::Index
    valueDimension()
    {
        return 4;
    }

    [[nodiscard]] Linear<3>
    difference() const
    {
        return {};
    }

    template <typename Derived>
    void
    normalize(Eigen::MatrixBase<Derived> const & block) const
    {
        using T = typename Derived::Scalar;
        auto & q = writable(block);
        for (Eigen::Index j = 0; j < q.cols(); ++j)
        {
            T const sign = q(3, j) < T(0) ? T(-1) : T(1);
            q.col(j) *= sign / q.col(j).norm();
        }
    }

    template <typename X, typename D, typename Out>
    void
    plus(Eigen::MatrixBase<X> const & x, Eigen::MatrixBase<D> const & d, Eigen::MatrixBase<Out> const & out) const
    {
        using T = typename Out::Scalar;
        auto & o = writable(out);
        for (Eigen::Index j = 0; j < o.cols(); ++j)
        {
            o.col(j) = (quaternionOf<T>(x.col(j)) * exponential(d.col(j).template cast<T>())).coeffs();
        }
        normalize(o);
    }

    template <typename A, typename B, typename Out>
    void
    minus(Eigen::MatrixBase<A> const & a, Eigen::MatrixBase<B> const & b, Eigen::MatrixBase<Out> const & out) const
    {
        using T = typename Out::Scalar;
        auto & o = writable(out);
        for (Eigen::Index j = 0; j < o.cols(); ++j)
        {
            o.col(j) = logarithm(quaternionOf<T>(b.col(j)).conjugate() * quaternionOf<T>(a.col(j)));
        }
    }

    template <typename X, typename D, typename Out>
    void
    resetJacobian(
        Eigen::MatrixBase<X> const &,
        Eigen::MatrixBase<D> const & d,
        Eigen::MatrixBase<Out> const & out
    ) const
    {
        using T = typename Out::Scalar;
        writable(out) = rightJacobian(d.template cast<T>());
    }

    template <typename Derived>
    [[nodiscard]] static Eigen::Quaternion<typename Derived::Scalar>
    exponential(Eigen::MatrixBase<Derived> const & d)
    {
        using T = typename Derived::Scalar;
        T const angle2 = d.squaredNorm();
        if (angle2 < T(1e-12))
        {
            T const scale = T(0.5) - angle2 / T(48);
            return Eigen::Quaternion<T>{T(1) - angle2 / T(8), d(0) * scale, d(1) * scale, d(2) * scale};
        }
        T const angle = Eigen::numext::sqrt(angle2);
        T const scale = T(Eigen::numext::sin(T(angle / T(2))) / angle);
        return Eigen::Quaternion<T>{Eigen::numext::cos(T(angle / T(2))), d(0) * scale, d(1) * scale, d(2) * scale};
    }

    template <typename Derived>
    [[nodiscard]] static Eigen::Matrix<typename Derived::Scalar, 3, 1>
    logarithm(Eigen::QuaternionBase<Derived> const & q)
    {
        using T = typename Derived::Scalar;
        T const sign = q.w() < T(0) ? T(-1) : T(1);
        T const w = sign * q.w();
        Eigen::Matrix<T, 3, 1> const v = sign * q.vec();
        T const s2 = v.squaredNorm();
        if (s2 < T(1e-12))
        {
            return v * T(T(2) / w);
        }
        T const s = Eigen::numext::sqrt(s2);
        T const angle = T(2) * Eigen::numext::atan2(s, w);
        return v * T(angle / s);
    }

    template <typename Derived>
    [[nodiscard]] static Eigen::Matrix<typename Derived::Scalar, 3, 3>
    rightJacobian(Eigen::MatrixBase<Derived> const & d)
    {
        using T = typename Derived::Scalar;
        auto skew = Eigen::Matrix<T, 3, 3>{};
        skew << T(0), -d(2), d(1), d(2), T(0), -d(0), -d(1), d(0), T(0);
        T const angle2 = d.squaredNorm();
        if (angle2 < T(1e-8))
        {
            return Eigen::Matrix<T, 3, 3>::Identity() - T(0.5) * skew + (skew * skew) / T(6);
        }
        T const angle = Eigen::numext::sqrt(angle2);
        return Eigen::Matrix<T, 3, 3>::Identity() - T((T(1) - Eigen::numext::cos(angle)) / angle2) * skew
             + T((angle - Eigen::numext::sin(angle)) / (angle2 * angle)) * (skew * skew);
    }

    template <typename Scalar = double>
    [[nodiscard]] static Eigen::Matrix<Scalar, 4, 1>
    identity()
    {
        return Eigen::Matrix<Scalar, 4, 1>{Scalar(0), Scalar(0), Scalar(0), Scalar(1)};
    }

    template <typename Derived>
    [[nodiscard]] static Eigen::Matrix<typename Derived::Scalar, 4, 1>
    coefficientsOf(Eigen::QuaternionBase<Derived> const & q)
    {
        return q.coeffs();
    }

    template <typename Block>
    [[nodiscard]] static auto
    map(Block && block)
    {
        using Derived = std::remove_cvref_t<Block>;
        static_assert(MatrixExpression<Block>, "map() takes an Eigen vector of four coordinates");
        static_assert(
            Derived::ColsAtCompileTime == 1
                && (Derived::RowsAtCompileTime == 4 || Derived::RowsAtCompileTime == Eigen::Dynamic),
            "map() takes one column of four coordinates (x, y, z, w)"
        );
        static_assert(
            (Derived::Flags & Eigen::DirectAccessBit) && Derived::InnerStrideAtCompileTime == 1,
            "map() needs coefficients in memory with unit stride; evaluate the expression first"
        );
        static_assert(
            std::is_lvalue_reference_v<Block> || !std::derived_from<Derived, Eigen::PlainObjectBase<Derived>>,
            "map() of a temporary matrix would dangle; store it first"
        );
        EMRYS_FILTER_ASSERT(block.size() == 4 && "a unit quaternion has four coordinates");
        using Scalar = typename Derived::Scalar;
        if constexpr (!std::is_const_v<std::remove_reference_t<Block>> && bool(Derived::Flags & Eigen::LvalueBit))
        {
            return Eigen::Map<Eigen::Quaternion<Scalar>>{block.derived().data()};
        }
        else
        {
            return Eigen::Map<Eigen::Quaternion<Scalar> const>{block.derived().data()};
        }
    }
 
private:

    template <typename T, typename Derived>
    [[nodiscard]] static Eigen::Quaternion<T>
    quaternionOf(Eigen::MatrixBase<Derived> const & coefficients)
    {
        Eigen::Matrix<T, 4, 1> const fixed = coefficients.template cast<T>();
        return Eigen::Quaternion<T>{fixed};
    }
 
public:

    friend bool
    operator==(UnitQuaternion const &, UnitQuaternion const &) = default;
};
 
}  // namespace Emrys