1 // =============================================================================
2 // PROJECT CHRONO - http://projectchrono.org
3 //
4 // Copyright (c) 2014 projectchrono.org
5 // All rights reserved.
6 //
7 // Use of this source code is governed by a BSD-style license that can be found
8 // in the LICENSE file at the top level of the distribution and at
9 // http://projectchrono.org/license-chrono.txt.
10 //
11 // =============================================================================
12 // Authors: Alessandro Tasora, Radu Serban
13 // =============================================================================
14
15 #include <cmath>
16
17 #include "chrono/core/ChQuaternion.h"
18 #include "chrono/core/ChMatrix33.h"
19
20 namespace chrono {
21
22 ChApi const ChQuaternion<double> QNULL(0., 0., 0., 0.);
23 ChApi const ChQuaternion<double> QUNIT(1., 0., 0., 0.);
24
25 ChApi const ChQuaternion<double> Q_ROTATE_Y_TO_X(CH_C_SQRT_1_2, 0, 0, -CH_C_SQRT_1_2 );
26 ChApi const ChQuaternion<double> Q_ROTATE_Y_TO_Z(CH_C_SQRT_1_2, CH_C_SQRT_1_2, 0, 0 );
27 ChApi const ChQuaternion<double> Q_ROTATE_X_TO_Y(CH_C_SQRT_1_2, 0, 0, CH_C_SQRT_1_2 );
28 ChApi const ChQuaternion<double> Q_ROTATE_X_TO_Z(CH_C_SQRT_1_2, 0, -CH_C_SQRT_1_2, 0 );
29 ChApi const ChQuaternion<double> Q_ROTATE_Z_TO_Y(CH_C_SQRT_1_2,-CH_C_SQRT_1_2, 0, 0 );
30 ChApi const ChQuaternion<double> Q_ROTATE_Z_TO_X(CH_C_SQRT_1_2, 0, CH_C_SQRT_1_2, 0 );
31
32 ChApi const ChQuaternion<double> Q_FLIP_AROUND_X(0., 1., 0., 0.);
33 ChApi const ChQuaternion<double> Q_FLIP_AROUND_Y(0., 0., 1., 0.);
34 ChApi const ChQuaternion<double> Q_FLIP_AROUND_Z(0., 0., 0., 1.);
35
36 // -----------------------------------------------------------------------------
37 // QUATERNION OPERATIONS
38
Qlength(const ChQuaternion<double> & q)39 double Qlength(const ChQuaternion<double>& q) {
40 return (sqrt(pow(q.e0(), 2) + pow(q.e1(), 2) + pow(q.e2(), 2) + pow(q.e3(), 2)));
41 }
42
Qscale(const ChQuaternion<double> & q,double fact)43 ChQuaternion<double> Qscale(const ChQuaternion<double>& q, double fact) {
44 ChQuaternion<double> result;
45 result.e0() = q.e0() * fact;
46 result.e1() = q.e1() * fact;
47 result.e2() = q.e2() * fact;
48 result.e3() = q.e3() * fact;
49 return result;
50 }
51
Qadd(const ChQuaternion<double> & qa,const ChQuaternion<double> & qb)52 ChQuaternion<double> Qadd(const ChQuaternion<double>& qa, const ChQuaternion<double>& qb) {
53 ChQuaternion<double> result;
54 result.e0() = qa.e0() + qb.e0();
55 result.e1() = qa.e1() + qb.e1();
56 result.e2() = qa.e2() + qb.e2();
57 result.e3() = qa.e3() + qb.e3();
58 return result;
59 }
60
Qsub(const ChQuaternion<double> & qa,const ChQuaternion<double> & qb)61 ChQuaternion<double> Qsub(const ChQuaternion<double>& qa, const ChQuaternion<double>& qb) {
62 ChQuaternion<double> result;
63 result.e0() = qa.e0() - qb.e0();
64 result.e1() = qa.e1() - qb.e1();
65 result.e2() = qa.e2() - qb.e2();
66 result.e3() = qa.e3() - qb.e3();
67 return result;
68 }
69
70 // Return the norm two of the quaternion. Euler's parameters have norm = 1
Qnorm(const ChQuaternion<double> & q)71 ChQuaternion<double> Qnorm(const ChQuaternion<double>& q) {
72 double invlength;
73 invlength = 1 / (Qlength(q));
74 return Qscale(q, invlength);
75 }
76
77 // Return the conjugate of the quaternion [s,v1,v2,v3] is [s,-v1,-v2,-v3]
Qconjugate(const ChQuaternion<double> & q)78 ChQuaternion<double> Qconjugate(const ChQuaternion<double>& q) {
79 ChQuaternion<double> res;
80 res.e0() = q.e0();
81 res.e1() = -q.e1();
82 res.e2() = -q.e2();
83 res.e3() = -q.e3();
84 return (res);
85 }
86
87 // Return the product of two quaternions. It is non-commutative (like cross product in vectors).
Qcross(const ChQuaternion<double> & qa,const ChQuaternion<double> & qb)88 ChQuaternion<double> Qcross(const ChQuaternion<double>& qa, const ChQuaternion<double>& qb) {
89 ChQuaternion<double> res;
90 res.e0() = qa.e0() * qb.e0() - qa.e1() * qb.e1() - qa.e2() * qb.e2() - qa.e3() * qb.e3();
91 res.e1() = qa.e0() * qb.e1() + qa.e1() * qb.e0() - qa.e3() * qb.e2() + qa.e2() * qb.e3();
92 res.e2() = qa.e0() * qb.e2() + qa.e2() * qb.e0() + qa.e3() * qb.e1() - qa.e1() * qb.e3();
93 res.e3() = qa.e0() * qb.e3() + qa.e3() * qb.e0() - qa.e2() * qb.e1() + qa.e1() * qb.e2();
94 return (res);
95 }
96
97 // Get the quaternion from an angle of rotation and an axis, defined in _abs_ coords.
98 // The axis is supposed to be fixed, i.e. it is constant during rotation.
99 // The 'axis' vector must be normalized.
Q_from_AngAxis(double angle,const ChVector<double> & axis)100 ChQuaternion<double> Q_from_AngAxis(double angle, const ChVector<double>& axis) {
101 ChQuaternion<double> quat;
102 double halfang;
103 double sinhalf;
104
105 halfang = (angle * 0.5);
106 sinhalf = sin(halfang);
107
108 quat.e0() = cos(halfang);
109 quat.e1() = axis.x() * sinhalf;
110 quat.e2() = axis.y() * sinhalf;
111 quat.e3() = axis.z() * sinhalf;
112 return (quat);
113 }
114
115 // Get the quaternion from a source vector and a destination vector which specifies
116 // the rotation from one to the other. The vectors do not need to be normalized.
Q_from_Vect_to_Vect(const ChVector<double> & fr_vect,const ChVector<double> & to_vect)117 ChQuaternion<double> Q_from_Vect_to_Vect(const ChVector<double>& fr_vect, const ChVector<double>& to_vect) {
118 const double ANGLE_TOLERANCE = 1e-6;
119 ChQuaternion<double> quat;
120 double halfang;
121 double sinhalf;
122 ChVector<double> axis;
123
124 double lenXlen = fr_vect.Length() * to_vect.Length();
125 axis = fr_vect % to_vect;
126 double sinangle = ChClamp(axis.Length() / lenXlen, -1.0, +1.0);
127 double cosangle = ChClamp(fr_vect ^ to_vect / lenXlen, -1.0, +1.0);
128
129 // Consider three cases: Parallel, Opposite, non-collinear
130 if (std::abs(sinangle) == 0.0 && cosangle > 0) {
131 // fr_vect & to_vect are parallel
132 quat.e0() = 1.0;
133 quat.e1() = 0.0;
134 quat.e2() = 0.0;
135 quat.e3() = 0.0;
136 } else if (std::abs(sinangle) < ANGLE_TOLERANCE && cosangle < 0) {
137 // fr_vect & to_vect are opposite, i.e. ~180 deg apart
138 axis = fr_vect.GetOrthogonalVector() + (-to_vect).GetOrthogonalVector();
139 axis.Normalize();
140 quat.e0() = 0.0;
141 quat.e1() = ChClamp(axis.x(), -1.0, +1.0);
142 quat.e2() = ChClamp(axis.y(), -1.0, +1.0);
143 quat.e3() = ChClamp(axis.z(), -1.0, +1.0);
144 } else {
145 // fr_vect & to_vect are not co-linear case
146 axis.Normalize();
147 halfang = 0.5 * ChAtan2(cosangle, sinangle);
148 sinhalf = sin(halfang);
149
150 quat.e0() = cos(halfang);
151 quat.e1() = sinhalf* axis.x();
152 quat.e2() = sinhalf* axis.y();
153 quat.e3() = sinhalf* axis.z();
154 }
155 return (quat);
156 }
157
Q_from_AngZ(double angleZ)158 ChQuaternion<double> Q_from_AngZ(double angleZ) {
159 return Q_from_AngAxis(angleZ, VECT_Z);
160 }
Q_from_AngX(double angleX)161 ChQuaternion<double> Q_from_AngX(double angleX) {
162 return Q_from_AngAxis(angleX, VECT_X);
163 }
Q_from_AngY(double angleY)164 ChQuaternion<double> Q_from_AngY(double angleY) {
165 return Q_from_AngAxis(angleY, VECT_Y);
166 }
167
Q_from_NasaAngles(const ChVector<double> & mang)168 ChQuaternion<double> Q_from_NasaAngles(const ChVector<double>& mang) {
169 ChQuaternion<double> mq;
170 double c1 = cos(mang.z() / 2);
171 double s1 = sin(mang.z() / 2);
172 double c2 = cos(mang.x() / 2);
173 double s2 = sin(mang.x() / 2);
174 double c3 = cos(mang.y() / 2);
175 double s3 = sin(mang.y() / 2);
176 double c1c2 = c1 * c2;
177 double s1s2 = s1 * s2;
178 mq.e0() = c1c2 * c3 + s1s2 * s3;
179 mq.e1() = c1c2 * s3 - s1s2 * c3;
180 mq.e2() = c1 * s2 * c3 + s1 * c2 * s3;
181 mq.e3() = s1 * c2 * c3 - c1 * s2 * s3;
182 return mq;
183 }
184
Q_to_NasaAngles(const ChQuaternion<double> & q1)185 ChVector<double> Q_to_NasaAngles(const ChQuaternion<double>& q1) {
186 ChVector<double> mnasa;
187 double sqw = q1.e0() * q1.e0();
188 double sqx = q1.e1() * q1.e1();
189 double sqy = q1.e2() * q1.e2();
190 double sqz = q1.e3() * q1.e3();
191 // heading
192 mnasa.z() = atan2(2.0 * (q1.e1() * q1.e2() + q1.e3() * q1.e0()), (sqx - sqy - sqz + sqw));
193 // bank
194 mnasa.y() = atan2(2.0 * (q1.e2() * q1.e3() + q1.e1() * q1.e0()), (-sqx - sqy + sqz + sqw));
195 // attitude
196 mnasa.x() = asin(-2.0 * (q1.e1() * q1.e3() - q1.e2() * q1.e0()));
197 return mnasa;
198 }
199
Q_from_Euler123(const ChVector<double> & ang)200 ChQuaternion<double> Q_from_Euler123(const ChVector<double>& ang) {
201 ChQuaternion<double> q;
202 double t0 = cos(ang.z() * 0.5);
203 double t1 = sin(ang.z() * 0.5);
204 double t2 = cos(ang.x() * 0.5);
205 double t3 = sin(ang.x() * 0.5);
206 double t4 = cos(ang.y() * 0.5);
207 double t5 = sin(ang.y() * 0.5);
208
209 q.e0() = t0 * t2 * t4 + t1 * t3 * t5;
210 q.e1() = t0 * t3 * t4 - t1 * t2 * t5;
211 q.e2() = t0 * t2 * t5 + t1 * t3 * t4;
212 q.e3() = t1 * t2 * t4 - t0 * t3 * t5;
213
214 return q;
215 }
216
Q_to_Euler123(const ChQuaternion<double> & mq)217 ChVector<double> Q_to_Euler123(const ChQuaternion<double>& mq) {
218 ChVector<double> euler;
219 double sq0 = mq.e0() * mq.e0();
220 double sq1 = mq.e1() * mq.e1();
221 double sq2 = mq.e2() * mq.e2();
222 double sq3 = mq.e3() * mq.e3();
223 // roll
224 euler.x() = atan2(2 * (mq.e2() * mq.e3() + mq.e0() * mq.e1()), sq3 - sq2 - sq1 + sq0);
225 // pitch
226 euler.y() = -asin(2 * (mq.e1() * mq.e3() - mq.e0() * mq.e2()));
227 // yaw
228 euler.z() = atan2(2 * (mq.e1() * mq.e2() + mq.e3() * mq.e0()), sq1 + sq0 - sq3 - sq2);
229
230 return euler;
231 }
232
Q_to_AngAxis(const ChQuaternion<double> & quat,double & angle,ChVector<double> & axis)233 void Q_to_AngAxis(const ChQuaternion<double>& quat, double& angle, ChVector<double>& axis) {
234 if (std::abs(quat.e0()) < 0.99999999) {
235 double arg = acos(quat.e0());
236 double invsine = 1 / sin(arg);
237 ChVector<double> vtemp;
238 vtemp.x() = invsine * quat.e1();
239 vtemp.y() = invsine * quat.e2();
240 vtemp.z() = invsine * quat.e3();
241 angle = 2 * arg;
242 axis = Vnorm(vtemp);
243 } else {
244 axis.x() = 1;
245 axis.y() = 0;
246 axis.z() = 0;
247 angle = 0;
248 }
249 }
250
251 // Get the quaternion time derivative from the vector of angular speed, with w specified in _absolute_ coords.
Qdt_from_Wabs(const ChVector<double> & w,const ChQuaternion<double> & q)252 ChQuaternion<double> Qdt_from_Wabs(const ChVector<double>& w, const ChQuaternion<double>& q) {
253 ChQuaternion<double> qw;
254 double half = 0.5;
255
256 qw.e0() = 0;
257 qw.e1() = w.x();
258 qw.e2() = w.y();
259 qw.e3() = w.z();
260
261 return Qscale(Qcross(qw, q), half); // {q_dt} = 1/2 {0,w}*{q}
262 }
263
264 // Get the quaternion time derivative from the vector of angular speed, with w specified in _local_ coords.
Qdt_from_Wrel(const ChVector<double> & w,const ChQuaternion<double> & q)265 ChQuaternion<double> Qdt_from_Wrel(const ChVector<double>& w, const ChQuaternion<double>& q) {
266 ChQuaternion<double> qw;
267 double half = 0.5;
268
269 qw.e0() = 0;
270 qw.e1() = w.x();
271 qw.e2() = w.y();
272 qw.e3() = w.z();
273
274 return Qscale(Qcross(q, qw), half); // {q_dt} = 1/2 {q}*{0,w_rel}
275 }
276
277 // Get the quaternion first derivative from the vector of angular acceleration with a specified in _absolute_ coords.
Qdtdt_from_Aabs(const ChVector<double> & a,const ChQuaternion<double> & q,const ChQuaternion<double> & q_dt)278 ChQuaternion<double> Qdtdt_from_Aabs(const ChVector<double>& a,
279 const ChQuaternion<double>& q,
280 const ChQuaternion<double>& q_dt) {
281 ChQuaternion<double> ret;
282 ret.Qdtdt_from_Aabs(a, q, q_dt);
283 return ret;
284 }
285
286 // Get the quaternion second derivative from the vector of angular acceleration with a specified in _relative_ coords.
Qdtdt_from_Arel(const ChVector<double> & a,const ChQuaternion<double> & q,const ChQuaternion<double> & q_dt)287 ChQuaternion<double> Qdtdt_from_Arel(const ChVector<double>& a,
288 const ChQuaternion<double>& q,
289 const ChQuaternion<double>& q_dt) {
290 ChQuaternion<double> ret;
291 ret.Qdtdt_from_Arel(a, q, q_dt);
292 return ret;
293 }
294
295 // Get the time derivative from a quaternion, a speed of rotation and an axis, defined in _abs_ coords.
Qdt_from_AngAxis(const ChQuaternion<double> & quat,double angle_dt,const ChVector<double> & axis)296 ChQuaternion<double> Qdt_from_AngAxis(const ChQuaternion<double>& quat, double angle_dt, const ChVector<double>& axis) {
297 ChVector<double> W;
298
299 W = Vmul(axis, angle_dt);
300
301 return Qdt_from_Wabs(W, quat);
302 }
303
304 // Get the second time derivative from a quaternion, an angular acceleration and an axis, defined in _abs_ coords.
Qdtdt_from_AngAxis(double angle_dtdt,const ChVector<double> & axis,const ChQuaternion<double> & q,const ChQuaternion<double> & q_dt)305 ChQuaternion<double> Qdtdt_from_AngAxis(double angle_dtdt,
306 const ChVector<double>& axis,
307 const ChQuaternion<double>& q,
308 const ChQuaternion<double>& q_dt) {
309 ChVector<double> Acc;
310
311 Acc = Vmul(axis, angle_dtdt);
312
313 return Qdtdt_from_Aabs(Acc, q, q_dt);
314 }
315
316 // Check if two quaternions are equal
Qequal(const ChQuaternion<double> & qa,const ChQuaternion<double> & qb)317 bool Qequal(const ChQuaternion<double>& qa, const ChQuaternion<double>& qb) {
318 return qa == qb;
319 }
320
321 // Check if quaternion is not null
Qnotnull(const ChQuaternion<double> & qa)322 bool Qnotnull(const ChQuaternion<double>& qa) {
323 return (qa.e0() != 0) || (qa.e1() != 0) || (qa.e2() != 0) || (qa.e3() != 0);
324 }
325
326 // Given the imaginary (vectorial) {e1 e2 e3} part of a quaternion,
327 // find the entire quaternion q = {e0, e1, e2, e3}.
328 // Note: singularities are possible.
ImmQ_complete(const ChVector<double> & qimm)329 ChQuaternion<double> ImmQ_complete(const ChVector<double>& qimm) {
330 ChQuaternion<double> mq;
331 mq.e1() = qimm.x();
332 mq.e2() = qimm.y();
333 mq.e3() = qimm.z();
334 mq.e0() = sqrt(1 - mq.e1() * mq.e1() - mq.e2() * mq.e2() - mq.e3() * mq.e3());
335 return mq;
336 }
337
338 // Given the imaginary (vectorial) {e1 e2 e3} part of a quaternion time derivative,
339 // find the entire quaternion q = {e0, e1, e2, e3}.
340 // Note: singularities are possible.
ImmQ_dt_complete(const ChQuaternion<double> & mq,const ChVector<double> & qimm_dt)341 ChQuaternion<double> ImmQ_dt_complete(const ChQuaternion<double>& mq, const ChVector<double>& qimm_dt) {
342 ChQuaternion<double> mqdt;
343 mqdt.e1() = qimm_dt.x();
344 mqdt.e2() = qimm_dt.y();
345 mqdt.e3() = qimm_dt.z();
346 mqdt.e0() = (-mq.e1() * mqdt.e1() - mq.e2() * mqdt.e2() - mq.e3() * mqdt.e3()) / mq.e0();
347 return mqdt;
348 }
349
350 // Given the imaginary (vectorial) {e1 e2 e3} part of a quaternion second time derivative,
351 // find the entire quaternion q = {e0, e1, e2, e3}.
352 // Note: singularities are possible.
ImmQ_dtdt_complete(const ChQuaternion<double> & mq,const ChQuaternion<double> & mqdt,const ChVector<double> & qimm_dtdt)353 ChQuaternion<double> ImmQ_dtdt_complete(const ChQuaternion<double>& mq,
354 const ChQuaternion<double>& mqdt,
355 const ChVector<double>& qimm_dtdt) {
356 ChQuaternion<double> mqdtdt;
357 mqdtdt.e1() = qimm_dtdt.x();
358 mqdtdt.e2() = qimm_dtdt.y();
359 mqdtdt.e3() = qimm_dtdt.z();
360 mqdtdt.e0() = (-mq.e1() * mqdtdt.e1() - mq.e2() * mqdtdt.e2() - mq.e3() * mqdtdt.e3() - mqdt.e0() * mqdt.e0() -
361 mqdt.e1() * mqdt.e1() - mqdt.e2() * mqdt.e2() - mqdt.e3() * mqdt.e3()) /
362 mq.e0();
363 return mqdtdt;
364 }
365
366 // -----------------------------------------------------------------------------
367 // ANGLE CONVERSION UTILITIES
368
Angle_to_Quat(AngleSet angset,const ChVector<double> & mangles)369 ChQuaternion<double> Angle_to_Quat(AngleSet angset, const ChVector<double>& mangles) {
370 ChQuaternion<double> res;
371 ChMatrix33<> Acoord;
372
373 switch (angset) {
374 case AngleSet::EULERO:
375 Acoord.Set_A_Eulero(mangles);
376 break;
377 case AngleSet::CARDANO:
378 Acoord.Set_A_Cardano(mangles);
379 break;
380 case AngleSet::HPB:
381 Acoord.Set_A_Hpb(mangles);
382 break;
383 case AngleSet::RXYZ:
384 Acoord.Set_A_Rxyz(mangles);
385 break;
386 case AngleSet::RODRIGUEZ:
387 Acoord.Set_A_Rodriguez(mangles);
388 break;
389 default:
390 break;
391 }
392 res = Acoord.Get_A_quaternion();
393 return res;
394 }
395
Quat_to_Angle(AngleSet angset,const ChQuaternion<double> & mquat)396 ChVector<double> Quat_to_Angle(AngleSet angset, const ChQuaternion<double>& mquat) {
397 ChVector<double> res;
398 ChMatrix33<> Acoord(mquat);
399
400 switch (angset) {
401 case AngleSet::EULERO:
402 res = Acoord.Get_A_Eulero();
403 break;
404 case AngleSet::CARDANO:
405 res = Acoord.Get_A_Cardano();
406 break;
407 case AngleSet::HPB:
408 res = Acoord.Get_A_Hpb();
409 break;
410 case AngleSet::RXYZ:
411 res = Acoord.Get_A_Rxyz();
412 break;
413 case AngleSet::RODRIGUEZ:
414 res = Acoord.Get_A_Rodriguez();
415 break;
416 default:
417 break;
418 }
419 return res;
420 }
421
AngleDT_to_QuatDT(AngleSet angset,const ChVector<double> & mangles,const ChQuaternion<double> & q)422 ChQuaternion<double> AngleDT_to_QuatDT(AngleSet angset,
423 const ChVector<double>& mangles,
424 const ChQuaternion<double>& q) {
425 ChQuaternion<double> res;
426 ChQuaternion<double> q2;
427 ChVector<double> ang1, ang2;
428
429 ang1 = Quat_to_Angle(angset, q);
430 ang2 = Vadd(ang1, Vmul(mangles, BDF_STEP_HIGH));
431 q2 = Angle_to_Quat(angset, ang2);
432 res = Qscale(Qsub(q2, q), 1 / BDF_STEP_HIGH);
433
434 return res;
435 }
436
AngleDTDT_to_QuatDTDT(AngleSet angset,const ChVector<double> & mangles,const ChQuaternion<double> & q)437 ChQuaternion<double> AngleDTDT_to_QuatDTDT(AngleSet angset,
438 const ChVector<double>& mangles,
439 const ChQuaternion<double>& q) {
440 ChQuaternion<double> res;
441 ChQuaternion<double> qa, qb;
442 ChVector<double> ang0, angA, angB;
443
444 ang0 = Quat_to_Angle(angset, q);
445 angA = Vsub(ang0, Vmul(mangles, BDF_STEP_HIGH));
446 angB = Vadd(ang0, Vmul(mangles, BDF_STEP_HIGH));
447 qa = Angle_to_Quat(angset, angA);
448 qb = Angle_to_Quat(angset, angB);
449 res = Qscale(Qadd(Qadd(qa, qb), Qscale(q, -2)), 1 / BDF_STEP_HIGH);
450
451 return res;
452 }
453
Angle_to_Angle(AngleSet setfrom,AngleSet setto,const ChVector<double> & mangles)454 ChVector<double> Angle_to_Angle(AngleSet setfrom, AngleSet setto, const ChVector<double>& mangles) {
455 ChVector<double> res;
456 ChMatrix33<> Acoord;
457
458 switch (setfrom) {
459 case AngleSet::EULERO:
460 Acoord.Set_A_Eulero(mangles);
461 break;
462 case AngleSet::CARDANO:
463 Acoord.Set_A_Cardano(mangles);
464 break;
465 case AngleSet::HPB:
466 Acoord.Set_A_Hpb(mangles);
467 break;
468 case AngleSet::RXYZ:
469 Acoord.Set_A_Rxyz(mangles);
470 break;
471 case AngleSet::RODRIGUEZ:
472 Acoord.Set_A_Rodriguez(mangles);
473 break;
474 default:
475 break;
476 }
477
478 switch (setto) {
479 case AngleSet::EULERO:
480 res = Acoord.Get_A_Eulero();
481 break;
482 case AngleSet::CARDANO:
483 res = Acoord.Get_A_Cardano();
484 break;
485 case AngleSet::HPB:
486 res = Acoord.Get_A_Hpb();
487 break;
488 case AngleSet::RXYZ:
489 res = Acoord.Get_A_Rxyz();
490 break;
491 case AngleSet::RODRIGUEZ:
492 res = Acoord.Get_A_Rodriguez();
493 break;
494 default:
495 break;
496 }
497 return res;
498 }
499
500 // Get the X axis of a coordsystem, given the quaternion which
501 // represents the alignment of the coordsystem.
VaxisXfromQuat(const ChQuaternion<double> & quat)502 ChVector<double> VaxisXfromQuat(const ChQuaternion<double>& quat) {
503 ChVector<double> res;
504 res.x() = (pow(quat.e0(), 2) + pow(quat.e1(), 2)) * 2 - 1;
505 res.y() = ((quat.e1() * quat.e2()) + (quat.e0() * quat.e3())) * 2;
506 res.z() = ((quat.e1() * quat.e3()) - (quat.e0() * quat.e2())) * 2;
507 return res;
508 }
509
510 } // end namespace chrono
511