Biorbd Release_1.13.0
Loading...
Searching...
No Matches
Quaternion.h
1#ifndef BIORBD_UTILS_QUATERNION_H
2#define BIORBD_UTILS_QUATERNION_H
3
4#include "biorbdConfig.h"
5
6#include "Utils/Scalar.h"
7namespace BIORBD_NAMESPACE {
8namespace utils {
9class Vector3d;
10class Matrix3d;
11class Vector;
12class RotoTrans;
13class Rotation;
14class String;
15
22#ifdef SWIG
23class BIORBD_API Quaternion
24#else
25class BIORBD_API Quaternion : public RigidBodyDynamics::Math::Vector4d
26#endif
27{
28 public:
33 Quaternion(const Scalar& kStabilizer = 1);
34
39 Quaternion(const Quaternion& other);
40
47 const RigidBodyDynamics::Math::Vector4d& vec4,
48 const Scalar& kStabilizer = 1);
49
59 const Scalar& w,
60 const Scalar& x,
61 const Scalar& y,
62 const Scalar& z,
63 const Scalar& kStabilizer = 1);
64
72 const Scalar& w,
73 const Vector3d& vec3,
74 const Scalar& kStabilizer = 1);
75
80 Scalar w() const;
81
86 Scalar x() const;
87
92 Scalar y() const;
93
98 Scalar z() const;
99
104 void setKStab(const Scalar& newKStab);
105
113 Scalar kStab() const;
114
115#ifndef SWIG
116
117#ifdef BIORBD_USE_EIGEN3_MATH
118
123 template <typename OtherDerived>
124 Quaternion& operator=(const Eigen::MatrixBase<OtherDerived>& other) {
125 this->Eigen::Vector4d::operator=(other);
126 // I don't understand why the next line doesn't SegFault...
127 this->m_Kstab = static_cast<Quaternion>(other).m_Kstab;
128 return *this;
129 }
130
131#endif
132
133#endif
134
140 Quaternion operator*(const Quaternion& other) const;
141
147 Quaternion operator*(const Scalar& scalar) const;
148
154 Quaternion operator*(float scalar) const;
155
156#ifdef BIORBD_USE_CASADI_MATH
161 Quaternion operator*(double scalar) const;
162#endif
163
169 Quaternion operator+(const Quaternion& other) const;
170
176 Quaternion operator-(const Quaternion& other) const;
177
188 const Scalar& angle,
189 const Scalar& x,
190 const Scalar& y,
191 const Scalar& z,
192 const Scalar& kStab = 1);
193
202 const Scalar& angle,
203 const Vector3d& axis,
204 const Scalar& kStab = 1);
205
212 static Quaternion fromMatrix(const RotoTrans& rt, const Scalar& kStab = 1);
213
220 static Quaternion fromMatrix(const Rotation& mat, const Scalar& kStab = 1);
221
230 const Vector3d& zyx_angles,
231 const Scalar& kStab = 1);
232
241 const Vector3d& yxz_angles,
242 const Scalar& kStab = 1);
243
252 const Vector3d& xyz_angles,
253 const Scalar& kStab = 1);
254
265 Rotation toMatrix(bool skipAsserts = false) const;
266
267#ifndef BIORBD_USE_CASADI_MATH
274 Quaternion slerp(double alpha, const Quaternion& quat) const;
275#endif
276
281 Quaternion conjugate() const;
282
289 Quaternion timeStep(const Vector3d& omega, const Scalar& dt);
290
296 Vector3d rotate(const Vector3d& vec) const;
297
305 Quaternion omegaToQuatDot(const Vector3d& omega) const;
306
314 Matrix3d velocityMatrix(const Vector3d& euler, const String& seq);
315
324 const Vector3d& eulerDot,
325 const Vector3d& euler,
326 const String& seq);
327
337 omegaToEulerDot(const Vector3d& euler, const Vector3d& w, const String& seq);
338
343 void derivate(const Vector& w);
344
348 void normalize();
349
350 protected:
351 utils::Scalar m_Kstab;
352};
353
354} // namespace utils
355} // namespace BIORBD_NAMESPACE
356
357#endif // BIORBD_UTILS_QUATERNION_H
A wrapper for the Eigen::MatrixXd.
Definition Matrix3d.h:21
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.
Definition Quaternion.h:351
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.
Definition RotoTrans.h:33
Wrapper around the std::string class with augmented functionality.
Definition String.h:18
Wrapper around Eigen Vector3d and attach it to a parent.
Definition Vector3d.h:27
Wrapper of the Eigen VectorXd.
Definition Vector.h:21