Quaternion.hpp

Go to the documentation of this file.

 
#pragma once
 
#include <array>
#include <cmath>
#include <iostream>
#include <string>
 
#include "NumCpp/Core/Internal/Error.hpp"
#include "NumCpp/Core/Internal/StlAlgorithms.hpp"
#include "NumCpp/Core/Types.hpp"
#include "NumCpp/Functions/argmax.hpp"
#include "NumCpp/Functions/clip.hpp"
#include "NumCpp/Functions/dot.hpp"
#include "NumCpp/Functions/norm.hpp"
#include "NumCpp/Functions/square.hpp"
#include "NumCpp/Linalg/hat.hpp"
#include "NumCpp/NdArray.hpp"
#include "NumCpp/Utils/essentiallyEqual.hpp"
#include "NumCpp/Utils/num2str.hpp"
#include "NumCpp/Utils/sqr.hpp"
#include "NumCpp/Vector/Vec3.hpp"
 
namespace nc::rotations
{
    //================================================================================
    // Class Description:
    class Quaternion
    {
    public:
        //============================================================================
        // Method Description:
        Quaternion() = default;
 
        //============================================================================
        // Method Description:
        Quaternion(double roll, double pitch, double yaw) noexcept
        {
            eulerToQuat(roll, pitch, yaw);
        }
 
        //============================================================================
        // Method Description:
        Quaternion(double inI, double inJ, double inK, double inS) noexcept :
            components_{ inI, inJ, inK, inS }
        {
            normalize();
        }
 
        //============================================================================
        // Method Description:
        Quaternion(const std::array<double, 4>& components) noexcept :
            components_{ components }
        {
            normalize();
        }
 
        //============================================================================
        // Method Description:
        Quaternion(const NdArray<double>& inArray) :
            components_{ 0., 0., 0., 0. }
        {
            if (inArray.size() == 3)
            {
                // euler angles
                eulerToQuat(inArray[0], inArray[1], inArray[2]);
            }
            else if (inArray.size() == 4)
            {
                // quaternion i, j, k, s components
                stl_algorithms::copy(inArray.cbegin(), inArray.cend(), components_.begin());
                normalize();
            }
            else if (inArray.size() == 9)
            {
                // direction cosine matrix
                dcmToQuat(inArray);
            }
            else
            {
                THROW_INVALID_ARGUMENT_ERROR("input array is not a valid size.");
            }
        }
 
        //============================================================================
        // Method Description:
        Quaternion(const Vec3& inAxis, double inAngle) noexcept
        {
            // normalize the input vector
            Vec3 normAxis = inAxis.normalize();
 
            const double halfAngle    = inAngle / 2.;
            const double sinHalfAngle = std::sin(halfAngle);
 
            components_[0] = normAxis.x * sinHalfAngle;
            components_[1] = normAxis.y * sinHalfAngle;
            components_[2] = normAxis.z * sinHalfAngle;
            components_[3] = std::cos(halfAngle);
        }
 
        //============================================================================
        // Method Description:
        Quaternion(const NdArray<double>& inAxis, double inAngle) :
            Quaternion(Vec3(inAxis), inAngle)
        {
        }
 
        //============================================================================
        // Method Description:
        [[nodiscard]] double angleOfRotation() const noexcept
        {
            return 2. * std::acos(s());
        }
 
        //============================================================================
        // Method Description:
        static NdArray<double> angularVelocity(const Quaternion& inQuat1, const Quaternion& inQuat2, double inTime)
        {
            NdArray<double> q0 = inQuat1.toNdArray();
            NdArray<double> q1 = inQuat2.toNdArray();
 
            NdArray<double> qDot = q1 - q0;
            qDot /= inTime;
 
            NdArray<double> eyeTimesScalar(3);
            eyeTimesScalar.zeros();
            eyeTimesScalar(0, 0) = inQuat2.s();
            eyeTimesScalar(1, 1) = inQuat2.s();
            eyeTimesScalar(2, 2) = inQuat2.s();
 
            NdArray<double> epsilonHat = linalg::hat<double>(inQuat2.i(), inQuat2.j(), inQuat2.k());
            NdArray<double> q(4, 3);
            q.put(Slice(0, 3), Slice(0, 3), eyeTimesScalar + epsilonHat);
            q(3, 0) = -inQuat2.i();
            q(3, 1) = -inQuat2.j();
            q(3, 2) = -inQuat2.k();
 
            NdArray<double> omega = q.transpose().dot(qDot.transpose());
            return omega *= 2.;
        }
 
        //============================================================================
        // Method Description:
        [[nodiscard]] NdArray<double> angularVelocity(const Quaternion& inQuat2, double inTime) const
        {
            return angularVelocity(*this, inQuat2, inTime);
        }
 
        //============================================================================
        // Method Description:
        [[nodiscard]] Vec3 axisOfRotation() const noexcept
        {
            const auto halfAngle    = angleOfRotation() / 2.;
            const auto sinHalfAngle = std::sin(halfAngle);
            auto       axis         = Vec3(i() / sinHalfAngle, j() / sinHalfAngle, k() / sinHalfAngle);
 
            // shouldn't be necessary, but let's be pedantic
            return axis.normalize();
        }
 
        //============================================================================
        // Method Description:
        [[nodiscard]] Quaternion conjugate() const noexcept
        {
            return { -i(), -j(), -k(), s() };
        }
 
        //============================================================================
        // Method Description:
        [[nodiscard]] double i() const noexcept
        {
            return components_[0];
        }
 
        //============================================================================
        // Method Description:
        static Quaternion identity() noexcept
        {
            return {};
        }
 
        //============================================================================
        // Method Description:
        [[nodiscard]] Quaternion inverse() const noexcept
        {
            return conjugate();
        }
 
        //============================================================================
        // Method Description:
        [[nodiscard]] double j() const noexcept
        {
            return components_[1];
        }
 
        //============================================================================
        // Method Description:
        [[nodiscard]] double k() const noexcept
        {
            return components_[2];
        }
 
        //============================================================================
        // Method Description:
        static Quaternion nlerp(const Quaternion& inQuat1, const Quaternion& inQuat2, double inPercent)
        {
            if (inPercent < 0. || inPercent > 1.)
            {
                THROW_INVALID_ARGUMENT_ERROR("input percent must be of the range [0,1].");
            }
 
            if (utils::essentiallyEqual(inPercent, 0.))
            {
                return inQuat1;
            }
            if (utils::essentiallyEqual(inPercent, 1.))
            {
                return inQuat2;
            }
 
            const double          oneMinus = 1. - inPercent;
            std::array<double, 4> newComponents{};
 
            stl_algorithms::transform(inQuat1.components_.begin(),
                                      inQuat1.components_.end(),
                                      inQuat2.components_.begin(),
                                      newComponents.begin(),
                                      [inPercent, oneMinus](double component1, double component2) -> double
                                      { return oneMinus * component1 + inPercent * component2; });
 
            return { newComponents };
        }
 
        //============================================================================
        // Method Description:
        [[nodiscard]] Quaternion nlerp(const Quaternion& inQuat2, double inPercent) const
        {
            return nlerp(*this, inQuat2, inPercent);
        }
 
        //============================================================================
        // Method Description:
        [[nodiscard]] double pitch() const noexcept
        {
            return std::asin(2 * (s() * j() - k() * i()));
        }
 
        //============================================================================
        // Method Description:
        static Quaternion pitchRotation(double inAngle) noexcept
        {
            return { 0., inAngle, 0. };
        }
 
        //============================================================================
        // Method Description:
        void print() const
        {
            std::cout << *this;
        }
 
        //============================================================================
        // Method Description:
        [[nodiscard]] double roll() const noexcept
        {
            return std::atan2(2. * (s() * i() + j() * k()), 1. - 2. * (utils::sqr(i()) + utils::sqr(j())));
        }
 
        //============================================================================
        // Method Description:
        static Quaternion rollRotation(double inAngle) noexcept
        {
            return { inAngle, 0., 0. };
        }
 
        //============================================================================
        // Method Description:
        [[nodiscard]] NdArray<double> rotate(const NdArray<double>& inVector) const
        {
            if (inVector.size() != 3)
            {
                THROW_INVALID_ARGUMENT_ERROR("input inVector must be a cartesion vector of length = 3.");
            }
 
            return *this * inVector;
        }
 
        //============================================================================
        // Method Description:
        [[nodiscard]] Vec3 rotate(const Vec3& inVec3) const
        {
            return *this * inVec3;
        }
 
        //============================================================================
        // Method Description:
        [[nodiscard]] double s() const noexcept
        {
            return components_[3];
        }
 
        //============================================================================
        // Method Description:
        static Quaternion slerp(const Quaternion& inQuat1, const Quaternion& inQuat2, double inPercent)
        {
            if (inPercent < 0 || inPercent > 1)
            {
                THROW_INVALID_ARGUMENT_ERROR("input percent must be of the range [0, 1]");
            }
 
            if (utils::essentiallyEqual(inPercent, 0.))
            {
                return inQuat1;
            }
            if (utils::essentiallyEqual(inPercent, 1.))
            {
                return inQuat2;
            }
 
            double dotProduct = dot<double>(inQuat1.toNdArray(), inQuat2.toNdArray()).item();
 
            // If the dot product is negative, the quaternions
            // have opposite handed-ness and slerp won't take
            // the shorter path. Fix by reversing one quaternion.
            Quaternion quat1Copy(inQuat1);
            if (dotProduct < 0.)
            {
                quat1Copy *= -1.;
                dotProduct *= -1.;
            }
 
            constexpr double DOT_THRESHOLD = 0.9995;
            if (dotProduct > DOT_THRESHOLD)
            {
                // If the inputs are too close for comfort, linearly interpolate
                // and normalize the result.
                return nlerp(inQuat1, inQuat2, inPercent);
            }
 
            dotProduct          = clip(dotProduct, -1., 1.); // Robustness: Stay within domain of acos()
            const double theta0 = std::acos(dotProduct);     // angle between input vectors
            const double theta  = theta0 * inPercent;        // angle between v0 and result
 
            const double s0 = std::cos(theta) -
                              dotProduct * std::sin(theta) / std::sin(theta0); // == sin(theta_0 - theta) / sin(theta_0)
            const double s1 = std::sin(theta) / std::sin(theta0);
 
            NdArray<double> interpQuat = (quat1Copy.toNdArray() * s0) + (inQuat2.toNdArray() * s1);
            return Quaternion(interpQuat); // NOLINT(modernize-return-braced-init-list)
        }
 
        //============================================================================
        // Method Description:
        [[nodiscard]] Quaternion slerp(const Quaternion& inQuat2, double inPercent) const
        {
            return slerp(*this, inQuat2, inPercent);
        }
 
        //============================================================================
        // Method Description:
        [[nodiscard]] std::string str() const
        {
            std::string output = "[" + utils::num2str(i()) + ", " + utils::num2str(j()) + ", " + utils::num2str(k()) +
                                 ", " + utils::num2str(s()) + "]\n";
 
            return output;
        }
 
        //============================================================================
        // Method Description:
        [[nodiscard]] NdArray<double> toDCM() const
        {
            NdArray<double> dcm(3);
 
            const double q0 = i();
            const double q1 = j();
            const double q2 = k();
            const double q3 = s();
 
            const double q0sqr = utils::sqr(q0);
            const double q1sqr = utils::sqr(q1);
            const double q2sqr = utils::sqr(q2);
            const double q3sqr = utils::sqr(q3);
 
            dcm(0, 0) = q3sqr + q0sqr - q1sqr - q2sqr;
            dcm(0, 1) = 2. * (q0 * q1 - q3 * q2);
            dcm(0, 2) = 2. * (q0 * q2 + q3 * q1);
            dcm(1, 0) = 2. * (q0 * q1 + q3 * q2);
            dcm(1, 1) = q3sqr + q1sqr - q0sqr - q2sqr;
            dcm(1, 2) = 2. * (q1 * q2 - q3 * q0);
            dcm(2, 0) = 2. * (q0 * q2 - q3 * q1);
            dcm(2, 1) = 2. * (q1 * q2 + q3 * q0);
            dcm(2, 2) = q3sqr + q2sqr - q0sqr - q1sqr;
 
            return dcm;
        }
 
        //============================================================================
        // Method Description:
        [[nodiscard]] NdArray<double> toNdArray() const
        {
            auto componentsCopy = components_;
            return NdArray<double>(componentsCopy); // NOLINT(modernize-return-braced-init-list)
        }
 
        //============================================================================
        // Method Description:
        static Quaternion xRotation(double inAngle) noexcept
        {
            const Vec3 eulerAxis = { 1., 0., 0. };
            return Quaternion(eulerAxis, inAngle); // NOLINT(modernize-return-braced-init-list)
        }
 
        //============================================================================
        // Method Description:
        [[nodiscard]] double yaw() const noexcept
        {
            return std::atan2(2. * (s() * k() + i() * j()), 1. - 2. * (utils::sqr(j()) + utils::sqr(k())));
        }
 
        //============================================================================
        // Method Description:
        static Quaternion yawRotation(double inAngle) noexcept
        {
            return { 0., 0., inAngle };
        }
 
        //============================================================================
        // Method Description:
        static Quaternion yRotation(double inAngle) noexcept
        {
            const Vec3 eulerAxis = { 0., 1., 0. };
            return Quaternion(eulerAxis, inAngle); // NOLINT(modernize-return-braced-init-list)
        }
 
        //============================================================================
        // Method Description:
        static Quaternion zRotation(double inAngle) noexcept
        {
            const Vec3 eulerAxis = { 0., 0., 1. };
            return Quaternion(eulerAxis, inAngle); // NOLINT(modernize-return-braced-init-list)
        }
 
        //============================================================================
        // Method Description:
        bool operator==(const Quaternion& inRhs) const noexcept
        {
            const auto comparitor = [](double value1, double value2) noexcept -> bool
            { return utils::essentiallyEqual(value1, value2); };
 
            return stl_algorithms::equal(components_.begin(), components_.end(), inRhs.components_.begin(), comparitor);
        }
 
        //============================================================================
        // Method Description:
        bool operator!=(const Quaternion& inRhs) const noexcept
        {
            return !(*this == inRhs);
        }
 
        //============================================================================
        // Method Description:
        Quaternion& operator+=(const Quaternion& inRhs) noexcept
        {
            stl_algorithms::transform(components_.begin(),
                                      components_.end(),
                                      inRhs.components_.begin(),
                                      components_.begin(),
                                      std::plus<double>()); // NOLINT(modernize-use-transparent-functors)
 
            normalize();
 
            return *this;
        }
 
        //============================================================================
        // Method Description:
        Quaternion operator+(const Quaternion& inRhs) const noexcept
        {
            return Quaternion(*this) += inRhs;
        }
 
        //============================================================================
        // Method Description:
        Quaternion& operator-=(const Quaternion& inRhs) noexcept
        {
            stl_algorithms::transform(components_.begin(),
                                      components_.end(),
                                      inRhs.components_.begin(),
                                      components_.begin(),
                                      std::minus<double>()); // NOLINT(modernize-use-transparent-functors)
 
            normalize();
 
            return *this;
        }
 
        //============================================================================
        // Method Description:
        Quaternion operator-(const Quaternion& inRhs) const noexcept
        {
            return Quaternion(*this) -= inRhs;
        }
 
        //============================================================================
        // Method Description:
        Quaternion operator-() const noexcept
        {
            return Quaternion(*this) *= -1.;
        }
 
        //============================================================================
        // Method Description:
        Quaternion& operator*=(const Quaternion& inRhs) noexcept
        {
            double q0 = inRhs.s() * i();
            q0 += inRhs.i() * s();
            q0 -= inRhs.j() * k();
            q0 += inRhs.k() * j();
 
            double q1 = inRhs.s() * j();
            q1 += inRhs.i() * k();
            q1 += inRhs.j() * s();
            q1 -= inRhs.k() * i();
 
            double q2 = inRhs.s() * k();
            q2 -= inRhs.i() * j();
            q2 += inRhs.j() * i();
            q2 += inRhs.k() * s();
 
            double q3 = inRhs.s() * s();
            q3 -= inRhs.i() * i();
            q3 -= inRhs.j() * j();
            q3 -= inRhs.k() * k();
 
            components_[0] = q0;
            components_[1] = q1;
            components_[2] = q2;
            components_[3] = q3;
 
            normalize();
 
            return *this;
        }
 
        //============================================================================
        // Method Description:
        Quaternion& operator*=(double inScalar) noexcept
        {
            stl_algorithms::for_each(components_.begin(),
                                     components_.end(),
                                     [inScalar](double& component) { component *= inScalar; });
 
            normalize();
 
            return *this;
        }
 
        //============================================================================
        // Method Description:
        Quaternion operator*(const Quaternion& inRhs) const noexcept
        {
            return Quaternion(*this) *= inRhs;
        }
 
        //============================================================================
        // Method Description:
        Quaternion operator*(double inScalar) const noexcept
        {
            return Quaternion(*this) *= inScalar;
        }
 
        //============================================================================
        // Method Description:
        NdArray<double> operator*(const NdArray<double>& inVec) const
        {
            if (inVec.size() != 3)
            {
                THROW_INVALID_ARGUMENT_ERROR("input vector must be a cartesion vector of length = 3.");
            }
 
            const auto vecNorm = norm(inVec).item();
            if (utils::essentiallyEqual(vecNorm, 0.))
            {
                return inVec;
            }
 
            const auto p      = Quaternion(inVec[0], inVec[1], inVec[2], 0.);
            const auto pPrime = *this * p * this->inverse();
 
            NdArray<double> rotatedVec = { pPrime.i(), pPrime.j(), pPrime.k() };
            rotatedVec *= vecNorm;
            return rotatedVec;
        }
 
        //============================================================================
        // Method Description:
        Vec3 operator*(const Vec3& inVec3) const
        {
            return *this * inVec3.toNdArray();
        }
 
        //============================================================================
        // Method Description:
        Quaternion& operator/=(const Quaternion& inRhs) noexcept
        {
            return *this *= inRhs.conjugate();
        }
 
        //============================================================================
        // Method Description:
        Quaternion operator/(const Quaternion& inRhs) const noexcept
        {
            return Quaternion(*this) /= inRhs;
        }
 
        //============================================================================
        // Method Description:
        friend std::ostream& operator<<(std::ostream& inOStream, const Quaternion& inQuat)
        {
            inOStream << inQuat.str();
            return inOStream;
        }
 
    private:
        //====================================Attributes==============================
        std::array<double, 4> components_{ { 0., 0., 0., 1. } };
 
        //============================================================================
        // Method Description:
        void normalize() noexcept
        {
            double sumOfSquares = 0.;
            std::for_each(components_.begin(),
                          components_.end(),
                          [&sumOfSquares](double component) noexcept -> void
                          { sumOfSquares += utils::sqr(component); });
 
            const double norm = std::sqrt(sumOfSquares);
            stl_algorithms::for_each(components_.begin(),
                                     components_.end(),
                                     [norm](double& component) noexcept -> void { component /= norm; });
        }
 
        //============================================================================
        // Method Description:
        void eulerToQuat(double roll, double pitch, double yaw) noexcept
        {
            const auto halfPhi   = roll / 2.;
            const auto halfTheta = pitch / 2.;
            const auto halfPsi   = yaw / 2.;
 
            const auto sinHalfPhi = std::sin(halfPhi);
            const auto cosHalfPhi = std::cos(halfPhi);
 
            const auto sinHalfTheta = std::sin(halfTheta);
            const auto cosHalfTheta = std::cos(halfTheta);
 
            const auto sinHalfPsi = std::sin(halfPsi);
            const auto cosHalfPsi = std::cos(halfPsi);
 
            components_[0] = sinHalfPhi * cosHalfTheta * cosHalfPsi;
            components_[0] -= cosHalfPhi * sinHalfTheta * sinHalfPsi;
 
            components_[1] = cosHalfPhi * sinHalfTheta * cosHalfPsi;
            components_[1] += sinHalfPhi * cosHalfTheta * sinHalfPsi;
 
            components_[2] = cosHalfPhi * cosHalfTheta * sinHalfPsi;
            components_[2] -= sinHalfPhi * sinHalfTheta * cosHalfPsi;
 
            components_[3] = cosHalfPhi * cosHalfTheta * cosHalfPsi;
            components_[3] += sinHalfPhi * sinHalfTheta * sinHalfPsi;
        }
 
        //============================================================================
        // Method Description:
        void dcmToQuat(const NdArray<double>& dcm)
        {
            const Shape inShape = dcm.shape();
            if (!(inShape.rows == 3 && inShape.cols == 3))
            {
                THROW_INVALID_ARGUMENT_ERROR("input direction cosine matrix must have shape = (3,3).");
            }
 
            NdArray<double> checks(1, 4);
            checks[0] = 1 + dcm(0, 0) + dcm(1, 1) + dcm(2, 2);
            checks[1] = 1 + dcm(0, 0) - dcm(1, 1) - dcm(2, 2);
            checks[2] = 1 - dcm(0, 0) + dcm(1, 1) - dcm(2, 2);
            checks[3] = 1 - dcm(0, 0) - dcm(1, 1) + dcm(2, 2);
 
            const uint32 maxIdx = argmax(checks).item();
 
            switch (maxIdx)
            {
                case 0:
                {
                    components_[3] = 0.5 * std::sqrt(1 + dcm(0, 0) + dcm(1, 1) + dcm(2, 2));
                    components_[0] = (dcm(2, 1) - dcm(1, 2)) / (4 * components_[3]);
                    components_[1] = (dcm(0, 2) - dcm(2, 0)) / (4 * components_[3]);
                    components_[2] = (dcm(1, 0) - dcm(0, 1)) / (4 * components_[3]);
 
                    break;
                }
                case 1:
                {
                    components_[0] = 0.5 * std::sqrt(1 + dcm(0, 0) - dcm(1, 1) - dcm(2, 2));
                    components_[1] = (dcm(1, 0) + dcm(0, 1)) / (4 * components_[0]);
                    components_[2] = (dcm(2, 0) + dcm(0, 2)) / (4 * components_[0]);
                    components_[3] = (dcm(2, 1) - dcm(1, 2)) / (4 * components_[0]);
 
                    break;
                }
                case 2:
                {
                    components_[1] = 0.5 * std::sqrt(1 - dcm(0, 0) + dcm(1, 1) - dcm(2, 2));
                    components_[0] = (dcm(1, 0) + dcm(0, 1)) / (4 * components_[1]);
                    components_[2] = (dcm(2, 1) + dcm(1, 2)) / (4 * components_[1]);
                    components_[3] = (dcm(0, 2) - dcm(2, 0)) / (4 * components_[1]);
 
                    break;
                }
                case 3:
                {
                    components_[2] = 0.5 * std::sqrt(1 - dcm(0, 0) - dcm(1, 1) + dcm(2, 2));
                    components_[0] = (dcm(2, 0) + dcm(0, 2)) / (4 * components_[2]);
                    components_[1] = (dcm(2, 1) + dcm(1, 2)) / (4 * components_[2]);
                    components_[3] = (dcm(1, 0) - dcm(0, 1)) / (4 * components_[2]);
 
                    break;
                }
            }
        }
    };
} // namespace nc::rotations