DualQuaternion.hpp

Go to the documentation of this file.

#pragma once
 
#include <Math/math/Dual.hpp>
#include <Math/math/Quaternion.hpp>
#include <Math/math/Vector3D.hpp>
#include <Math/math/Functions.hpp>
#include <cassert>
#include <concepts>
 
namespace Math
{
 
template <class T>
class BasicDualQuaternion
{
public:
    using value_type = T; 
 
    BasicDualQuaternion() = default;
 
    constexpr BasicDualQuaternion(const BasicQuaternion<T> &rotation,
                                  const BasicQuaternion<T> &translation)
        :
        _frame_of_reference{rotation, translation}
    {
    }
 
    constexpr BasicDualQuaternion(const BasicQuaternion<T> &rotation,
                                                        T   translation_x,
                                                        T   translation_y,
                                                        T   translation_z)
        :
        _frame_of_reference{ rotation,
                             T{0.5} * BasicQuaternion<T>::encode_point(translation_x, translation_y, translation_z) * rotation }
    {
        assert( real().isUnit() );
    }
    constexpr BasicDualQuaternion(const BasicQuaternion<T> &rotation,
                                  const BasicVector3D<T>   &translation)
        :
        _frame_of_reference{ rotation,
                             T{0.5} * BasicQuaternion<T>::encode_point(translation) * rotation }
    {
        assert( real().isUnit() );
    }
 
    explicit constexpr BasicDualQuaternion(const BasicDual<BasicQuaternion<T>> &underlying_representation)
        :
        _frame_of_reference(underlying_representation)
    {
    }
 
    constexpr static BasicDualQuaternion<T> identity()
    {
        return { BasicQuaternion<T>(1.0),
                 BasicQuaternion<T>::zero() };
    }
 
    constexpr static BasicDualQuaternion<T> zero()
    {
        return { BasicQuaternion<T>::zero(), BasicQuaternion<T>::zero() };
    }
 
    constexpr static BasicDualQuaternion<T> make_rotation(const BasicQuaternion<T> &rotation)
    {
        // A pure rotation has the dual part set to zero.
        return { rotation, BasicQuaternion<T>::zero() };
    }
 
    constexpr static BasicDualQuaternion<T> make_translation(T translation_x, T translation_y, T translation_z)
    {
        // No need to make the translation "0.5 * t * r" because "r" is an identity Quaterion,
        // so we just use "0.5 * t".
        return { BasicQuaternion<T>::identity(),
                 T{0.5} * BasicQuaternion<T>::encode_point(translation_x, translation_y, translation_z)
               };
    }
 
    constexpr static BasicDualQuaternion<T> make_translation(const BasicVector3D<T> &translation)
    {
        // No need to make the translation "0.5 * t * r" because "r" is an identity Quaterion,
        // so we just use "0.5 * t".
        return BasicDualQuaternion<T>{ BasicQuaternion<T>::identity(),
                                  T{0.5} * BasicQuaternion<T>::encode_point(translation)
                                };
    }
 
    constexpr static BasicDualQuaternion<T> make_coordinate_system(const BasicQuaternion<T> &rotation,
                                                                                         T   translation_x,
                                                                                         T   translation_y,
                                                                                         T   translation_z)
    {
        assert( rotation.isUnit() );
 
        return BasicDualQuaternion<T>{ rotation, translation_x, translation_y, translation_z };
    }
 
    constexpr static BasicDualQuaternion<T> encode_point(const BasicVector3D<T> &point)
    {
        return BasicDualQuaternion<T>{ BasicQuaternion<T>::identity(), point };
    }
 
    constexpr static BasicVector3D<T> decode_point(const BasicDualQuaternion<T>& encoded_point)
    {
        return encoded_point.translation();
    }
 
    constexpr BasicDualQuaternion<T> conjugate() const
    {
        return BasicDualQuaternion<T>{ real().conjugate(), dual().conjugate() };
    }
 
    constexpr BasicDual<T> normsquared() const
    {
        BasicDualQuaternion<T> normsquared{ *this * this->conjugate() };
 
        // We should have a dual scalar now
        // Make that assumption clear
        assert( approximately_equal_to(normsquared.real().i(), T{0.0}) );
        assert( approximately_equal_to(normsquared.real().j(), T{0.0}) );
        assert( approximately_equal_to(normsquared.real().k(), T{0.0}) );
 
#if 0
        assert( approximately_equal_to(normsquared.dual().i(), T{0}) );
        assert( approximately_equal_to(normsquared.dual().j(), T{0}) );
        assert( approximately_equal_to(normsquared.dual().k(), T{0}) );
#endif
 
        return BasicDual<T>{ normsquared.real().real(), normsquared.dual().real() };
    }
 
    constexpr BasicDual<T> norm() const
    {
        return dualscalar_sqrt( normsquared() );
    }
 
    constexpr BasicDual<T> magnitude() const { return norm(); }
 
    const BasicQuaternion<T> &real() const { return _frame_of_reference.real; }
    const BasicQuaternion<T> &dual() const { return _frame_of_reference.dual; }
 
    const BasicQuaternion<T> &rotation()    const { return real(); }
          BasicVector3D<T>    translation() const
    {
        return BasicQuaternion<T>(T{2.0} * dual() * rotation().conjugate()).imaginary();
    }
 
    constexpr BasicDualQuaternion<T> normalized() const
    {
        return *this / norm();
    }
 
    constexpr bool rotation_magnitude_is_one() const
    {
        return approximately_equal_to( dot(real(), real()), T{1.0} );
    }
 
    constexpr bool rotation_and_translation_are_orthogonal() const
    {
        return approximately_equal_to( dot(real(), dual()), T{0.0} );
    }
 
    constexpr bool is_unit() const
    {
        return rotation_magnitude_is_one() && rotation_and_translation_are_orthogonal();
    }
 
    bool isNaN() const { return _frame_of_reference.real.isNaN() || _frame_of_reference.dual.isNaN(); }
    bool isInf() const { return _frame_of_reference.real.isInf() || _frame_of_reference.dual.isInf(); }
 
private:
    BasicDual<BasicQuaternion<T>> _frame_of_reference{ BasicQuaternion<T>::identity(), BasicQuaternion<T>::zero() }; // The default value is an identity transformation
 
    friend constexpr bool operator ==(const BasicDualQuaternion<T> &left,
                                      const BasicDualQuaternion<T> &right)
    {
        return approximately_equal_to(left, right);
    }
 
    //template <std::floating_point OT = float>
    friend constexpr bool approximately_equal_to(const BasicDualQuaternion<T> &value_to_test,
                                                 const BasicDualQuaternion<T> &value_it_should_be,
                                                                           T  tolerance = T{0.0002})
    {
        // Just use the underlying BasicDual number's version of the same function...
        return approximately_equal_to( value_to_test._frame_of_reference, value_it_should_be._frame_of_reference, tolerance );
    }
 
    friend constexpr BasicDualQuaternion<T> operator +(const BasicDualQuaternion<T> &left_side,
                                                       const BasicDualQuaternion<T> &right_side)
    {
        return BasicDualQuaternion<T>{ left_side._frame_of_reference + right_side._frame_of_reference };
    }
 
    friend constexpr BasicDualQuaternion<T> operator *(T scalar, const BasicDualQuaternion<T> &dual_quaternion)
    {
        return BasicDualQuaternion<T>{ scalar * dual_quaternion._frame_of_reference };
    }
 
    friend constexpr BasicDualQuaternion<T> operator *(const BasicDualQuaternion<T> &dual_quaternion, T scalar)
    {
        return BasicDualQuaternion<T>{ dual_quaternion._frame_of_reference * scalar };
    }
 
    friend constexpr BasicDualQuaternion<T> operator *(const BasicDualQuaternion<T> &dual_quaternion,
                                                       const BasicDual<T>           &dual_scalar)
    {
        return dual_quaternion * BasicDualQuaternion<T>( BasicQuaternion<T>(dual_scalar.real),
                                                         BasicQuaternion<T>(dual_scalar.dual) );
    }
 
    friend constexpr BasicDualQuaternion<T> operator *(const BasicDualQuaternion<T> &left_side,
                                                       const BasicDualQuaternion<T> &right_side)
    {
        return BasicDualQuaternion<T>( left_side._frame_of_reference * right_side._frame_of_reference );
    }
 
    friend constexpr BasicDualQuaternion<T> operator /(const BasicDualQuaternion<T> &dual_quaternion,
                                                       const BasicDual<T>           &dual_scalar)
    {
        return BasicDualQuaternion<T>( BasicDualQuaternion<T>(dual_quaternion *
                                                              dual_scalar.conjugate())._frame_of_reference /
                                       dualscalar_normsquared(dual_scalar) );
    }
 
    template <std::floating_point OT = float>
    friend bool check_if_equal(const BasicDualQuaternion<T> &input,
                               const BasicDualQuaternion<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 = float>
    friend bool check_if_not_equal(const BasicDualQuaternion<T> &input,
                                   const BasicDualQuaternion<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 = float>
    friend void CHECK_IF_EQUAL(const BasicDualQuaternion<T> &input,
                               const BasicDualQuaternion<T> &near_to,
                                     OT                      tolerance = OT{0.0002})
    {
        assert( check_if_equal(input, near_to, tolerance) );
    }
 
    template <std::floating_point OT = float>
    friend void CHECK_IF_NOT_EQUAL(const BasicDualQuaternion<T> &input,
                                   const BasicDualQuaternion<T> &near_to,
                                         OT                      tolerance = OT{0.0002})
    {
        assert( check_if_not_equal(input, near_to, tolerance) );
    }
 
    template <std::floating_point OT = float>
    friend void CHECK_IF_ZERO(const BasicDualQuaternion<T> &input, OT tolerance = OT{0.0002})
    {
        assert( check_if_equal(input, BasicDualQuaternion<T>::zero(), tolerance));
    }
 
    friend constexpr BasicDualQuaternion<T> normalized(const BasicDualQuaternion<T> &input)
    {
        return input.normalized();
    }
 
    template <std::floating_point OT = float>
    friend constexpr BasicDualQuaternion<T> blend(const BasicDualQuaternion<T> &beginning,
                                                  const BasicDualQuaternion<T> &end,
                                                        OT                      percentage)
    {
        auto blended = beginning + (end - beginning) * percentage;
 
        return blended.normalized();
    }
 
    friend constexpr BasicDualQuaternion<T> conjugate(const BasicDualQuaternion<T> &input)
    {
        return input.conjugate();
    }
 
    friend std::string format(const BasicDualQuaternion<T> &input)
    {
        return std::format("[real: {}, dual: {}]", input.real(), input.dual());
    }
};
 
 
using DualQuaternionf = BasicDualQuaternion<float>;
using DualQuaterniond = BasicDualQuaternion<double>;
using DualQuaternion  = BasicDualQuaternion<double>;
using DualQuaternionl = BasicDualQuaternion<long double>;
 
}