36 template <
typename Type>
39 template <
typename Type>
42 template <
typename Type>
52 template<
typename Type>
53 class Quaternion :
public Vector<Type, 4>
104 t =
sqrt(Type(1) + t);
105 q(0) = Type(0.5) * t;
107 q(1) = (R(2,1) - R(1,2)) * t;
108 q(2) = (R(0,2) - R(2,0)) * t;
109 q(3) = (R(1,0) - R(0,1)) * t;
110 }
else if (R(0,0) > R(1,1) && R(0,0) > R(2,2)) {
111 t =
sqrt(Type(1) + R(0,0) - R(1,1) - R(2,2));
112 q(1) = Type(0.5) * t;
114 q(0) = (R(2,1) - R(1,2)) * t;
115 q(2) = (R(1,0) + R(0,1)) * t;
116 q(3) = (R(0,2) + R(2,0)) * t;
117 }
else if (R(1,1) > R(2,2)) {
118 t =
sqrt(Type(1) - R(0,0) + R(1,1) - R(2,2));
119 q(2) = Type(0.5) * t;
121 q(0) = (R(0,2) - R(2,0)) * t;
122 q(1) = (R(1,0) + R(0,1)) * t;
123 q(3) = (R(2,1) + R(1,2)) * t;
125 t =
sqrt(Type(1) - R(0,0) - R(1,1) + R(2,2));
126 q(3) = Type(0.5) * t;
128 q(0) = (R(1,0) - R(0,1)) * t;
129 q(1) = (R(0,2) + R(2,0)) * t;
130 q(2) = (R(2,1) + R(1,2)) * t;
146 Type cosPhi_2 = Type(
cos(euler.
phi() / Type(2)));
147 Type cosTheta_2 = Type(
cos(euler.
theta() / Type(2)));
148 Type cosPsi_2 = Type(
cos(euler.
psi() / Type(2)));
149 Type sinPhi_2 = Type(
sin(euler.
phi() / Type(2)));
150 Type sinTheta_2 = Type(
sin(euler.
theta() / Type(2)));
151 Type sinPsi_2 = Type(
sin(euler.
psi() / Type(2)));
152 q(0) = cosPhi_2 * cosTheta_2 * cosPsi_2 +
153 sinPhi_2 * sinTheta_2 * sinPsi_2;
154 q(1) = sinPhi_2 * cosTheta_2 * cosPsi_2 -
155 cosPhi_2 * sinTheta_2 * sinPsi_2;
156 q(2) = cosPhi_2 * sinTheta_2 * cosPsi_2 +
157 sinPhi_2 * cosTheta_2 * sinPsi_2;
158 q(3) = cosPhi_2 * cosTheta_2 * sinPsi_2 -
159 sinPhi_2 * sinTheta_2 * cosPsi_2;
170 Type angle = aa.
norm();
172 if (angle < Type(1e-10)) {
174 q(1) = q(2) = q(3) = 0;
176 Type magnitude =
sin(angle / Type(2));
177 q(0) =
cos(angle / 2.0
f);
178 q(1) = axis(0) * magnitude;
179 q(2) = axis(1) * magnitude;
180 q(3) = axis(2) * magnitude;
196 const float dt = src.
dot(dst);
197 if (cr.
norm() < eps && dt < 0) {
259 r(0) = p(0) * q(0) - p(1) * q(1) - p(2) * q(2) - p(3) * q(3);
260 r(1) = p(0) * q(1) + p(1) * q(0) + p(2) * q(3) - p(3) * q(2);
261 r(2) = p(0) * q(2) - p(1) * q(3) + p(2) * q(0) + p(3) * q(1);
262 r(3) = p(0) * q(3) + p(1) * q(2) - p(2) * q(1) + p(3) * q(0);
312 return q * v * Type(0.5);
327 return v * q * Type(0.5);
346 Type normSq = q.dot(q);
371 if (q(0) < Type(0)) {
386 (*this) = res * (*this);
400 Quaternion v(Type(0), vec(0), vec(1), vec(2));
417 Quaternion v(Type(0), vec(0), vec(1), vec(2));
434 Type theta = vec.
norm();
436 if (theta < Type(1e-10)) {
438 q(1) = q(2) = q(3) = 0;
458 if (theta < Type(1e-10)) {
460 q(1) = q(2) = q(3) = Type(0);
463 Type magnitude =
sin(theta / 2.0
f);
465 q(0) =
cos(theta / 2.0
f);
466 q(1) = axis(0) * magnitude;
467 q(2) = axis(1) * magnitude;
468 q(3) = axis(2) * magnitude;
484 Type axis_magnitude = Type(
sqrt(q(1) * q(1) + q(2) * q(2) + q(3) * q(3)));
490 if (axis_magnitude >= Type(1e-10)) {
491 vec = vec / axis_magnitude;
492 vec = vec *
wrap_pi(Type(2) *
atan2(axis_magnitude, q(0)));
522 R_z(0) = 2 * (a * c + b * d);
523 R_z(1) = 2 * (c * d - a * b);
524 R_z(2) = a * a - b * b - c * c + d * d;
Quaternion(const Vector3< Type > &src, const Vector3< Type > &dst, const Type eps=Type(1e-5))
Quaternion from two vectors Generates shortest rotation from source to destination vector...
void canonicalize()
Bring quaternion to canonical form.
Vector3 cross(const Matrix31 &b) const
Quaternion operator*(Type scalar) const
Scalar multiplication operator.
Dcm< Type > to_dcm()
XXX DEPRECATED, can use assignment or ctor.
Vector3< Type > dcm_z() const
Corresponding body z-axis to an attitude quaternion / last orthogonal unit basis vector.
Quaternion(const AxisAngle< Type > &aa)
Quaternion from AxisAngle.
Quaternion from_dcm(const Matrix< Type, 3, 3 > &dcm)
XXX DEPRECATED, can use assignment or ctor.
void invert()
Invert quaternion in place.
Direction cosine matrix class.
void from_axis_angle(const Vector< Type, 3 > &axis, Type theta)
Rotation quaternion from axis and angle XXX DEPRECATED, use AxisAngle class.
Quaternion(const Dcm< Type > &R)
Constructor from dcm.
Quaternion canonical() const
Return canonical form of the quaternion.
Vector3< Type > conjugate(const Vector3< Type > &vec) const
Rotates vector v_1 in frame 1 to vector v_2 in frame 2 using the rotation quaternion q_21 describing ...
Type norm_squared() const
Matrix< Type, 3, 1 > Matrix31
Quaternion< float > Quaternionf
void operator*=(Type scalar)
Scalar self-multiplication operator.
Vector< Type, 3 > to_axis_angle() const
Rotation vector from quaternion XXX DEPRECATED, use AxisAngle class.
void rotate(const AxisAngle< Type > &vec)
Rotate quaternion from rotation vector.
Matrix41 derivative2(const Matrix31 &w) const
Computes the derivative of q_21 when rotated with angular velocity expressed in frame 2 v_2 = q_21 * ...
Dual< Scalar, N > cos(const Dual< Scalar, N > &a)
void from_axis_angle(Vector< Type, 3 > vec)
Rotation quaternion from vector.
Matrix41 derivative1(const Matrix31 &w) const
Computes the derivative of q_21 when rotated with angular velocity expressed in frame 1 v_2 = q_21 * ...
Quaternion operator*(const Quaternion &q) const
Quaternion multiplication operator.
Type dot(const MatrixM1 &b) const
Matrix< Type, 4, 1 > Matrix41
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Matrix< Type, M, N > abs() const
Vector3< Type > imag() const
Imaginary components of quaternion.
Type wrap_pi(Type x)
Wrap value in range [-π, π)
Quaternion(Type a, Type b, Type c, Type d)
Constructor from quaternion values.
Quaternion inversed() const
Invert quaternion.
Quaternion< float > Quatf
void operator*=(const Quaternion &other)
Self-multiplication operator.
Quaternion(const Type data_[4])
Constructor from array.
Quaternion(const Matrix41 &other)
Constructor from Matrix41.
Quaternion()
Standard constructor.
Vector3< Type > conjugate_inversed(const Vector3< Type > &vec) const
Rotates vector v_2 in frame 2 to vector v_1 in frame 1 using the rotation quaternion q_21 describing ...
Dual< Scalar, N > sin(const Dual< Scalar, N > &a)
Quaternion(const Euler< Type > &euler)
Constructor from euler angles.
Dual< Scalar, N > atan2(const Dual< Scalar, N > &a, const Dual< Scalar, N > &b)