Rigid Body Dynamics Library
Quaternion.h
Go to the documentation of this file.
1 /*
2  * RBDL - Rigid Body Dynamics Library
3  * Copyright (c) 2011-2018 Martin Felis <martin@fysx.org>
4  *
5  * Licensed under the zlib license. See LICENSE for more details.
6  */
7 
8 #ifndef RBDL_QUATERNION_H
9 #define RBDL_QUATERNION_H
10 
11 #include <cmath>
12 
13 namespace RigidBodyDynamics {
14 
15 namespace Math {
16 
21 class Quaternion : public Vector4d {
22  public:
24  Vector4d (0., 0., 0., 1.)
25  {}
26  Quaternion (const Vector4d &vec4) :
27  Vector4d (vec4)
28  {}
29  Quaternion (double x, double y, double z, double w):
30  Vector4d (x, y, z, w)
31  {}
32  Quaternion operator* (const double &s) const {
33  return Quaternion (
34  (*this)[0] * s,
35  (*this)[1] * s,
36  (*this)[2] * s,
37  (*this)[3] * s
38  );
39  }
41  Quaternion operator* (const Quaternion &q) const {
42  return Quaternion (
43  (*this)[3] * q[0] + (*this)[0] * q[3] + (*this)[1] * q[2] - (*this)[2] * q[1],
44  (*this)[3] * q[1] + (*this)[1] * q[3] + (*this)[2] * q[0] - (*this)[0] * q[2],
45  (*this)[3] * q[2] + (*this)[2] * q[3] + (*this)[0] * q[1] - (*this)[1] * q[0],
46  (*this)[3] * q[3] - (*this)[0] * q[0] - (*this)[1] * q[1] - (*this)[2] * q[2]
47  );
48  }
50  set (
51  (*this)[3] * q[0] + (*this)[0] * q[3] + (*this)[1] * q[2] - (*this)[2] * q[1],
52  (*this)[3] * q[1] + (*this)[1] * q[3] + (*this)[2] * q[0] - (*this)[0] * q[2],
53  (*this)[3] * q[2] + (*this)[2] * q[3] + (*this)[0] * q[1] - (*this)[1] * q[0],
54  (*this)[3] * q[3] - (*this)[0] * q[0] - (*this)[1] * q[1] - (*this)[2] * q[2]
55  );
56  return *this;
57  }
58 
59  static Quaternion fromGLRotate (double angle, double x, double y, double z) {
60  double st = std::sin (angle * M_PI / 360.);
61  return Quaternion (
62  st * x,
63  st * y,
64  st * z,
65  std::cos (angle * M_PI / 360.)
66  );
67  }
68 
69  Quaternion slerp (double alpha, const Quaternion &quat) const {
70  // check whether one of the two has 0 length
71  double s = std::sqrt (squaredNorm() * quat.squaredNorm());
72 
73  // division by 0.f is unhealthy!
74  assert (s != 0.);
75 
76  double angle = acos (dot(quat) / s);
77  if (angle == 0. || std::isnan(angle)) {
78  return *this;
79  }
80  assert(!std::isnan(angle));
81 
82  double d = 1. / std::sin (angle);
83  double p0 = std::sin ((1. - alpha) * angle);
84  double p1 = std::sin (alpha * angle);
85 
86  if (dot (quat) < 0.) {
87  return Quaternion( ((*this) * p0 - quat * p1) * d);
88  }
89  return Quaternion( ((*this) * p0 + quat * p1) * d);
90  }
91 
92  static Quaternion fromAxisAngle (const Vector3d &axis, double angle_rad) {
93  double d = axis.norm();
94  double s2 = std::sin (angle_rad * 0.5) / d;
95  return Quaternion (
96  axis[0] * s2,
97  axis[1] * s2,
98  axis[2] * s2,
99  std::cos(angle_rad * 0.5)
100  );
101  }
102 
103  static Quaternion fromMatrix (const Matrix3d &mat) {
104  double w = std::sqrt (1. + mat(0,0) + mat(1,1) + mat(2,2)) * 0.5;
105  return Quaternion (
106  (mat(1,2) - mat(2,1)) / (w * 4.),
107  (mat(2,0) - mat(0,2)) / (w * 4.),
108  (mat(0,1) - mat(1,0)) / (w * 4.),
109  w);
110  }
111 
112  static Quaternion fromZYXAngles (const Vector3d &zyx_angles) {
113  return Quaternion::fromAxisAngle (Vector3d (0., 0., 1.), zyx_angles[0])
114  * Quaternion::fromAxisAngle (Vector3d (0., 1., 0.), zyx_angles[1])
115  * Quaternion::fromAxisAngle (Vector3d (1., 0., 0.), zyx_angles[2]);
116  }
117 
118  static Quaternion fromYXZAngles (const Vector3d &yxz_angles) {
119  return Quaternion::fromAxisAngle (Vector3d (0., 1., 0.), yxz_angles[0])
120  * Quaternion::fromAxisAngle (Vector3d (1., 0., 0.), yxz_angles[1])
121  * Quaternion::fromAxisAngle (Vector3d (0., 0., 1.), yxz_angles[2]);
122  }
123 
124  static Quaternion fromXYZAngles (const Vector3d &xyz_angles) {
125  return Quaternion::fromAxisAngle (Vector3d (0., 0., 01.), xyz_angles[2])
126  * Quaternion::fromAxisAngle (Vector3d (0., 1., 0.), xyz_angles[1])
127  * Quaternion::fromAxisAngle (Vector3d (1., 0., 0.), xyz_angles[0]);
128  }
129 
130  Matrix3d toMatrix() const {
131  double x = (*this)[0];
132  double y = (*this)[1];
133  double z = (*this)[2];
134  double w = (*this)[3];
135  return Matrix3d (
136  1 - 2*y*y - 2*z*z,
137  2*x*y + 2*w*z,
138  2*x*z - 2*w*y,
139 
140  2*x*y - 2*w*z,
141  1 - 2*x*x - 2*z*z,
142  2*y*z + 2*w*x,
143 
144  2*x*z + 2*w*y,
145  2*y*z - 2*w*x,
146  1 - 2*x*x - 2*y*y
147 
148  /*
149  1 - 2*y*y - 2*z*z,
150  2*x*y - 2*w*z,
151  2*x*z + 2*w*y,
152 
153  2*x*y + 2*w*z,
154  1 - 2*x*x - 2*z*z,
155  2*y*z - 2*w*x,
156 
157  2*x*z - 2*w*y,
158  2*y*z + 2*w*x,
159  1 - 2*x*x - 2*y*y
160  */
161  );
162  }
163 
165  return Quaternion (
166  -(*this)[0],
167  -(*this)[1],
168  -(*this)[2],
169  (*this)[3]);
170  }
171 
172  Quaternion timeStep (const Vector3d &omega, double dt) {
173  double omega_norm = omega.norm();
174  return Quaternion::fromAxisAngle (omega / omega_norm, dt * omega_norm) * (*this);
175  }
176 
177  Vector3d rotate (const Vector3d &vec) const {
178  Vector3d vn (vec);
179  Quaternion vec_quat (vn[0], vn[1], vn[2], 0.f), res_quat;
180 
181  res_quat = vec_quat * (*this);
182  res_quat = conjugate() * res_quat;
183 
184  return Vector3d (res_quat[0], res_quat[1], res_quat[2]);
185  }
186 
196  Vector4d omegaToQDot(const Vector3d& omega) const {
197  Math::Matrix43 m;
198  m(0, 0) = (*this)[3]; m(0, 1) = -(*this)[2]; m(0, 2) = (*this)[1];
199  m(1, 0) = (*this)[2]; m(1, 1) = (*this)[3]; m(1, 2) = -(*this)[0];
200  m(2, 0) = -(*this)[1]; m(2, 1) = (*this)[0]; m(2, 2) = (*this)[3];
201  m(3, 0) = -(*this)[0]; m(3, 1) = -(*this)[1]; m(3, 2) = -(*this)[2];
202  return Quaternion(0.5 * m * omega);
203  }
204 };
205 
206 }
207 
208 }
209 
210 /* RBDL_QUATERNION_H */
211 #endif
Vector4d omegaToQDot(const Vector3d &omega) const
Converts a 3d angular velocity vector into a 4d derivative of the components of the quaternion...
Definition: Quaternion.h:196
Quaternion that are used for singularity free joints.
Definition: Quaternion.h:21
Quaternion operator*(const double &s) const
Definition: Quaternion.h:32
static Quaternion fromYXZAngles(const Vector3d &yxz_angles)
Definition: Quaternion.h:118
static Quaternion fromAxisAngle(const Vector3d &axis, double angle_rad)
Definition: Quaternion.h:92
Namespace for all structures of the RigidBodyDynamics library.
Definition: Constraints.cc:23
static Quaternion fromZYXAngles(const Vector3d &zyx_angles)
Definition: Quaternion.h:112
Quaternion timeStep(const Vector3d &omega, double dt)
Definition: Quaternion.h:172
Quaternion(const Vector4d &vec4)
Definition: Quaternion.h:26
Quaternion(double x, double y, double z, double w)
Definition: Quaternion.h:29
Quaternion slerp(double alpha, const Quaternion &quat) const
Definition: Quaternion.h:69
Vector3d rotate(const Vector3d &vec) const
Definition: Quaternion.h:177
Quaternion & operator*=(const Quaternion &q)
Definition: Quaternion.h:49
static Quaternion fromMatrix(const Matrix3d &mat)
Definition: Quaternion.h:103
static Quaternion fromGLRotate(double angle, double x, double y, double z)
Definition: Quaternion.h:59
static Quaternion fromXYZAngles(const Vector3d &xyz_angles)
Definition: Quaternion.h:124