1#ifndef BIORBD_UTILS_QUATERNION_H
2#define BIORBD_UTILS_QUATERNION_H
4#include "biorbdConfig.h"
6#include "Utils/Scalar.h"
7namespace BIORBD_NAMESPACE {
25class BIORBD_API
Quaternion :
public RigidBodyDynamics::Math::Vector4d
47 const RigidBodyDynamics::Math::Vector4d& vec4,
48 const Scalar& kStabilizer = 1);
63 const Scalar& kStabilizer = 1);
74 const Scalar& kStabilizer = 1);
104 void setKStab(
const Scalar& newKStab);
113 Scalar
kStab()
const;
117#ifdef BIORBD_USE_EIGEN3_MATH
123 template <
typename OtherDerived>
124 Quaternion& operator=(
const Eigen::MatrixBase<OtherDerived>& other) {
125 this->Eigen::Vector4d::operator=(other);
156#ifdef BIORBD_USE_CASADI_MATH
192 const Scalar&
kStab = 1);
204 const Scalar&
kStab = 1);
231 const Scalar&
kStab = 1);
242 const Scalar&
kStab = 1);
253 const Scalar&
kStab = 1);
267#ifndef BIORBD_USE_CASADI_MATH
A wrapper for the Eigen::MatrixXd.
Quaternion slerp(double alpha, const Quaternion &quat) const
Interpolation of the quaternion between to position.
Quaternion conjugate() const
Return the conjugate of the quaternion.
Scalar y() const
Return the Y-Component of the imaginary part of the Quaternion.
Scalar x() const
Return the X-Component of the imaginary part of the Quaternion.
Quaternion omegaToQuatDot(const Vector3d &omega) const
Converts a 3d angular velocity vector into a 4d derivative of the components of the quaternion.
Scalar w() const
Return the real part (w) the Quaternion.
Matrix3d velocityMatrix(const Vector3d &euler, const String &seq)
Generate the velocity matrix which allows to go from/to euler angles to/from omega(body velocity).
Quaternion timeStep(const Vector3d &omega, const Scalar &dt)
Integrate the quaternion from its velocity.
static Quaternion fromGLRotate(const Scalar &angle, const Scalar &x, const Scalar &y, const Scalar &z, const Scalar &kStab=1)
Construct Quaternion from a GL.
Quaternion operator+(const Quaternion &other) const
Add the quaternion to another.
Scalar kStab() const
Return the k stabilizer.
Vector3d eulerDotToOmega(const Vector3d &eulerDot, const Vector3d &euler, const String &seq)
Converts a 3d angular velocity vector.
Vector3d omegaToEulerDot(const Vector3d &euler, const Vector3d &w, const String &seq)
converts a 3d vector of the body angular velocity (omega) into a 3d vector of the euler angles rate.
static Quaternion fromYXZAngles(const Vector3d &yxz_angles, const Scalar &kStab=1)
Construct Quaternion from Euler angles (sequece YXZ).
static Quaternion fromZYXAngles(const Vector3d &zyx_angles, const Scalar &kStab=1)
Construct Quaternion from Euler angles (sequece ZYX).
void normalize()
Force the normalization of the quaternion.
static Quaternion fromXYZAngles(const Vector3d &xyz_angles, const Scalar &kStab=1)
Construct Quaternion from Euler angles (sequece XYZ).
static Quaternion fromMatrix(const RotoTrans &rt, const Scalar &kStab=1)
Construct Quaternion from a RotoTrans matrix.
void setKStab(const Scalar &newKStab)
Set the k stabilizer.
Rotation toMatrix(bool skipAsserts=false) const
Convert the quaternion to a RotoTrans.
Scalar z() const
Return the Z-Component of the imaginary part of the Quaternion.
Quaternion operator-(const Quaternion &other) const
Subtract the quaternion to another.
utils::Scalar m_Kstab
Stabilization factor for the derivation.
void derivate(const Vector &w)
Return the time derivative of the quaterion.
Quaternion operator*(const Quaternion &other) const
Quaternion multiplication.
Quaternion(const Scalar &kStabilizer=1)
Construct Quaternion.
Vector3d rotate(const Vector3d &vec) const
Return a rotated vector from the quaternion.
static Quaternion fromAxisAngle(const Scalar &angle, const Vector3d &axis, const Scalar &kStab=1)
Construct Quaternion from an axis angle.
Homogenous matrix to describe translations and rotations simultaneously.
Wrapper around the std::string class with augmented functionality.
Wrapper around Eigen Vector3d and attach it to a parent.
Wrapper of the Eigen VectorXd.