Logo ROOT   6.13/01
Reference Guide
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
8 space 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 
26 Thus, quaternion product is a generalization of real number product and product of a
27 vector by a real number. Product of two pure vectors gives a quaternion whose real part
28 is the opposite of scalar product and the vector part the cross product.
29 
30 The conjugate of a quaternion \f$ Q = Q|r + Q|V \f$ is \f$ \bar{Q} = Q|r - Q|V \f$
31 
32 The 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 
34 Therefore, 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$
37 Unit 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 
81 http://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation
82 
83 http://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 
96 ClassImp(TQuaternion);
97 
98 /****************** CONSTRUCTORS ****************************************************/
99 ////////////////////////////////////////////////////////////////////////////////
100 
101 TQuaternion::TQuaternion(const TQuaternion & q) : TObject(q),
102  fRealPart(q.fRealPart), fVectorPart(q.fVectorPart) {}
103 
104 TQuaternion::TQuaternion(const TVector3 & vect, Double_t real)
105  : fRealPart(real), fVectorPart(vect) {}
106 
107 TQuaternion::TQuaternion(const Double_t * x0)
108  : fRealPart(x0[3]), fVectorPart(x0) {}
109 
110 TQuaternion::TQuaternion(const Float_t * x0)
111  : fRealPart(x0[3]), fVectorPart(x0) {}
112 
113 TQuaternion::TQuaternion(Double_t real, Double_t X, Double_t Y, Double_t Z)
114  : fRealPart(real), fVectorPart(X,Y,Z) {}
115 
117 
118 ////////////////////////////////////////////////////////////////////////////////
119 ///dereferencing operator const
120 
121 Double_t TQuaternion::operator () (int i) const {
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 
138 Double_t & TQuaternion::operator () (int i) {
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 
155 Double_t TQuaternion::GetQAngle() const {
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 
194 TQuaternion TQuaternion::operator+(Double_t real) const {
195  return TQuaternion(fVectorPart, fRealPart + real);
196 }
197 
198 ////////////////////////////////////////////////////////////////////////////////
199 /// substraction of real to quaternion
200 
201 TQuaternion TQuaternion::operator-(Double_t real) const {
202  return TQuaternion(fVectorPart, fRealPart - real);
203 }
204 
205 ////////////////////////////////////////////////////////////////////////////////
206 /// product of quaternion with a real
207 
208 TQuaternion TQuaternion::operator*(Double_t real) const {
209  return TQuaternion(fRealPart*real,fVectorPart.x()*real,fVectorPart.y()*real,fVectorPart.z()*real);
210 }
211 
212 
213 ////////////////////////////////////////////////////////////////////////////////
214 /// division by a real
215 
216 TQuaternion TQuaternion::operator/(Double_t real) const {
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 
226 TQuaternion operator + (Double_t r, const TQuaternion & q) { return (q+r); }
227 TQuaternion operator - (Double_t r, const TQuaternion & q) { return (-q+r); }
228 TQuaternion operator * (Double_t r, const TQuaternion & q) { return (q*r); }
229 TQuaternion 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);
253  fVectorPart = vect.Cross(fVectorPart);
254  fVectorPart += (vect * savedRealPart);
255 
256  return (*this);
257 }
258 
259 ////////////////////////////////////////////////////////////////////////////////
260 /// right multiplication
261 
263  Double_t savedRealPart = fRealPart;
264  fRealPart = -(fVectorPart * vect);
265  fVectorPart = fVectorPart.Cross(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 
347 TQuaternion operator + (const TVector3 &V, const TQuaternion &Q) { return (Q+V); }
348 TQuaternion operator - (const TVector3 &V, const TQuaternion &Q) { return (-Q+V); }
349 TQuaternion operator * (const TVector3 &V, const TQuaternion &Q) { return Q.LeftProduct(V); }
350 
351 TQuaternion 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 
493 void 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)
512  TVector3 cross(fVectorPart.Cross(quat.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 
527 void TQuaternion::Print(Option_t*) const
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 }
Double_t X() const
Definition: TVector3.h:216
TQuaternion operator-() const
Definition: TQuaternion.h:279
TVector3 fVectorPart
Definition: TQuaternion.h:111
Quaternion is a 4-component mathematic object quite convenient when dealing with space rotation (or r...
Definition: TQuaternion.h:11
Double_t Mag2() const
Definition: TVector3.h:339
TQuaternion & SetQAngle(Double_t angle)
Set angle of quaternion (rad) - keep quaternion norm N.B : this angle is half of the corresponding ro...
Double_t y() const
Definition: TVector3.h:214
void Rotate(TVector3 &vect) const
rotate vect by current quaternion
double cos(double)
Double_t Mag() const
Definition: TVector3.h:86
TQuaternion & operator/=(Double_t real)
Definition: TQuaternion.h:176
virtual ~TQuaternion()
Double_t Y() const
Definition: TVector3.h:217
TQuaternion & DivideLeft(const TVector3 &vector)
left division
double sin(double)
TVector3 is a general three vector class, which can be used for the description of different vectors ...
Definition: TVector3.h:22
TVector3 Cross(const TVector3 &) const
Definition: TVector3.h:335
Double_t Z() const
Definition: TVector3.h:218
TQuaternion & operator*=(Double_t real)
Definition: TQuaternion.h:170
SVector< double, 2 > v
Definition: Dict.h:5
TQuaternion(Double_t real=0, Double_t X=0, Double_t Y=0, Double_t Z=0)
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...
Double_t Theta() const
Return the polar angle.
Definition: TVector3.cxx:238
void Print(Option_t *option="") const
Print Quaternion parameters.
TQuaternion operator*(Double_t real) const
product of quaternion with a real
Double_t Norm() const
Definition: TQuaternion.h:265
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:215
TQuaternion & MultiplyLeft(const TVector3 &vector)
left multiplication
TQuaternion Conjugate() const
Definition: TQuaternion.h:283
TQuaternion operator/(Double_t real) const
division by a real
TVector3 Rotation(const TVector3 &vect) const
rotation of vect by current quaternion
TQuaternion Invert() const
invert
TQuaternion LeftQuotient(const TVector3 &vector) const
left quotient
double atan(double)
TQuaternion operator+(Double_t real) const
sum of quaternion with a real
constexpr Double_t RadToDeg()
Conversion from radian to degree: .
Definition: TMath.h:74
Double_t Phi() const
Return the azimuth angle. Returns phi from -pi to pi.
Definition: TVector3.cxx:230
Double_t x() const
Definition: TVector3.h:213
Double_t fRealPart
Definition: TQuaternion.h:110
Double_t operator()(int) const
dereferencing operator const
Double_t Norm2() const
Definition: TQuaternion.h:269
constexpr Double_t PiOver2()
Definition: TMath.h:52
TQuaternion LeftProduct(const TVector3 &vector) const
left product
static double Q[]