Logo ROOT  
Reference Guide
 
Loading...
Searching...
No Matches
TQuaternion.cxx
Go to the documentation of this file.
1// @(#)root/physics:$Id$
2// Author: Eric Anciant 28/06/2005
3
4
5/** \class TQuaternion
6 \ingroup Physics
7 Quaternion is a 4-component mathematic object quite convenient when dealing with
8space rotation (or reference frame transformation).
9
10 In short, think of quaternion Q as a 3-vector augmented by a real number.
11 \f$ Q = Q|_r + Q|_V \f$
12
13 #### Quaternion multiplication :
14
15 Quaternion multiplication is given by :
16 \f[
17 Q.Q' = (Q|_r + Q|_V )*( Q'|_r + Q'|_V) = [ Q|_r*Q'|_r - Q|_V*Q'|_V ] + [ Q|_r*Q'|_V + Q'|_r*Q|_V + Q|_V X Q'|_V ]
18\f]
19
20 where :
21 - \f$ Q|_r*Q'|_r \f$ is a real number product of real numbers
22 - \f$ Q|_V*Q'|_V \f$ is a real number, scalar product of two 3-vectors
23 - \f$ Q|_r*Q'|_V \f$ is a 3-vector, scaling of a 3-vector by a real number
24 - \f$ Q|_VXQ'|_V \f$ is a 3-vector, cross product of two 3-vectors
25
26Thus, quaternion product is a generalization of real number product and product of a
27vector by a real number. Product of two pure vectors gives a quaternion whose real part
28is the opposite of scalar product and the vector part the cross product.
29
30The conjugate of a quaternion \f$ Q = Q|r + Q|V \f$ is \f$ \bar{Q} = Q|r - Q|V \f$
31
32The magnitude of a quaternion \f$ Q \f$ is given by \f$ |Q|^2 = Q.\bar{Q} = \bar{Q}.Q = Q^2|r + |Q|V|^2 \f$
33
34Therefore, the inverse of a quaternion is \f$ Q-1 = \bar{Q} /|Q|^2 \f$
35
36"unit" quaternion is a quaternion of magnitude 1 : \f$ |Q|^2 = 1. \f$
37Unit quaternions are a subset of the quaternions set.
38
39 #### Quaternion and rotations :
40
41
42 A rotation of angle \f$ f \f$ around a given axis, is represented by a unit quaternion Q :
43 - The axis of the rotation is given by the vector part of Q.
44 - The ratio between the magnitude of the vector part and the real part of Q equals tan(\frac{f}{2}).
45
46 In other words : \f$ Q = Q|_r + Q|_V = cos(\frac{f}{2}) + sin(\frac{f}{2}) \f$.
47 (where u is a unit vector // to the rotation axis,
48\f$ cos(\frac{f}{2}) \f$ is the real part, \f$ sin(\frac{f}{2}) \f$ .u is the vector part)
49 Note : The quaternion of identity is \f$ Q_I = cos(0) + sin(0)*(AnyVector) = 1\f$ .
50
51 The composition of two rotations is described by the product of the two corresponding quaternions.
52 As for 3-space rotations, quaternion multiplication is not commutative !
53
54 \f$ Q = Q_1.Q_2 \f$ represents the composition of the successive rotation R1 and R2 expressed in the current frame (the axis of rotation hold by \f$ Q_2 \f$ is expressed in the frame as it is after R1 rotation).
55 \f$ Q = Q_2.Q_1 \f$ represents the composition of the successive rotation R1 and R2 expressed in the initial reference frame.
56
57 The inverse of a rotation is a rotation about the same axis but of opposite angle, thus if Q is a unit quaternion,
58 \f$ Q = cos(\frac{f}{2}) + sin(\frac{f}{2}).u = Q|_r + Q|_V\f$ , then :
59 \f$ Q^{-1} =cos(-\frac{f}{2}) + sin(-\frac{f}{2}).u = cos(\frac{f}{2}) - sin(\frac{f}{2}).u = Q|_r -Q|_V \f$ is its inverse quaternion.
60
61 One verifies that :
62 \f$ Q.Q^{-1} = Q^{-1}.Q = Q|_r*Q|_r + Q|_V*Q|_V + Q|_r*Q|_V -Q|_r*Q|_V + Q|_VXQ|_V = Q\leq|_r + Q\leq|_V = 1 \f$
63
64
65 The rotation of a vector V by the rotation described by a unit quaternion Q is obtained by the following operation :
66 \f$ V' = Q*V*Q^{-1} \f$, considering V as a quaternion whose real part is null.
67
68 #### Numeric computation considerations :
69
70 Numerically, the quaternion multiplication involves 12 additions and 16 multiplications.
71 It is therefore faster than 3x3 matrixes multiplication involving 18 additions and 27 multiplications.
72
73 On the contrary, rotation of a vector by the above formula ( \f$ Q*V*Q^{-1} \f$ ) involves 18 additions
74 and 24 multiplications, whereas multiplication of a 3-vector by a 3x3 matrix involves only 6 additions
75 and 9 multiplications.
76
77 When dealing with numerous composition of space rotation, it is therefore faster to use quaternion product. On the other hand if a huge set of vectors must be rotated by a given quaternion, it is more optimized to convert the quaternion into a rotation matrix once, and then use that later to rotate the set of vectors.
78
79 #### More information :
80
81http://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation
82
83http://en.wikipedia.org/wiki/Quaternion
84
85 This Class represents all quaternions (unit or non-unit)
86 It possesses a Normalize() method to make a given quaternion unit
87 The Rotate(TVector3&) and Rotation(TVector3&) methods can be used even for a non-unit quaternion, in that case, the proper normalization is applied to perform the rotation.
88
89 A TRotation constructor exists than takes a quaternion for parameter (even non-unit), in that cas the proper normalisation is applied.
90*/
91
92#include "TMath.h"
93#include "TQuaternion.h"
94#include "TString.h"
95
97
98/****************** CONSTRUCTORS ****************************************************/
99////////////////////////////////////////////////////////////////////////////////
100
102 fRealPart(q.fRealPart), fVectorPart(q.fVectorPart) {}
103
105 : fRealPart(real), fVectorPart(vect) {}
106
108 : fRealPart(x0[3]), fVectorPart(x0) {}
109
111 : fRealPart(x0[3]), fVectorPart(x0) {}
112
114 : fRealPart(real), fVectorPart(X,Y,Z) {}
115
117
118////////////////////////////////////////////////////////////////////////////////
119///dereferencing operator const
120
122 switch(i) {
123 case 0:
124 case 1:
125 case 2:
126 return fVectorPart(i);
127 case 3:
128 return fRealPart;
129 default:
130 Error("operator()(i)", "bad index (%d) returning 0",i);
131 }
132 return 0.;
133}
134
135////////////////////////////////////////////////////////////////////////////////
136///dereferencing operator
137
139 switch(i) {
140 case 0:
141 case 1:
142 case 2:
143 return fVectorPart(i);
144 case 3:
145 return fRealPart;
146 default:
147 Error("operator()(i)", "bad index (%d) returning &fRealPart",i);
148 }
149 return fRealPart;
150}
151////////////////////////////////////////////////////////////////////////////////
152/// Get angle of quaternion (rad)
153/// N.B : this angle is half of the corresponding rotation angle
154
156 if (fRealPart == 0) return TMath::PiOver2();
157 Double_t denominator = fVectorPart.Mag();
158 return atan(denominator/fRealPart);
159}
160
161////////////////////////////////////////////////////////////////////////////////
162/// Set angle of quaternion (rad) - keep quaternion norm
163/// N.B : this angle is half of the corresponding rotation angle
164
166 Double_t norm = Norm();
167 Double_t normSinV = fVectorPart.Mag();
168 if (normSinV != 0) fVectorPart *= (sin(angle)*norm/normSinV);
169 fRealPart = cos(angle)*norm;
170
171 return (*this);
172}
173
174////////////////////////////////////////////////////////////////////////////////
175/// set quaternion from vector and angle (rad)
176/// quaternion is set as unitary
177/// N.B : this angle is half of the corresponding rotation angle
178
180 fVectorPart = v;
181 double norm = v.Mag();
182 if (norm>0) fVectorPart*=(1./norm);
183 fVectorPart*=sin(QAngle);
184 fRealPart = cos(QAngle);
185
186 return (*this);
187}
188
189/**************** REAL TO QUATERNION ALGEBRA ****************************************/
190
191////////////////////////////////////////////////////////////////////////////////
192/// sum of quaternion with a real
193
195 return TQuaternion(fVectorPart, fRealPart + real);
196}
197
198////////////////////////////////////////////////////////////////////////////////
199/// substraction of real to quaternion
200
202 return TQuaternion(fVectorPart, fRealPart - real);
203}
204
205////////////////////////////////////////////////////////////////////////////////
206/// product of quaternion with a real
207
209 return TQuaternion(fRealPart*real,fVectorPart.x()*real,fVectorPart.y()*real,fVectorPart.z()*real);
210}
211
212
213////////////////////////////////////////////////////////////////////////////////
214/// division by a real
215
217 if (real !=0 ) {
218 return TQuaternion(fRealPart/real,fVectorPart.x()/real,fVectorPart.y()/real,fVectorPart.z()/real);
219 } else {
220 Error("operator/(Double_t)", "bad value (%f) ignored",real);
221 }
222
223 return (*this);
224}
225
229TQuaternion operator / (Double_t r, const TQuaternion & q) { return (q.Invert()*r); }
230
231/**************** VECTOR TO QUATERNION ALGEBRA ****************************************/
232
233////////////////////////////////////////////////////////////////////////////////
234/// sum of quaternion with a real
235
237 return TQuaternion(fVectorPart + vect, fRealPart);
238}
239
240////////////////////////////////////////////////////////////////////////////////
241/// substraction of real to quaternion
242
244 return TQuaternion(fVectorPart - vect, fRealPart);
245}
246
247////////////////////////////////////////////////////////////////////////////////
248/// left multiplication
249
251 Double_t savedRealPart = fRealPart;
252 fRealPart = - (fVectorPart * vect);
254 fVectorPart += (vect * savedRealPart);
255
256 return (*this);
257}
258
259////////////////////////////////////////////////////////////////////////////////
260/// right multiplication
261
263 Double_t savedRealPart = fRealPart;
264 fRealPart = -(fVectorPart * vect);
266 fVectorPart += (vect * savedRealPart );
267
268 return (*this);
269}
270
271////////////////////////////////////////////////////////////////////////////////
272/// left product
273
275 return TQuaternion(vect * fRealPart + vect.Cross(fVectorPart), -(fVectorPart * vect));
276}
277
278////////////////////////////////////////////////////////////////////////////////
279/// right product
280
282 return TQuaternion(vect * fRealPart + fVectorPart.Cross(vect), -(fVectorPart * vect));
283}
284
285////////////////////////////////////////////////////////////////////////////////
286/// left division
287
289 Double_t norm2 = vect.Mag2();
290 MultiplyLeft(vect);
291 if (norm2 > 0 ) {
292 // use (1./nom2) to be numerically compliant with LeftQuotient(const TVector3 &)
293 (*this) *= -(1./norm2); // minus <- using conjugate of vect
294 } else {
295 Error("DivideLeft(const TVector3)", "bad norm2 (%f) ignored",norm2);
296 }
297 return (*this);
298}
299
300////////////////////////////////////////////////////////////////////////////////
301/// right division
302
304 Double_t norm2 = vect.Mag2();
305 (*this) *= vect;
306 if (norm2 > 0 ) {
307 // use (1./real) to be numerically compliant with operator/(const TVector3 &)
308 (*this) *= - (1./norm2); // minus <- using conjugate of vect
309 } else {
310 Error("operator/=(const TVector3 &)", "bad norm2 (%f) ignored",norm2);
311 }
312 return (*this);
313}
314
315////////////////////////////////////////////////////////////////////////////////
316/// left quotient
317
319 Double_t norm2 = vect.Mag2();
320
321 if (norm2>0) {
322 double invNorm2 = 1./norm2;
323 return TQuaternion((vect * -fRealPart - vect.Cross(fVectorPart))*invNorm2,
324 (fVectorPart * vect ) * invNorm2 );
325 } else {
326 Error("LeftQuotient(const TVector3 &)", "bad norm2 (%f) ignored",norm2);
327 }
328 return (*this);
329}
330
331////////////////////////////////////////////////////////////////////////////////
332/// right quotient
333
335 Double_t norm2 = vect.Mag2();
336
337 if (norm2>0) {
338 double invNorm2 = 1./norm2;
339 return TQuaternion((vect * -fRealPart - fVectorPart.Cross(vect)) * invNorm2,
340 (fVectorPart * vect) * invNorm2 );
341 } else {
342 Error("operator/(const TVector3 &)", "bad norm2 (%f) ignored",norm2);
343 }
344 return (*this);
345}
346
347TQuaternion operator + (const TVector3 &V, const TQuaternion &Q) { return (Q+V); }
348TQuaternion operator - (const TVector3 &V, const TQuaternion &Q) { return (-Q+V); }
349TQuaternion operator * (const TVector3 &V, const TQuaternion &Q) { return Q.LeftProduct(V); }
350
351TQuaternion operator / (const TVector3 &vect, const TQuaternion &quat) {
352 //divide operator
353 TQuaternion res(vect);
354 res /= quat;
355 return res;
356}
357
358/**************** QUATERNION ALGEBRA ****************************************/
359
360////////////////////////////////////////////////////////////////////////////////
361/// right multiplication
362
364 Double_t saveRP = fRealPart;
365 TVector3 cross(fVectorPart.Cross(quaternion.fVectorPart));
366
367 fRealPart = fRealPart * quaternion.fRealPart - fVectorPart * quaternion.fVectorPart;
368
369 fVectorPart *= quaternion.fRealPart;
370 fVectorPart += quaternion.fVectorPart * saveRP;
371 fVectorPart += cross;
372 return (*this);
373}
374
375////////////////////////////////////////////////////////////////////////////////
376/// left multiplication
377
379 Double_t saveRP = fRealPart;
380 TVector3 cross(quaternion.fVectorPart.Cross(fVectorPart));
381
382 fRealPart = fRealPart * quaternion.fRealPart - fVectorPart * quaternion.fVectorPart;
383
384 fVectorPart *= quaternion.fRealPart;
385 fVectorPart += quaternion.fVectorPart * saveRP;
386 fVectorPart += cross;
387
388 return (*this);
389}
390
391////////////////////////////////////////////////////////////////////////////////
392/// left product
393
395 return TQuaternion( fVectorPart*quaternion.fRealPart + quaternion.fVectorPart*fRealPart
396 + quaternion.fVectorPart.Cross(fVectorPart),
397 fRealPart*quaternion.fRealPart - fVectorPart*quaternion.fVectorPart );
398}
399
400////////////////////////////////////////////////////////////////////////////////
401/// right product
402
404 return TQuaternion(fVectorPart*quaternion.fRealPart + quaternion.fVectorPart*fRealPart
405 + fVectorPart.Cross(quaternion.fVectorPart),
406 fRealPart*quaternion.fRealPart - fVectorPart*quaternion.fVectorPart );
407}
408
409////////////////////////////////////////////////////////////////////////////////
410/// left division
411
413 Double_t norm2 = quaternion.Norm2();
414
415 if (norm2 > 0 ) {
416 MultiplyLeft(quaternion.Conjugate());
417 (*this) *= (1./norm2);
418 } else {
419 Error("DivideLeft(const TQuaternion &)", "bad norm2 (%f) ignored",norm2);
420 }
421 return (*this);
422}
423
424////////////////////////////////////////////////////////////////////////////////
425/// right division
426
428 Double_t norm2 = quaternion.Norm2();
429
430 if (norm2 > 0 ) {
431 (*this) *= quaternion.Conjugate();
432 // use (1./norm2) top be numerically compliant with operator/(const TQuaternion&)
433 (*this) *= (1./norm2);
434 } else {
435 Error("operator/=(const TQuaternion&)", "bad norm2 (%f) ignored",norm2);
436 }
437 return (*this);
438}
439
440////////////////////////////////////////////////////////////////////////////////
441/// left quotient
442
444 Double_t norm2 = quaternion.Norm2();
445
446 if (norm2 > 0 ) {
447 double invNorm2 = 1./norm2;
448 return TQuaternion(
449 (fVectorPart*quaternion.fRealPart - quaternion.fVectorPart*fRealPart
450 - quaternion.fVectorPart.Cross(fVectorPart)) * invNorm2,
451 (fRealPart*quaternion.fRealPart + fVectorPart*quaternion.fVectorPart)*invNorm2 );
452 } else {
453 Error("LeftQuotient(const TQuaternion&)", "bad norm2 (%f) ignored",norm2);
454 }
455 return (*this);
456}
457
458////////////////////////////////////////////////////////////////////////////////
459/// right quotient
460
462 Double_t norm2 = quaternion.Norm2();
463
464 if (norm2 > 0 ) {
465 double invNorm2 = 1./norm2;
466 return TQuaternion(
467 (fVectorPart*quaternion.fRealPart - quaternion.fVectorPart*fRealPart
468 - fVectorPart.Cross(quaternion.fVectorPart)) * invNorm2,
469 (fRealPart*quaternion.fRealPart + fVectorPart*quaternion.fVectorPart) * invNorm2 );
470 } else {
471 Error("operator/(const TQuaternion &)", "bad norm2 (%f) ignored",norm2);
472 }
473 return (*this);
474}
475
476////////////////////////////////////////////////////////////////////////////////
477/// invert
478
480 double norm2 = Norm2();
481 if (norm2 > 0 ) {
482 double invNorm2 = 1./norm2;
483 return TQuaternion(fVectorPart*(-invNorm2), fRealPart*invNorm2 );
484 } else {
485 Error("Invert()", "bad norm2 (%f) ignored",norm2);
486 }
487 return (*this);
488}
489
490////////////////////////////////////////////////////////////////////////////////
491/// rotate vect by current quaternion
492
493void TQuaternion::Rotate(TVector3 & vect) const {
494 vect = Rotation(vect);
495}
496
497////////////////////////////////////////////////////////////////////////////////
498/// rotation of vect by current quaternion
499
501 // Vres = (*this) * vect * (this->Invert());
502 double norm2 = Norm2();
503
504 if (norm2>0) {
505 TQuaternion quat(*this);
506 quat *= vect;
507
508 // only compute vect part : (real part has to be 0 ) :
509 // VECT [ quat * ( this->Conjugate() ) ] = quat.fRealPart * -this->fVectorPart
510 // + this->fRealPart * quat.fVectorPart
511 // + quat.fVectorPart X (-this->fVectorPart)
513 quat.fVectorPart *= fRealPart;
514 quat.fVectorPart -= fVectorPart * quat.fRealPart;
515 quat.fVectorPart += cross;
516
517 return quat.fVectorPart*(1./norm2);
518 } else {
519 Error("Rotation()", "bad norm2 (%f) ignored",norm2);
520 }
521 return vect;
522}
523
524////////////////////////////////////////////////////////////////////////////////
525///Print Quaternion parameters
526
528{
529 Printf("%s %s (r,x,y,z)=(%f,%f,%f,%f) \n (alpha,rho,theta,phi)=(%f,%f,%f,%f)",GetName(),GetTitle(),
532}
ROOT::R::TRInterface & r
Definition Object.C:4
double Double_t
Definition RtypesCore.h:59
float Float_t
Definition RtypesCore.h:57
const char Option_t
Definition RtypesCore.h:66
#define ClassImp(name)
Definition Rtypes.h:364
float * q
TQuaternion operator*(Double_t r, const TQuaternion &q)
TQuaternion operator+(Double_t r, const TQuaternion &q)
TQuaternion operator-(Double_t r, const TQuaternion &q)
TQuaternion operator/(Double_t r, const TQuaternion &q)
void Printf(const char *fmt,...)
Mother of all ROOT objects.
Definition TObject.h:41
virtual const char * GetName() const
Returns name of object.
Definition TObject.cxx:429
virtual void Error(const char *method, const char *msgfmt,...) const
Issue error message.
Definition TObject.cxx:963
virtual const char * GetTitle() const
Returns title of object.
Definition TObject.cxx:473
Quaternion is a 4-component mathematic object quite convenient when dealing with space rotation (or r...
Definition TQuaternion.h:11
TQuaternion & MultiplyLeft(const TVector3 &vector)
left multiplication
TQuaternion LeftQuotient(const TVector3 &vector) const
left quotient
TQuaternion operator*(Double_t real) const
product of quaternion with a real
TQuaternion(Double_t real=0, Double_t X=0, Double_t Y=0, Double_t Z=0)
virtual ~TQuaternion()
void Rotate(TVector3 &vect) const
rotate vect by current quaternion
Double_t operator()(int) const
dereferencing operator const
TQuaternion & DivideLeft(const TVector3 &vector)
left division
void Print(Option_t *option="") const
Print Quaternion parameters.
Double_t Norm() const
TQuaternion operator+(Double_t real) const
sum of quaternion with a real
TQuaternion LeftProduct(const TVector3 &vector) const
left product
TQuaternion & operator/=(Double_t real)
Double_t Norm2() const
TQuaternion & operator*=(Double_t real)
TVector3 fVectorPart
TQuaternion & SetQAngle(Double_t angle)
Set angle of quaternion (rad) - keep quaternion norm N.B : this angle is half of the corresponding ro...
TQuaternion operator/(Double_t real) const
division by a real
Double_t fRealPart
TQuaternion Invert() const
invert
TVector3 Rotation(const TVector3 &vect) const
rotation of vect by current quaternion
TQuaternion operator-() const
TQuaternion & SetAxisQAngle(TVector3 &v, Double_t QAngle)
set quaternion from vector and angle (rad) quaternion is set as unitary N.B : this angle is half of t...
TQuaternion Conjugate() const
Double_t GetQAngle() const
Get angle of quaternion (rad) N.B : this angle is half of the corresponding rotation angle.
Double_t Z() const
Definition TVector3.h:218
Double_t Phi() const
Return the azimuth angle. Returns phi from -pi to pi.
Definition TVector3.cxx:236
Double_t Y() const
Definition TVector3.h:217
Double_t Mag2() const
Definition TVector3.h:339
Double_t X() const
Definition TVector3.h:216
Double_t z() const
Definition TVector3.h:215
Double_t x() const
Definition TVector3.h:213
Double_t Mag() const
Definition TVector3.h:86
Double_t Theta() const
Return the polar angle.
Definition TVector3.cxx:244
Double_t y() const
Definition TVector3.h:214
TVector3 Cross(const TVector3 &) const
Definition TVector3.h:335
constexpr Double_t PiOver2()
Definition TMath.h:51
constexpr Double_t RadToDeg()
Conversion from radian to degree:
Definition TMath.h:73