Quaternion.h

Go to the documentation of this file.

//Copyright (c) 2013-2020, The MercuryDPM Developers Team. All rights reserved.
//For the list of developers, see <http://www.MercuryDPM.org/Team>.
//
//Redistribution and use in source and binary forms, with or without
//modification, are permitted provided that the following conditions are met:
//  * Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
//  * Redistributions in binary form must reproduce the above copyright
//    notice, this list of conditions and the following disclaimer in the
//    documentation and/or other materials provided with the distribution.
//  * Neither the name MercuryDPM nor the
//    names of its contributors may be used to endorse or promote products
//    derived from this software without specific prior written permission.
//
//THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
//ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
//WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
//DISCLAIMED. IN NO EVENT SHALL THE MERCURYDPM DEVELOPERS TEAM BE LIABLE FOR ANY
//DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
//(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
//LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
//ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
//(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
//SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

#ifndef QUATERNION_H
#define 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;
    
    void rotate(SmallVector<3>& position) const;
    
    void rotateBack(Vec3D& position) const;
    
    Mdouble getDistance(Vec3D p, Vec3D p0) const;
};

#endif