Quaternion.h

Go to the documentation of this file.

/*
 * Original Copyright (c) 2011-2016 Martin Felis <martin.felis@iwr.uni-heidelberg.de>
 *
 *
 * RDL - Robot Dynamics Library
 * Modifications Copyright (c) 2017 Jordan Lack <jlack1987@gmail.com>
 *
 * Licensed under the zlib license. See LICENSE for more details.
 */

#ifndef __RDL_QUATERNION_H__
#define __RDL_QUATERNION_H__

#include <cmath>
#include "rdl_dynamics/rdl_eigenmath.h"

namespace RobotDynamics
{
namespace Math
{
class Quaternion : public Vector4d, public TransformableGeometricObject
{
  public:
    Quaternion() : Vector4d(0., 0., 0., 1.)
    {
    }

    // cppcheck-suppress noExplicitConstructor
    Quaternion(const Eigen::Quaterniond& q) : Quaternion(q.x(), q.y(), q.z(), q.w())
    {
    }

    // cppcheck-suppress noExplicitConstructor
    Quaternion(const RobotDynamics::Math::Quaternion& q) : Vector4d(q.x(), q.y(), q.z(), q.w())
    {
    }

    // cppcheck-suppress noExplicitConstructor
    Quaternion(const RobotDynamics::Math::Matrix3d& E) : Quaternion(toQuaternion(E))
    {
    }

    // cppcheck-suppress noExplicitConstructor
    Quaternion(const Vector4d& q) : Vector4d(q)
    {
    }

    // cppcheck-suppress noExplicitConstructor
    Quaternion(const AxisAngle& axis_angle) : Quaternion(axis_angle.axis(), axis_angle.angle())
    {
    }

    Quaternion(const Vector3d& axis, double angle)
    {
        set(axis, angle);
    }

    Quaternion(double x, double y, double z, double w) : Vector4d(x, y, z, w)
    {
    }

    EIGEN_STRONG_INLINE double& x()
    {
        return this->data()[0];
    }

    EIGEN_STRONG_INLINE double x() const
    {
        return this->data()[0];
    }

    EIGEN_STRONG_INLINE double& y()
    {
        return this->data()[1];
    }

    EIGEN_STRONG_INLINE double y() const
    {
        return this->data()[1];
    }

    EIGEN_STRONG_INLINE double& z()
    {
        return this->data()[2];
    }

    EIGEN_STRONG_INLINE double z() const
    {
        return this->data()[2];
    }

    EIGEN_STRONG_INLINE double& w()
    {
        return this->data()[3];
    }

    EIGEN_STRONG_INLINE double w() const
    {
        return this->data()[3];
    }

    void set(double x, double y, double z, double w)
    {
        this->data()[0] = x;
        this->data()[1] = y;
        this->data()[2] = z;
        this->data()[3] = w;
    }

    EIGEN_STRONG_INLINE Vector3d vec() const
    {
        return Vector3d(this->data()[0], this->data()[1], this->data()[2]);
    }

    [[deprecated("Use Quaternion::w() instead")]] EIGEN_STRONG_INLINE double getScalarPart() const { return this->data()[3]; }

    Quaternion&
    operator=(const Eigen::Quaterniond& q)
    {
        set(q.x(), q.y(), q.z(), q.w());
        return *this;
    }

    void sanitize()
    {
        if (!std::signbit(w()))
        {
            return;
        }

        x() *= -1.0;
        y() *= -1.0;
        z() *= -1.0;
        w() *= -1.0;
    }

    Quaternion& operator=(const RobotDynamics::Math::Quaternion& q)
    {
        set(q.x(), q.y(), q.z(), q.w());
        return *this;
    }

    Quaternion& operator=(const Vector4d& q)
    {
        set(q[0], q[1], q[2], q[3]);
        return *this;
    }

    Quaternion& operator=(const Matrix3d& E)
    {
        set(E);
        return *this;
    }

    Quaternion& operator=(const AxisAngle& axis_angle)
    {
        set(axis_angle);
        return *this;
    }

    Quaternion operator*(const double& s) const
    {
        return Quaternion((*this)[0] * s, (*this)[1] * s, (*this)[2] * s, (*this)[3] * s);
    }

    Quaternion operator*(const Quaternion& q) const
    {
        return Quaternion(
            (*this)[3] * q[0] + (*this)[0] * q[3] + (*this)[1] * q[2] - (*this)[2] * q[1], (*this)[3] * q[1] + (*this)[1] * q[3] + (*this)[2] * q[0] - (*this)[0] * q[2],
            (*this)[3] * q[2] + (*this)[2] * q[3] + (*this)[0] * q[1] - (*this)[1] * q[0], (*this)[3] * q[3] - (*this)[0] * q[0] - (*this)[1] * q[1] - (*this)[2] * q[2]);
    }

    Quaternion& operator*=(const Quaternion& q)
    {
        set((*this)[3] * q[0] + (*this)[0] * q[3] + (*this)[1] * q[2] - (*this)[2] * q[1], (*this)[3] * q[1] + (*this)[1] * q[3] + (*this)[2] * q[0] - (*this)[0] * q[2],
            (*this)[3] * q[2] + (*this)[2] * q[3] + (*this)[0] * q[1] - (*this)[1] * q[0], (*this)[3] * q[3] - (*this)[0] * q[0] - (*this)[1] * q[1] - (*this)[2] * q[2]);
        return *this;
    }

    void set(const Matrix3d& E)
    {
        *this = toQuaternion(E);
    }

    void set(const Quaternion& q)
    {
        *this = q;
    }

    void set(const AxisAngle& axis_angle)
    {
        set(axis_angle.axis(), axis_angle.angle());
    }

    void set(Vector3d axis, double angle)
    {
        double d = axis.norm();
        double s2 = std::sin(angle * 0.5) / d;
        set(axis[0] * s2, axis[1] * s2, axis[2] * s2, std::cos(angle * 0.5));
    }

    void transform(const RobotDynamics::Math::SpatialTransform& X)
    {
        *this = toQuaternion(X.E.transpose()) * (*this);
    }

    Quaternion slerp(double alpha, const Quaternion& quat) const
    {
        // check whether one of the two has 0 length
        double s = std::sqrt(squaredNorm() * quat.squaredNorm());

        // division by 0.f is unhealthy!
        assert(s != 0.);

        double angle = acos(dot(quat) / s);

        if ((angle == 0.) || std::isnan(angle))
        {
            return *this;
        }
        assert(!std::isnan(angle));

        double d = 1. / std::sin(angle);
        double p0 = std::sin((1. - alpha) * angle);
        double p1 = std::sin(alpha * angle);

        if (dot(quat) < 0.)
        {
            return Quaternion(Vector4d(((*this) * p0 - quat * p1) * d));
        }
        return Quaternion(Vector4d(((*this) * p0 + quat * p1) * d));
    }

    Quaternion conjugate() const
    {
        return Quaternion(-(*this)[0], -(*this)[1], -(*this)[2], (*this)[3]);
    }

    Quaternion timeStep(const Vector3d& omega, double dt)
    {
        double omega_norm = omega.norm();

        return fromAxisAngle(omega / omega_norm, dt * omega_norm) * (*this);
    }

    Vector3d rotate(const Vector3d& vec) const
    {
        Vector3d vn(vec);
        Quaternion vec_quat(vn[0], vn[1], vn[2], 0.f), res_quat;

        res_quat = vec_quat * (*this);
        res_quat = conjugate() * res_quat;

        return Vector3d(res_quat[0], res_quat[1], res_quat[2]);
    }

    void swingTwistDecomposition(const Vector3d& twist_axis, Quaternion& swing, Quaternion& twist)
    {
        double u = twist_axis.dot(vec());
        double n = twist_axis.squaredNorm();
        double m = w() * n;
        double l = std::sqrt(std::pow(m, 2) + std::pow(u, 2) * n);
        twist.set(twist_axis.x() * (u / l), twist_axis.y() * (u / l), twist_axis.z() * (u / l), m / l);
        swing.set(*this * twist.conjugate());
    }

  private:
    Quaternion toQuaternion(const Matrix3d& mat)
    {
        double trace = mat.trace();

        if (trace > 0.)
        {
            double s = 2. * std::sqrt(trace + 1.);
            return Quaternion((mat(1, 2) - mat(2, 1)) / s, (mat(2, 0) - mat(0, 2)) / s, (mat(0, 1) - mat(1, 0)) / s, 0.25 * s);
        }
        else if ((mat(0, 0) > mat(1, 1)) && (mat(0, 0) > mat(2, 2)))
        {
            double s = 2. * std::sqrt(1. + mat(0, 0) - mat(1, 1) - mat(2, 2));
            return Quaternion(-0.25 * s, (-mat(0, 1) - mat(1, 0)) / s, (-mat(0, 2) - mat(2, 0)) / s, (mat(2, 1) - mat(1, 2)) / s);
        }
        else if (mat(1, 1) > mat(2, 2))
        {
            double s = 2. * std::sqrt(1. + mat(1, 1) - mat(0, 0) - mat(2, 2));
            return Quaternion((-mat(0, 1) - mat(1, 0)) / s, -0.25 * s, (-mat(1, 2) - mat(2, 1)) / s, (mat(0, 2) - mat(2, 0)) / s);
        }
        else
        {
            double s = 2. * std::sqrt(1. + mat(2, 2) - mat(0, 0) - mat(1, 1));
            return Quaternion((-mat(0, 2) - mat(2, 0)) / s, (-mat(1, 2) - mat(2, 1)) / s, -0.25 * s, (mat(1, 0) - mat(0, 1)) / s);
        }
    }

    Quaternion fromAxisAngle(const Vector3d& axis, double angle_rad)
    {
        double d = axis.norm();
        double s2 = std::sin(angle_rad * 0.5) / d;

        return Quaternion(axis[0] * s2, axis[1] * s2, axis[2] * s2, std::cos(angle_rad * 0.5));
    }
};
}  // namespace Math
}  // namespace RobotDynamics

EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(RobotDynamics::Math::Quaternion)

/* __RDL_QUATERNION_H__ */
#endif  // ifndef __RDL_QUATERNION_H__