Kernel/Math/Quaternion.h

Go to the documentation of this file.

// This file is part of the MercuryDPM project (https://www.mercurydpm.org).
// Copyright (c), The MercuryDPM Developers Team. All rights reserved.
// License: BSD 3-Clause License; see the LICENSE file in the root directory.
 
#ifndef MECURYDPM_QUATERNION_H
#define MECURYDPM_QUATERNION_H
 
#include <cmath>
#include <sstream>
#include <iostream>
#include <cstdlib>
#include "Math/Vector.h"
#include "Math/MatrixSymmetric.h"
 
#include "GeneralDefine.h"
#include "SmallVector.h"
#include "SmallMatrix.h"
 
class Quaternion
{
public:
    
    Mdouble q0;
    Mdouble q1;
    Mdouble q2;
    Mdouble q3;
    
    Quaternion();
    
    Quaternion(Mdouble q0, Mdouble q1, Mdouble q2, Mdouble q3);
    
    Quaternion(Vec3D normal)
    {
        setOrientationViaNormal(normal);
    }
    
    void setUnity();
    
    bool isUnity() const
    {
        return q0 == 1.0 && q1 == 0.0 && q2 == 0.0 && q3 == 0.0;
    };
    
    Quaternion operator+(const Quaternion& a) const;
    
    Quaternion operator-(const Quaternion& a) const;
    
    Quaternion operator*(Mdouble a) const;
    
    Quaternion operator/(Mdouble a) const;
    
    Quaternion& operator+=(const Quaternion& a);
    
    Quaternion& operator-=(const Quaternion& a);
    
    Quaternion& operator*=(Mdouble a);
    
    Quaternion& operator/=(Mdouble a);
    
    void normalise();
    
    void setLength(Mdouble length);
    
    static Mdouble getDistance(const Quaternion& a, const Quaternion& b);
    
    static Mdouble getDistanceSquared(const Quaternion& a, const Quaternion& b);
    
    static Mdouble getLength(const Quaternion& a);
    
    static Mdouble getLengthSquared(const Quaternion& a);
    
    Mdouble getLength() const;
    
    Mdouble getLengthSquared() const;
    
    Mdouble getComponent(int index) const;
    
    Quaternion angularVelocityBodyFixedFrameToAngularDisplacement(Vec3D v) const;
    
    Quaternion angularDisplacementTimeDerivative(Vec3D v) const;
    
    void updateAngularDisplacement(Vec3D angularVelocityDt);
    
    Vec3D applyCInverse(Quaternion q) const;
    
    void setComponent(int index, double val);
    
    bool isEqualTo(const Quaternion& other, double tol) const;
    
    static Quaternion getUnitQuaternion(const Quaternion& a);
    
    friend std::ostream& operator<<(std::ostream& os, const Quaternion& a);
    
    friend std::istream& operator>>(std::istream& is, Quaternion& a);
    
    friend Quaternion operator+(Mdouble a, const Quaternion& b);
    
    friend Quaternion operator-(Mdouble a, const Quaternion& b);
    
    friend Quaternion operator-(const Quaternion& a);
    
    friend Quaternion operator*(Mdouble a, const Quaternion& b);
    
    Vec3D getEuler() const;
    
    
    void setEuler(const Vec3D& e);
    
    Mdouble getAngleZ() const;
    
    void setAngleZ(Mdouble psi);
    
    MatrixSymmetric3D rotateInverseInertiaTensor(const MatrixSymmetric3D& invI) const;
    
    void rotateTensor(SmallMatrix<3, 3> I) const;
    
    Vec3D getAxis() const;
    
    void setOrientationViaNormal(Vec3D normal);
    
    void getRotationMatrix(SmallMatrix<3, 3>& A) const; //note: A is output parameter
    
    void rotate(Vec3D& position) const;
    
    Vec3D rotate(const Vec3D& position) const;
    
    void rotate(SmallVector<3>& position) const;
    
    void rotateBack(Vec3D& position) const;
    
    Vec3D rotateBack(const Vec3D& position) const;
    
    Mdouble getDistance(Vec3D p, Vec3D p0) const;
 
    // Quaternion multiplication
    static Quaternion multiplyQuaternions(const Quaternion& q, const Quaternion& r) {
        double w = q.q0 * r.q0 - q.q1 * r.q1 - q.q2 * r.q2 - q.q3 * r.q3;
        double x = q.q0 * r.q1 + q.q1 * r.q0 + q.q2 * r.q3 - q.q3 * r.q2;
        double y = q.q0 * r.q2 - q.q1 * r.q3 + q.q2 * r.q0 + q.q3 * r.q1;
        double z = q.q0 * r.q3 + q.q1 * r.q2 - q.q2 * r.q1 + q.q3 * r.q0;
        return {w, x, y, z};
    }
 
    static Quaternion fromAxisAndAngle(const Vec3D &axis, const Mdouble &angle)
    {
        double axisMagnitude = axis.getLength();
        logger.assert_debug(axisMagnitude > 0, "Quaternion::fromAxisAndangle: Axis Magnitude is %. Cannot compute a quaternion.", axisMagnitude);
 
        Mdouble halfAngle = angle / 2.0;
        Mdouble sinHalfAngle = std::sin(halfAngle);
        Mdouble q0 = std::cos(halfAngle);
        Mdouble q1 = axis.X * sinHalfAngle / axisMagnitude;
        Mdouble q2 = axis.Y * sinHalfAngle / axisMagnitude;
        Mdouble q3 = axis.Z * sinHalfAngle / axisMagnitude;
        return Quaternion(q0, q1, q2, q3);
    };
};
 
#endif