Quaternion.hpp

Go to the documentation of this file.

#pragma once
 
#include <Math/math/Functions.hpp>
#include <Math/math/Vector3D.hpp>
#include <Math/math/Angle.hpp>
#include <cassert>
#include <cmath>
#include <concepts>
#include <type_traits>
 
namespace Math
{
 
template <class T>
class BasicQuaternion
{
public:
    using value_type = T; 
 
    constexpr BasicQuaternion() = default;
    constexpr BasicQuaternion(T real_number) : _w(real_number) { } 
    constexpr BasicQuaternion(T w, T i, T j, T k) : _w(w), _i(i), _j(j), _k(k) { }
 
    constexpr static BasicQuaternion<T> identity() { return BasicQuaternion{ T{1}, T{}, T{}, T{} }; }
 
    constexpr static BasicQuaternion<T> zero() { return BasicQuaternion{}; }
    constexpr static BasicQuaternion<T> unit_real() { return identity(); }
    constexpr static BasicQuaternion<T> unit_i() { return BasicQuaternion{ T{}, T{1}, T{},  T{} }; }
    constexpr static BasicQuaternion<T> unit_j() { return BasicQuaternion{ T{}, T{},  T{1}, T{} }; }
    constexpr static BasicQuaternion<T> unit_k() { return BasicQuaternion{ T{}, T{},  T{},  T{1} }; }
 
    constexpr BasicQuaternion<T> conjugate() const
    {
        if constexpr (std::is_floating_point_v<T>)
            return BasicQuaternion<T>{ _w, -_i, -_j, -_k };
        else
            return BasicQuaternion<T>{ _w, conjugate(_i), conjugate(_j), conjugate(_k) };
    }
 
    BasicQuaternion<T> pow(T exponent) const
    {
        assert( isUnit() );
 
        T magnitude{ imaginary().magnitude() };
 
        // Are we a purely real number?
        if ( approximately_equal_to(magnitude, T{0.0}) )
            return BasicQuaternion{ std::pow( real(), exponent ) }; // Yes, so only compute the real part
 
        // Calculate the angle
        T theta{ std::atan2(magnitude, w()) };
        T new_theta{ exponent * theta };
        T coefficient{ std::sin(new_theta) / magnitude };
 
        // NOW we can calculate to the power of "exponent"...
        T temp{ w() * w() + magnitude * magnitude };
 
        return BasicQuaternion<T>{ std::cos(new_theta),
                                   coefficient * i(),
                                   coefficient * j(),
                                   coefficient * k() } * std::pow(temp, exponent);
    }
 
    BasicQuaternion<T> exp() const
    {
        T e_to_the_w{ std::exp( w() ) };
        T vector_part_magnitude{ imaginary().magnitude() };
        T cos_v{ std::cos(vector_part_magnitude) };
        T sin_v{ (vector_part_magnitude > T{0.0}) ? std::sin(vector_part_magnitude) / vector_part_magnitude : T{0.0} };
 
        return BasicQuaternion{ cos_v,
                           sin_v * i(),
                           sin_v * j(),
                           sin_v * k() } * e_to_the_w;
    }
 
    BasicQuaternion<T> log() const
    {
        T magnitude_of_imaginary_part{ imaginary().magnitude() };
 
        // Are we purely a real number?
        if ( approximately_equal_to(magnitude_of_imaginary_part, T{0.0}) )
            return BasicQuaternion{ std::log( w() ) }; // YES, so just set the w() component (the others will be zero)
 
        T this_norm{ norm() };
        T theta{ std::acos( w() / this_norm ) };
        T coefficient{ theta / magnitude_of_imaginary_part };
 
        return BasicQuaternion{ std::log(this_norm),
                           coefficient * i(),
                           coefficient * j(),
                           coefficient * k() };
    }
 
    T    normSquared() const { return accumulate(*this * conjugate()); }
    T    norm() const { return std::sqrt( normSquared() ); }
 
    T    magnitudeSquared() const { return normSquared(); }
    T    magnitude() const { return norm(); }
 
    constexpr BasicQuaternion<T> normalized() const
    {
        assert( BasicQuaternion<T>{*this / magnitude()}.isUnit() ); // Verify that we actually return a unit-length BasicQuaternion
 
        return *this / this->magnitude();
    }
 
    BasicQuaternion<T> inverse() const { return conjugate() / normSquared(); }
 
    BasicRadian<T> angle() const
    {
        return T{2.0} * std::atan2( imaginary().magnitude(), w() );
    }
 
    constexpr BasicVector3D<T> axis() const
    {
        return imaginary().normalized();
    }
 
    constexpr const T &w() const { return _w; }
    constexpr const T &real() const { return _w; }
 
    constexpr const T &i() const { return _i; }
    constexpr const T &j() const { return _j; }
    constexpr const T &k() const { return _k; }
 
    constexpr BasicVector3D<T> imaginary() const { return { _i, _j, _k }; }
 
    bool isUnit() const { return approximately_equal_to( magnitude(), T{1} ); }
    bool isUnit(T tolerance) const { return approximately_equal_to( magnitude(), T{1}, tolerance ); }
 
    bool isZero() const { return approximately_equal_to( magnitude(), T{} ); }
    bool isZero(T tolerance) const { return approximately_equal_to( magnitude(), T{}, tolerance ); }
 
    // Checks if the real() part is 0
    bool isPure() const { return approximately_equal_to(real(), T{}); }
 
    bool isNaN() const
    {
        if constexpr ( std::is_floating_point_v<T> )
            return std::isnan(_w) || std::isnan(_i) || std::isnan(_j) || std::isnan(_k);
        else
            return _w.isNaN() || _i.isNaN() || _j.isNaN() || _k.isNaN();
    }
    bool isInf() const
    {
        if constexpr ( std::is_floating_point_v<T> )
            return std::isinf(_w) || std::isinf(_i) || std::isinf(_j) || std::isinf(_k);
        else
            return _w.isInf() || _i.isInf() || _j.isInf() || _k.isInf();
    }
 
    constexpr static BasicQuaternion<T> make_pure(T x, T y, T z) { return BasicQuaternion<T>{ T{}, x, y, z }; }
 
    constexpr static BasicQuaternion<T> make_pure(const BasicVector3D<T> &t) { return BasicQuaternion<T>{ T{}, t.x, t.y, t.z }; }
 
    constexpr static BasicQuaternion<T> encode_point(T x, T y, T z) { return make_pure(x, y, z); }
 
    constexpr static BasicQuaternion<T> encode_point(const BasicVector3D<T> &point) { return make_pure(point); }
 
    constexpr static BasicQuaternion<T> make_rotation(const BasicRadian<T> &radians, T axis_x, T axis_y, T axis_z)
    {
        return make_rotation(radians, { axis_x, axis_y, axis_z });
    }
 
    constexpr static BasicQuaternion<T> make_rotation(const BasicRadian<T> &radians, const BasicVector3D<T> &axis)
    {
        T half_angle = radians.value() * T{0.5};
        T cos_theta = cos( half_angle );
        T sin_theta = sin( half_angle );
        BasicVector3D<T> n{ axis.normalized() };
 
        return BasicQuaternion<T>{ cos_theta,
                                   sin_theta * n.x,
                                   sin_theta * n.y,
                                   sin_theta * n.z };
    }
 
private:
    T _w{};
    T _i{};
    T _j{};
    T _k{};
 
    friend constexpr bool operator ==(const BasicQuaternion<T> &left, const BasicQuaternion<T> &right)
    {
        return approximately_equal_to(left, right);
    }
 
    template <std::floating_point OT = T>
        requires std::is_floating_point_v<T>
    friend constexpr bool approximately_equal_to(const BasicQuaternion<T> &value_to_test,
                                                 const BasicQuaternion<T> &value_it_should_be,
                                                       OT                  tolerance = OT{0.0002})
    {
        return approximately_equal_to(value_to_test.w(), value_it_should_be.w(), tolerance) &&
               approximately_equal_to(value_to_test.i(), value_it_should_be.i(), tolerance) &&
               approximately_equal_to(value_to_test.j(), value_it_should_be.j(), tolerance) &&
               approximately_equal_to(value_to_test.k(), value_it_should_be.k(), tolerance);
    }
 
    friend constexpr BasicQuaternion<T> operator *(const BasicQuaternion<T> &left, const BasicQuaternion<T> &right)
    {
        return { left.w() * right.w() - (left.i() * right.i() +
                                         left.j() * right.j() +
                                         left.k() * right.k()),
                 left.w() * right.i() +
                 left.i() * right.w() +
                 left.j() * right.k() -
                 left.k() * right.j(),
            
                 left.w() * right.j() -
                 left.i() * right.k() +
                 left.j() * right.w() +
                 left.k() * right.i(),
            
                 left.w() * right.k() +
                 left.i() * right.j() -
                 left.j() * right.i() +
                 left.k() * right.w()
        };
    }
 
    template <std::floating_point OT>
    friend constexpr BasicQuaternion<T> operator *(const BasicQuaternion<T> &quaternion, OT scalar)
    {
        return { quaternion.w() * scalar,
                 quaternion.i() * scalar,
                 quaternion.j() * scalar,
                 quaternion.k() * scalar };
    }
 
    friend constexpr BasicQuaternion<T> operator /(const BasicQuaternion<T> &left, const BasicQuaternion<T> &right)
    {
        return left * right.inverse();
    }
 
    template <std::floating_point OT>
    friend constexpr BasicQuaternion<T> operator /(const BasicQuaternion<T> &quaternion, OT scalar)
    {
        return { quaternion.w() / T(scalar),
                 quaternion.i() / T(scalar),
                 quaternion.j() / T(scalar),
                 quaternion.k() / T(scalar) };
    }
 
    friend constexpr BasicQuaternion<T> operator +(const BasicQuaternion<T> &left, const BasicQuaternion<T> &right)
    {
        return { left.w() + right.w(),
                 left.i() + right.i(),
                 left.j() + right.j(),
                 left.k() + right.k() };
    }
 
    friend constexpr BasicQuaternion<T> operator -(const BasicQuaternion<T> &left, const BasicQuaternion<T> &right)
    {
        return { left.w() - right.w(),
                 left.i() - right.i(),
                 left.j() - right.j(),
                 left.k() - right.k() };
    }
 
    friend constexpr BasicQuaternion<T> operator -(const BasicQuaternion<T> &q)
    {
        return { -q.w(), -q.i(), -q.j(), -q.k() };
    }
 
    template <std::floating_point OT = T>
        requires std::is_floating_point_v<T>
    friend bool check_if_equal(const BasicQuaternion<T> &input,
                               const BasicQuaternion<T> &near_to,
                                     OT                  tolerance = OT{0.0002})
    {
        if (!approximately_equal_to(input, near_to, tolerance))
        {
            auto diff{ near_to - input };
 
            std::cout << std::format("input: {} is not equal to near_to: {} within tolerance: {}.  Difference is {} .",
                                     format(input),
                                     format(near_to),
                                     tolerance,
                                     format(near_to - input))
            << std::endl;
            return  false;
        }
        return true;
    }
 
    template <std::floating_point OT = T>
        requires std::is_floating_point_v<T>
    friend bool check_if_not_equal(const BasicQuaternion<T> &input,
                                   const BasicQuaternion<T> &near_to,
                                         OT                  tolerance = OT{0.0002})
    {
        if (approximately_equal_to(input, near_to, tolerance))
        {
            auto diff{ near_to - input };
 
            std::cout << std::format("input: {} is equal to near_to: {} within tolerance: {}.  Difference is {} .",
                                     format(input),
                                     format(near_to),
                                     tolerance,
                                     format(near_to - input))
            << std::endl;
            return  false;
        }
        return true;
    }
 
    template <std::floating_point OT = T>
        requires std::is_floating_point_v<T>
    friend void CHECK_IF_EQUAL(const BasicQuaternion<T> &input,
                               const BasicQuaternion<T> &near_to,
                                     OT                  tolerance = OT{0.0002})
    {
        assert( check_if_equal(input, near_to, tolerance) );
    }
 
    template <std::floating_point OT = T>
        requires std::is_floating_point_v<T>
    friend void CHECK_IF_NOT_EQUAL(const BasicQuaternion<T> &input,
                                   const BasicQuaternion<T> &near_to,
                                         OT                  tolerance = OT{0.0002})
    {
        assert( check_if_not_equal(input, near_to, tolerance) );
    }
 
    template <std::floating_point OT = T>
        requires std::is_floating_point_v<T>
    friend void CHECK_IF_ZERO(const BasicQuaternion<T> &input, OT tolerance = OT{0.0002})
    {
        assert( check_if_equal(input, BasicQuaternion<T>::zero(), tolerance));
    }
 
    friend constexpr T dot(const BasicQuaternion<T> &left, const BasicQuaternion<T> &right)
    {
        return left.w() * right.w() +
               left.i() * right.i() +
               left.j() * right.j() +
               left.k() * right.k();
    }
 
    friend constexpr T dot_normalized(const BasicQuaternion<T> &left, const BasicQuaternion<T> &right)
    {
        return ( left.w() * right.w() +
                 left.i() * right.i() +
                 left.j() * right.j() +
                 left.k() * right.k()) / (left.magnitude() * right.magnitude()
               );
    }
 
    friend constexpr BasicQuaternion<T> passively_rotate_encoded_point(const BasicQuaternion<T> &rotation,
                                                                       const BasicQuaternion<T> &encoded_point)
    {
        assert( rotation.isUnit() );
        assert( encoded_point.isPure() );
 
        return rotation * encoded_point * rotation.conjugate();
    }
 
    friend constexpr BasicQuaternion<T> locally_rotate_encoded_point(const BasicQuaternion<T> &rotation,
                                                                     const BasicQuaternion<T> &encoded_point)
    {
        return passively_rotate_encoded_point(rotation, encoded_point);
    }
 
    friend constexpr BasicQuaternion<T> actively_rotate_encoded_point(const BasicQuaternion<T> &rotation,
                                                                      const BasicQuaternion<T> &encoded_point)
    {
        assert( rotation.isUnit() );
        assert( encoded_point.isPure() );
 
        return rotation.conjugate() * encoded_point * rotation;
    }
 
    friend constexpr BasicQuaternion<T> globally_rotate_encoded_point(const BasicQuaternion<T> &rotation,
                                                                      const BasicQuaternion<T> &encoded_point)
    {
        return actively_rotate_encoded_point(rotation, encoded_point);
    }
 
    friend constexpr BasicQuaternion<T> compose_rotations(const BasicQuaternion<T> &rotation_1,
                                                          const BasicQuaternion<T> &rotation_2)
    {
        return rotation_2 * rotation_1;
    }
 
    friend constexpr BasicQuaternion<T> normalized(const BasicQuaternion<T> &input)
    {
        return input.normalized();
    }
 
    friend constexpr T arg(const BasicQuaternion<T> &input)
    {
        return input.angle().value();
    }
 
    friend constexpr BasicQuaternion<T> polar(const BasicVector3D<T> &axis,
                                              const BasicRadian<T>    angle = BasicRadian<T>{})
    {
        assert( axis.magnitude() == T{1} );
 
        return BasicQuaternion<T>::make_rotation( angle, axis );
    }
 
    friend constexpr T accumulate(const BasicQuaternion<T> &input)
    {
        return T{input.real() + input.i() + input.j() + input.k()};
    }
 
    friend constexpr BasicQuaternion<T> slerp(const BasicQuaternion<T> &begin,
                                              const BasicQuaternion<T> &end,
                                                    T                   percent)
    {
        BasicQuaternion<T> combined{ begin.conjugate() * end };
 
        return begin * combined.pow(percent);
    }
 
    friend std::string format(const BasicQuaternion<T> &input)
    {
        return std::format("[w: {}, i: {}, j: {}, k: {}]", input.w(), input.i(), input.j(), input.k());
    }
 
    friend constexpr BasicQuaternion<T> conjugate(const BasicQuaternion<T> &input)
    {
        return input.conjugate();
    }
 
    friend constexpr BasicQuaternion<T> log(const BasicQuaternion<T> &input)
    {
        return input.log();
    }
 
    friend BasicQuaternion<T> exp(const BasicQuaternion<T> &input)
    {
        return input.exp();
    }
};
 
 
using Quaternionf = BasicQuaternion<float>;
using Quaterniond = BasicQuaternion<double>;
using Quaternion  = BasicQuaternion<double>;
using Quaternionl = BasicQuaternion<long double>;
 
}