Quaternion.h

#ifndef BIORBD_UTILS_QUATERNION_H
#define BIORBD_UTILS_QUATERNION_H
 
#include <memory>
#include "Utils/Vector3d.h"
#include "Utils/Scalar.h"
 
#include "biorbdConfig.h"
namespace biorbd {
namespace utils {
class Vector3d;
class Vector;
class RotoTrans;
class Rotation;
class String;
 
#ifdef SWIG
class BIORBD_API Quaternion
#else
class BIORBD_API Quaternion : public RigidBodyDynamics::Math::Vector4d
#endif
{
public:
    Quaternion (
            double kStabilizer = 1);
    
    Quaternion(
            const biorbd::utils::Quaternion &other);
 
    Quaternion (
        const RigidBodyDynamics::Math::Vector4d &vec4,
        double kStabilizer = 1);
 
    Quaternion (
            const biorbd::utils::Scalar& w,
            const biorbd::utils::Scalar& x,
            const biorbd::utils::Scalar& y,
            const biorbd::utils::Scalar& z,
            double kStabilizer = 1);
    
    Quaternion (
        const biorbd::utils::Scalar& w,
        const biorbd::utils::Vector3d &vec3, 
        double kStabilizer = 1);
 
    biorbd::utils::Scalar w() const;
 
    biorbd::utils::Scalar x() const;
 
    biorbd::utils::Scalar y() const;
 
    biorbd::utils::Scalar z() const;
 
    void setKStab(double newKStab);
 
    double kStab() const;
 
#ifndef SWIG
 
#ifdef BIORBD_USE_EIGEN3_MATH
 
    template<typename OtherDerived>
    biorbd::utils::Quaternion& operator=(
            const Eigen::MatrixBase <OtherDerived>& other){
        this->Eigen::Vector4d::operator=(other);
        // I don't understand why the next line doesn't SegFault...
        this->m_Kstab = static_cast<biorbd::utils::Quaternion>(other).m_Kstab;
        return *this;
    }
 
#endif
 
#endif
 
    biorbd::utils::Quaternion operator*(
            const biorbd::utils::Quaternion& other) const;
 
    biorbd::utils::Quaternion operator*(
            const biorbd::utils::Scalar& scalar) const;
 
    biorbd::utils::Quaternion operator*(
            float scalar) const;
 
#ifdef BIORBD_USE_CASADI_MATH
    biorbd::utils::Quaternion operator*(
            double scalar) const;
#endif
 
    biorbd::utils::Quaternion operator+(
            const biorbd::utils::Quaternion& other) const;
 
    biorbd::utils::Quaternion operator-(
            const biorbd::utils::Quaternion& other) const;
 
    static biorbd::utils::Quaternion fromGLRotate (
            const biorbd::utils::Scalar& angle,
            const biorbd::utils::Scalar& x,
            const biorbd::utils::Scalar& y,
            const biorbd::utils::Scalar& z,
            double kStab = 1);
 
    static biorbd::utils::Quaternion fromAxisAngle (
            const biorbd::utils::Scalar& angle,
            const biorbd::utils::Vector3d &axis,
            double kStab = 1);
 
    static biorbd::utils::Quaternion fromMatrix (
            const biorbd::utils::RotoTrans& rt,
            double kStab = 1);
 
    static biorbd::utils::Quaternion fromMatrix (
            const biorbd::utils::Rotation &mat,
            double kStab = 1);
 
    static biorbd::utils::Quaternion fromZYXAngles (
            const biorbd::utils::Vector3d &zyx_angles,
            double kStab = 1);
 
    static biorbd::utils::Quaternion fromYXZAngles (
            const biorbd::utils::Vector3d &yxz_angles,
            double kStab = 1);
 
    static biorbd::utils::Quaternion fromXYZAngles (
            const biorbd::utils::Vector3d &xyz_angles,
            double kStab = 1);
 
    biorbd::utils::Rotation toMatrix(
            bool skipAsserts = false) const;
 
#ifndef BIORBD_USE_CASADI_MATH
    biorbd::utils::Quaternion slerp (
            double alpha,
            const Quaternion &quat) const;
#endif
 
    biorbd::utils::Quaternion conjugate() const;
 
    biorbd::utils::Quaternion timeStep (
            const biorbd::utils::Vector3d &omega,
            double dt);
 
    biorbd::utils::Vector3d rotate (
            const biorbd::utils::Vector3d &vec) const;
 
    biorbd::utils::Quaternion omegaToQDot(
            const biorbd::utils::Vector3d& omega) const;
 
    biorbd::utils::Vector3d  eulerDotToOmega(
            const biorbd::utils::Vector3d &eulerDot, 
            const biorbd::utils::Vector3d &euler,
            const biorbd::utils::String& seq);
 
    void derivate(
            const biorbd::utils::Vector &w);
 
    void normalize();
 
protected:
    double m_Kstab; 
 
};
 
}}
 
#endif // BIORBD_UTILS_QUATERNION_H