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