Logo ROOT   6.14/05
Reference Guide
3DConversions.cxx
Go to the documentation of this file.
1 // @(#)root/mathcore:$Id$
2 // Authors: W. Brown, M. Fischler, L. Moneta 2005
3 
4  /**********************************************************************
5  * *
6  * Copyright (c) 2005, LCG ROOT FNAL MathLib Team *
7  * *
8  * *
9  **********************************************************************/
10 
11 // Source file for something else
12 //
13 // Created by: Mark Fischler and Walter Brown Thurs July 7, 2005
14 //
15 // Last update: $Id$
16 //
17 
18 // TODO - For now, all conversions are grouped in this one compilation unit.
19 // The intention is to seraparte them into a few .cpp files instead,
20 // so that users needing one form need not incorporate code for them all.
21 
23 
24 #include "Math/Math.h"
25 
34 
35 #include <cmath>
36 #include <limits>
37 
38 namespace ROOT {
39 namespace Math {
40 namespace gv_detail {
41 
46 };
47 
48 
49 // ----------------------------------------------------------------------
50 void convert( Rotation3D const & from, AxisAngle & to)
51 {
52  // conversions from Rotation3D
53  double m[9];
54  from.GetComponents(m, m+9);
55 
56  const double uZ = m[kYX] - m[kXY];
57  const double uY = m[kXZ] - m[kZX];
58  const double uX = m[kZY] - m[kYZ];
59 
60 
61  // in case of rotaiton of an angle PI, the rotation matrix is symmetric and
62  // uX = uY = uZ = 0. Use then conversion through the quaternion
66  Quaternion tmp;
67  convert (from,tmp);
68  convert (tmp,to);
69  return;
70  }
71 
73 
74  u.SetCoordinates( uX, uY, uZ );
75 
76  static const double pi = M_PI;
77 
78  double angle;
79  const double cosdelta = (m[kXX] + m[kYY] + m[kZZ] - 1.0) / 2.0;
80  if (cosdelta > 1.0) {
81  angle = 0;
82  } else if (cosdelta < -1.0) {
83  angle = pi;
84  } else {
85  angle = std::acos( cosdelta );
86  }
87 
88 
89  //to.SetAngle(angle);
90  to.SetComponents(u, angle);
91  to.Rectify();
92 
93 } // convert to AxisAngle
94 
95 static void correctByPi ( double& psi, double& phi ) {
96  static const double pi = M_PI;
97  if (psi > 0) {
98  psi -= pi;
99  } else {
100  psi += pi;
101  }
102  if (phi > 0) {
103  phi -= pi;
104  } else {
105  phi += pi;
106  }
107 }
108 
109 void convert( Rotation3D const & from, EulerAngles & to)
110 {
111  // conversion from Rotation3D to Euler Angles
112  // Mathematical justification appears in
113  // http://www.cern.ch/mathlibs/documents/eulerAngleComputation.pdf
114 
115  double r[9];
116  from.GetComponents(r,r+9);
117 
118  double phi, theta, psi;
119  double psiPlusPhi, psiMinusPhi;
120  static const double pi = M_PI;
121  static const double pi_2 = M_PI_2;
122 
123  theta = (std::fabs(r[kZZ]) <= 1.0) ? std::acos(r[kZZ]) :
124  (r[kZZ] > 0.0) ? 0 : pi;
125 
126  double cosTheta = r[kZZ];
127  if (cosTheta > 1) cosTheta = 1;
128  if (cosTheta < -1) cosTheta = -1;
129 
130  // Compute psi +/- phi:
131  // Depending on whether cosTheta is positive or negative and whether it
132  // is less than 1 in absolute value, different mathematically equivalent
133  // expressions are numerically stable.
134  if (cosTheta == 1) {
135  psiPlusPhi = atan2 ( r[kXY] - r[kYX], r[kXX] + r[kYY] );
136  psiMinusPhi = 0;
137  } else if (cosTheta >= 0) {
138  psiPlusPhi = atan2 ( r[kXY] - r[kYX], r[kXX] + r[kYY] );
139  double s = -r[kXY] - r[kYX]; // sin (psi-phi) * (1 - cos theta)
140  double c = r[kXX] - r[kYY]; // cos (psi-phi) * (1 - cos theta)
141  psiMinusPhi = atan2 ( s, c );
142  } else if (cosTheta > -1) {
143  psiMinusPhi = atan2 ( -r[kXY] - r[kYX], r[kXX] - r[kYY] );
144  double s = r[kXY] - r[kYX]; // sin (psi+phi) * (1 + cos theta)
145  double c = r[kXX] + r[kYY]; // cos (psi+phi) * (1 + cos theta)
146  psiPlusPhi = atan2 ( s, c );
147  } else { // cosTheta == -1
148  psiMinusPhi = atan2 ( -r[kXY] - r[kYX], r[kXX] - r[kYY] );
149  psiPlusPhi = 0;
150  }
151 
152  psi = .5 * (psiPlusPhi + psiMinusPhi);
153  phi = .5 * (psiPlusPhi - psiMinusPhi);
154 
155  // Now correct by pi if we have managed to get a value of psiPlusPhi
156  // or psiMinusPhi that was off by 2 pi:
157 
158  // set up w[i], all of which would be positive if sin and cosine of
159  // psi and phi were positive:
160  double w[4];
161  w[0] = r[kXZ]; w[1] = r[kZX]; w[2] = r[kYZ]; w[3] = -r[kZY];
162 
163  // find biggest relevant term, which is the best one to use in correcting.
164  double maxw = std::fabs(w[0]);
165  int imax = 0;
166  for (int i = 1; i < 4; ++i) {
167  if (std::fabs(w[i]) > maxw) {
168  maxw = std::fabs(w[i]);
169  imax = i;
170  }
171  }
172  // Determine if the correction needs to be applied: The criteria are
173  // different depending on whether a sine or cosine was the determinor:
174  switch (imax) {
175  case 0:
176  if (w[0] > 0 && psi < 0) correctByPi ( psi, phi );
177  if (w[0] < 0 && psi > 0) correctByPi ( psi, phi );
178  break;
179  case 1:
180  if (w[1] > 0 && phi < 0) correctByPi ( psi, phi );
181  if (w[1] < 0 && phi > 0) correctByPi ( psi, phi );
182  break;
183  case 2:
184  if (w[2] > 0 && std::fabs(psi) > pi_2) correctByPi ( psi, phi );
185  if (w[2] < 0 && std::fabs(psi) < pi_2) correctByPi ( psi, phi );
186  break;
187  case 3:
188  if (w[3] > 0 && std::fabs(phi) > pi_2) correctByPi ( psi, phi );
189  if (w[3] < 0 && std::fabs(phi) < pi_2) correctByPi ( psi, phi );
190  break;
191  }
192 
193  to.SetComponents( phi, theta, psi );
194 
195 } // convert to EulerAngles
196 
197 ////////////////////////////////////////////////////////////////////////////////
198 /// conversion from Rotation3D to Quaternion
199 
200 void convert( Rotation3D const & from, Quaternion & to)
201 {
202  double m[9];
203  from.GetComponents(m, m+9);
204 
205  const double d0 = m[kXX] + m[kYY] + m[kZZ];
206  const double d1 = + m[kXX] - m[kYY] - m[kZZ];
207  const double d2 = - m[kXX] + m[kYY] - m[kZZ];
208  const double d3 = - m[kXX] - m[kYY] + m[kZZ];
209 
210  // these are related to the various q^2 values;
211  // choose the largest to avoid dividing two small numbers and losing accuracy.
212 
213  if ( d0 >= d1 && d0 >= d2 && d0 >= d3 ) {
214  const double q0 = .5*std::sqrt(1+d0);
215  const double f = .25/q0;
216  const double q1 = f*(m[kZY]-m[kYZ]);
217  const double q2 = f*(m[kXZ]-m[kZX]);
218  const double q3 = f*(m[kYX]-m[kXY]);
219  to.SetComponents(q0,q1,q2,q3);
220  to.Rectify();
221  return;
222  } else if ( d1 >= d2 && d1 >= d3 ) {
223  const double q1 = .5*std::sqrt(1+d1);
224  const double f = .25/q1;
225  const double q0 = f*(m[kZY]-m[kYZ]);
226  const double q2 = f*(m[kXY]+m[kYX]);
227  const double q3 = f*(m[kXZ]+m[kZX]);
228  to.SetComponents(q0,q1,q2,q3);
229  to.Rectify();
230  return;
231  } else if ( d2 >= d3 ) {
232  const double q2 = .5*std::sqrt(1+d2);
233  const double f = .25/q2;
234  const double q0 = f*(m[kXZ]-m[kZX]);
235  const double q1 = f*(m[kXY]+m[kYX]);
236  const double q3 = f*(m[kYZ]+m[kZY]);
237  to.SetComponents(q0,q1,q2,q3);
238  to.Rectify();
239  return;
240  } else {
241  const double q3 = .5*std::sqrt(1+d3);
242  const double f = .25/q3;
243  const double q0 = f*(m[kYX]-m[kXY]);
244  const double q1 = f*(m[kXZ]+m[kZX]);
245  const double q2 = f*(m[kYZ]+m[kZY]);
246  to.SetComponents(q0,q1,q2,q3);
247  to.Rectify();
248  return;
249  }
250 } // convert to Quaternion
251 
252 ////////////////////////////////////////////////////////////////////////////////
253 /// conversion from Rotation3D to RotationZYX
254 /// same Math used as for EulerAngles apart from some different meaning of angles and
255 /// matrix elements. But the basic algoprithms principles are the same described in
256 /// http://www.cern.ch/mathlibs/documents/eulerAngleComputation.pdf
257 
258 void convert( Rotation3D const & from, RotationZYX & to)
259 {
260  // theta is assumed to be in range [-PI/2,PI/2].
261  // this is guranteed by the Rectify function
262 
263  static const double pi_2 = M_PI_2;
264 
265  double r[9];
266  from.GetComponents(r,r+9);
267 
268  double phi,theta,psi = 0;
269 
270  // careful for numeical error make sin(theta) ourtside [-1,1]
271  double sinTheta = r[kXZ];
272  if ( sinTheta < -1.0) sinTheta = -1.0;
273  if ( sinTheta > 1.0) sinTheta = 1.0;
274  theta = std::asin( sinTheta );
275 
276  // compute psi +/- phi
277  // Depending on whether cosTheta is positive or negative and whether it
278  // is less than 1 in absolute value, different mathematically equivalent
279  // expressions are numerically stable.
280  // algorithm from
281  // adapted for the case 3-2-1
282 
283  double psiPlusPhi = 0;
284  double psiMinusPhi = 0;
285 
286  // valid if sinTheta not eq to -1 otherwise is zero
287  if (sinTheta > - 1.0)
288  psiPlusPhi = atan2 ( r[kYX] + r[kZY], r[kYY] - r[kZX] );
289 
290  // valid if sinTheta not eq. to 1
291  if (sinTheta < 1.0)
292  psiMinusPhi = atan2 ( r[kZY] - r[kYX] , r[kYY] + r[kZX] );
293 
294  psi = .5 * (psiPlusPhi + psiMinusPhi);
295  phi = .5 * (psiPlusPhi - psiMinusPhi);
296 
297  // correction is not necessary if sinTheta = +/- 1
298  //if (sinTheta == 1.0 || sinTheta == -1.0) return;
299 
300  // apply the corrections according to max of the other terms
301  // I think is assumed convention that theta is between -PI/2,PI/2.
302  // OTHERWISE RESULT MIGHT BE DIFFERENT ???
303 
304  //since we determine phi+psi or phi-psi phi and psi can be both have a shift of +/- PI.
305  // The shift must be applied on both (the sum (or difference) is knows to +/- 2PI )
306  //This can be fixed looking at the other 4 matrix terms, which have terms in sin and cos of psi
307  // and phi. sin(psi+/-PI) = -sin(psi) and cos(psi+/-PI) = -cos(psi).
308  //Use then the biggest term for making the correction to minimize possible numerical errors
309 
310  // set up w[i], all of which would be positive if sin and cosine of
311  // psi and phi were positive:
312  double w[4];
313  w[0] = -r[kYZ]; w[1] = -r[kXY]; w[2] = r[kZZ]; w[3] = r[kXX];
314 
315  // find biggest relevant term, which is the best one to use in correcting.
316  double maxw = std::fabs(w[0]);
317  int imax = 0;
318  for (int i = 1; i < 4; ++i) {
319  if (std::fabs(w[i]) > maxw) {
320  maxw = std::fabs(w[i]);
321  imax = i;
322  }
323  }
324 
325  // Determine if the correction needs to be applied: The criteria are
326  // different depending on whether a sine or cosine was the determinor:
327  switch (imax) {
328  case 0:
329  if (w[0] > 0 && psi < 0) correctByPi ( psi, phi );
330  if (w[0] < 0 && psi > 0) correctByPi ( psi, phi );
331  break;
332  case 1:
333  if (w[1] > 0 && phi < 0) correctByPi ( psi, phi );
334  if (w[1] < 0 && phi > 0) correctByPi ( psi, phi );
335  break;
336  case 2:
337  if (w[2] > 0 && std::fabs(psi) > pi_2) correctByPi ( psi, phi );
338  if (w[2] < 0 && std::fabs(psi) < pi_2) correctByPi ( psi, phi );
339  break;
340  case 3:
341  if (w[3] > 0 && std::fabs(phi) > pi_2) correctByPi ( psi, phi );
342  if (w[3] < 0 && std::fabs(phi) < pi_2) correctByPi ( psi, phi );
343  break;
344  }
345 
346  to.SetComponents(phi, theta, psi);
347 
348 } // convert to RotationZYX
349 
350 // ----------------------------------------------------------------------
351 // conversions from AxisAngle
352 
353 void convert( AxisAngle const & from, Rotation3D & to)
354 {
355  // conversion from AxixAngle to Rotation3D
356 
357  const double sinDelta = std::sin( from.Angle() );
358  const double cosDelta = std::cos( from.Angle() );
359  const double oneMinusCosDelta = 1.0 - cosDelta;
360 
361  const AxisAngle::AxisVector & u = from.Axis();
362  const double uX = u.X();
363  const double uY = u.Y();
364  const double uZ = u.Z();
365 
366  double m[9];
367 
368  m[kXX] = oneMinusCosDelta * uX * uX + cosDelta;
369  m[kXY] = oneMinusCosDelta * uX * uY - sinDelta * uZ;
370  m[kXZ] = oneMinusCosDelta * uX * uZ + sinDelta * uY;
371 
372  m[kYX] = oneMinusCosDelta * uY * uX + sinDelta * uZ;
373  m[kYY] = oneMinusCosDelta * uY * uY + cosDelta;
374  m[kYZ] = oneMinusCosDelta * uY * uZ - sinDelta * uX;
375 
376  m[kZX] = oneMinusCosDelta * uZ * uX - sinDelta * uY;
377  m[kZY] = oneMinusCosDelta * uZ * uY + sinDelta * uX;
378  m[kZZ] = oneMinusCosDelta * uZ * uZ + cosDelta;
379 
380  to.SetComponents(m,m+9);
381 } // convert to Rotation3D
382 
383 void convert( AxisAngle const & from , EulerAngles & to )
384 {
385  // conversion from AxixAngle to EulerAngles
386  // TODO better : temporary make conversion using Rotation3D
387 
388  Rotation3D tmp;
389  convert(from,tmp);
390  convert(tmp,to);
391 }
392 
393 void convert( AxisAngle const & from, Quaternion & to)
394 {
395  // conversion from AxixAngle to Quaternion
396 
397  double s = std::sin (from.Angle()/2);
399 
400  to.SetComponents( std::cos(from.Angle()/2),
401  s*axis.X(),
402  s*axis.Y(),
403  s*axis.Z()
404  );
405 } // convert to Quaternion
406 
407 void convert( AxisAngle const & from , RotationZYX & to )
408 {
409  // conversion from AxisAngle to RotationZYX
410  // TODO better : temporary make conversion using Rotation3D
411  Rotation3D tmp;
412  convert(from,tmp);
413  convert(tmp,to);
414 }
415 
416 
417 // ----------------------------------------------------------------------
418 // conversions from EulerAngles
419 
420 void convert( EulerAngles const & from, Rotation3D & to)
421 {
422  // conversion from EulerAngles to Rotation3D
423 
424  typedef double Scalar;
425  const Scalar sPhi = std::sin( from.Phi() );
426  const Scalar cPhi = std::cos( from.Phi() );
427  const Scalar sTheta = std::sin( from.Theta() );
428  const Scalar cTheta = std::cos( from.Theta() );
429  const Scalar sPsi = std::sin( from.Psi() );
430  const Scalar cPsi = std::cos( from.Psi() );
431  to.SetComponents
432  ( cPsi*cPhi-sPsi*cTheta*sPhi, cPsi*sPhi+sPsi*cTheta*cPhi, sPsi*sTheta
433  , -sPsi*cPhi-cPsi*cTheta*sPhi, -sPsi*sPhi+cPsi*cTheta*cPhi, cPsi*sTheta
434  , sTheta*sPhi, -sTheta*cPhi, cTheta
435  );
436 }
437 
438 void convert( EulerAngles const & from, AxisAngle & to)
439 {
440  // conversion from EulerAngles to AxisAngle
441  // make converting first to quaternion
442  Quaternion q;
443  convert (from, q);
444  convert (q, to);
445 }
446 
447 void convert( EulerAngles const & from, Quaternion & to)
448 {
449  // conversion from EulerAngles to Quaternion
450 
451  typedef double Scalar;
452  const Scalar plus = (from.Phi()+from.Psi())/2;
453  const Scalar minus = (from.Phi()-from.Psi())/2;
454  const Scalar sPlus = std::sin( plus );
455  const Scalar cPlus = std::cos( plus );
456  const Scalar sMinus = std::sin( minus );
457  const Scalar cMinus = std::cos( minus );
458  const Scalar sTheta = std::sin( from.Theta()/2 );
459  const Scalar cTheta = std::cos( from.Theta()/2 );
460 
461  to.SetComponents ( cTheta*cPlus, -sTheta*cMinus, -sTheta*sMinus, -cTheta*sPlus );
462  // TODO -- carefully check that this is correct
463 }
464 
465 void convert( EulerAngles const & from , RotationZYX & to )
466 {
467  // conversion from EulerAngles to RotationZYX
468  // TODO better : temporary make conversion using Rotation3D
469  Rotation3D tmp;
470  convert(from,tmp);
471  convert(tmp,to);
472 }
473 
474 
475 // ----------------------------------------------------------------------
476 // conversions from Quaternion
477 
478 void convert( Quaternion const & from, Rotation3D & to)
479 {
480  // conversion from Quaternion to Rotation3D
481 
482  const double q0 = from.U();
483  const double q1 = from.I();
484  const double q2 = from.J();
485  const double q3 = from.K();
486  const double q00 = q0*q0;
487  const double q01 = q0*q1;
488  const double q02 = q0*q2;
489  const double q03 = q0*q3;
490  const double q11 = q1*q1;
491  const double q12 = q1*q2;
492  const double q13 = q1*q3;
493  const double q22 = q2*q2;
494  const double q23 = q2*q3;
495  const double q33 = q3*q3;
496 
497  to.SetComponents (
498  q00+q11-q22-q33 , 2*(q12-q03) , 2*(q02+q13),
499  2*(q12+q03) , q00-q11+q22-q33 , 2*(q23-q01),
500  2*(q13-q02) , 2*(q23+q01) , q00-q11-q22+q33 );
501 
502 } // conversion to Rotation3D
503 
504 void convert( Quaternion const & from, AxisAngle & to)
505 {
506  // conversion from Quaternion to AxisAngle
507 
508  double u = from.U();
509  if ( u >= 0 ) {
510  if ( u > 1 ) u = 1;
511  const double angle = 2.0 * std::acos ( from.U() );
513  axis (from.I(), from.J(), from.K());
514  to.SetComponents ( axis, angle );
515  } else {
516  if ( u < -1 ) u = -1;
517  const double angle = 2.0 * std::acos ( -from.U() );
519  axis (-from.I(), -from.J(), -from.K());
520  to.SetComponents ( axis, angle );
521  }
522 } // conversion to AxisAngle
523 
524 void convert( Quaternion const & from, EulerAngles & to )
525 {
526  // conversion from Quaternion to EulerAngles
527  // TODO better
528  // temporary make conversion using Rotation3D
529 
530  Rotation3D tmp;
531  convert(from,tmp);
532  convert(tmp,to);
533 }
534 
535 void convert( Quaternion const & from , RotationZYX & to )
536 {
537  // conversion from Quaternion to RotationZYX
538  // TODO better : temporary make conversion using Rotation3D
539  Rotation3D tmp;
540  convert(from,tmp);
541  convert(tmp,to);
542 }
543 
544 // ----------------------------------------------------------------------
545 // conversions from RotationZYX
546 void convert( RotationZYX const & from, Rotation3D & to) {
547  // conversion to Rotation3D (matrix)
548 
549  double phi,theta,psi = 0;
550  from.GetComponents(phi,theta,psi);
551  to.SetComponents( std::cos(theta)*std::cos(phi),
552  - std::cos(theta)*std::sin(phi),
553  std::sin(theta),
554 
555  std::cos(psi)*std::sin(phi) + std::sin(psi)*std::sin(theta)*std::cos(phi),
556  std::cos(psi)*std::cos(phi) - std::sin(psi)*std::sin(theta)*std::sin(phi),
557  -std::sin(psi)*std::cos(theta),
558 
559  std::sin(psi)*std::sin(phi) - std::cos(psi)*std::sin(theta)*std::cos(phi),
560  std::sin(psi)*std::cos(phi) + std::cos(psi)*std::sin(theta)*std::sin(phi),
561  std::cos(psi)*std::cos(theta)
562  );
563 
564 }
565 void convert( RotationZYX const & from, AxisAngle & to) {
566  // conversion to axis angle
567  // TODO better : temporary make conversion using Rotation3D
568  Rotation3D tmp;
569  convert(from,tmp);
570  convert(tmp,to);
571 }
572 void convert( RotationZYX const & from, EulerAngles & to) {
573  // conversion to Euler angle
574  // TODO better : temporary make conversion using Rotation3D
575  Rotation3D tmp;
576  convert(from,tmp);
577  convert(tmp,to);
578 }
579 void convert( RotationZYX const & from, Quaternion & to) {
580  double phi,theta,psi = 0;
581  from.GetComponents(phi,theta,psi);
582 
583  double sphi2 = std::sin(phi/2);
584  double cphi2 = std::cos(phi/2);
585  double stheta2 = std::sin(theta/2);
586  double ctheta2 = std::cos(theta/2);
587  double spsi2 = std::sin(psi/2);
588  double cpsi2 = std::cos(psi/2);
589  to.SetComponents( cphi2 * cpsi2 * ctheta2 - sphi2 * spsi2 * stheta2,
590  sphi2 * cpsi2 * stheta2 + cphi2 * spsi2 * ctheta2,
591  cphi2 * cpsi2 * stheta2 - sphi2 * spsi2 * ctheta2,
592  sphi2 * cpsi2 * ctheta2 + cphi2 * spsi2 * stheta2
593  );
594 }
595 
596 
597 // ----------------------------------------------------------------------
598 // conversions from RotationX
599 
600 void convert( RotationX const & from, Rotation3D & to)
601 {
602  // conversion from RotationX to Rotation3D
603 
604  const double c = from.CosAngle();
605  const double s = from.SinAngle();
606  to.SetComponents ( 1, 0, 0,
607  0, c, -s,
608  0, s, c );
609 }
610 
611 void convert( RotationX const & from, AxisAngle & to)
612 {
613  // conversion from RotationX to AxisAngle
614 
616  to.SetComponents ( axis, from.Angle() );
617 }
618 
619 void convert( RotationX const & from , EulerAngles & to )
620 {
621  // conversion from RotationX to EulerAngles
622  //TODO better: temporary make conversion using Rotation3D
623 
624  Rotation3D tmp;
625  convert(from,tmp);
626  convert(tmp,to);
627 }
628 
629 void convert( RotationX const & from, Quaternion & to)
630 {
631  // conversion from RotationX to Quaternion
632 
633  to.SetComponents (std::cos(from.Angle()/2), std::sin(from.Angle()/2), 0, 0);
634 }
635 
636 void convert( RotationX const & from , RotationZYX & to )
637 {
638  // conversion from RotationX to RotationZYX
639  to.SetComponents(0,0,from.Angle());
640 }
641 
642 
643 // ----------------------------------------------------------------------
644 // conversions from RotationY
645 
646 void convert( RotationY const & from, Rotation3D & to)
647 {
648  // conversion from RotationY to Rotation3D
649 
650  const double c = from.CosAngle();
651  const double s = from.SinAngle();
652  to.SetComponents ( c, 0, s,
653  0, 1, 0,
654  -s, 0, c );
655 }
656 
657 void convert( RotationY const & from, AxisAngle & to)
658 {
659  // conversion from RotationY to AxisAngle
660 
662  to.SetComponents ( axis, from.Angle() );
663 }
664 
665 void convert( RotationY const & from, EulerAngles & to )
666 {
667  // conversion from RotationY to EulerAngles
668  // TODO better: temporary make conversion using Rotation3D
669 
670  Rotation3D tmp;
671  convert(from,tmp);
672  convert(tmp,to);
673 }
674 
675 void convert( RotationY const & from , RotationZYX & to )
676 {
677  // conversion from RotationY to RotationZYX
678  to.SetComponents(0,from.Angle(),0);
679 }
680 
681 
682 void convert( RotationY const & from, Quaternion & to)
683 {
684  // conversion from RotationY to Quaternion
685 
686  to.SetComponents (std::cos(from.Angle()/2), 0, std::sin(from.Angle()/2), 0);
687 }
688 
689 
690 
691 // ----------------------------------------------------------------------
692 // conversions from RotationZ
693 
694 void convert( RotationZ const & from, Rotation3D & to)
695 {
696  // conversion from RotationZ to Rotation3D
697 
698  const double c = from.CosAngle();
699  const double s = from.SinAngle();
700  to.SetComponents ( c, -s, 0,
701  s, c, 0,
702  0, 0, 1 );
703 }
704 
705 void convert( RotationZ const & from, AxisAngle & to)
706 {
707  // conversion from RotationZ to AxisAngle
708 
710  to.SetComponents ( axis, from.Angle() );
711 }
712 
713 void convert( RotationZ const & from , EulerAngles & to )
714 {
715  // conversion from RotationZ to EulerAngles
716  // TODO better: temporary make conversion using Rotation3D
717 
718  Rotation3D tmp;
719  convert(from,tmp);
720  convert(tmp,to);
721 }
722 
723 void convert( RotationZ const & from , RotationZYX & to )
724 {
725  // conversion from RotationY to RotationZYX
726  to.SetComponents(from.Angle(),0,0);
727 }
728 
729 void convert( RotationZ const & from, Quaternion & to)
730 {
731  // conversion from RotationZ to Quaternion
732 
733  to.SetComponents (std::cos(from.Angle()/2), 0, 0, std::sin(from.Angle()/2));
734 }
735 
736 } //namespace gv_detail
737 } //namespace Math
738 } //namespace ROOT
Scalar SinAngle() const
Sine or Cosine of the rotation angle.
Definition: RotationZ.h:108
Scalar Angle() const
Angle of rotation.
Definition: RotationY.h:103
Scalar I() const
Definition: Quaternion.h:172
DisplacementVector3D< CoordSystem, Tag > & SetCoordinates(const Scalar src[])
Set internal data based on a C-style array of 3 Scalar numbers.
static constexpr double pi
auto * m
Definition: textangle.C:8
Namespace for new ROOT classes and functions.
Definition: StringConv.hxx:21
void SetComponents(IT begin, IT end)
Set the three Euler angles given a pair of pointers or iterators defining the beginning and end of an...
Definition: EulerAngles.h:152
void GetComponents(ForeignVector &v1, ForeignVector &v2, ForeignVector &v3) const
Get components into three vectors which will be the (orthonormal) columns of the rotation matrix...
Definition: Rotation3D.h:249
Rotation class representing a 3D rotation about the Z axis by the angle of rotation.
Definition: RotationZ.h:43
Scalar CosAngle() const
Definition: RotationZ.h:109
Scalar SinAngle() const
Sine or Cosine of the rotation angle.
Definition: RotationY.h:108
Scalar J() const
Definition: Quaternion.h:173
Rotation class with the (3D) rotation represented by angles describing first a rotation of an angle p...
Definition: RotationZYX.h:61
#define f(i)
Definition: RSha256.hxx:104
Rotation class with the (3D) rotation represented by a unit quaternion (u, i, j, k).
Definition: Quaternion.h:47
double cos(double)
Scalar Phi() const
Return Phi Euler angle.
Definition: EulerAngles.h:212
Scalar CosAngle() const
Definition: RotationY.h:109
AxisAngle class describing rotation represented with direction axis (3D Vector) and an angle of rotat...
Definition: AxisAngle.h:41
Scalar X() const
Cartesian X, converting if necessary from internal coordinate system.
Scalar Psi() const
Return Psi Euler angle.
Definition: EulerAngles.h:232
double sqrt(double)
double acos(double)
Rotation class representing a 3D rotation about the Y axis by the angle of rotation.
Definition: RotationY.h:43
Scalar U() const
Access to the four quaternion components: U() is the coefficient of the identity Pauli matrix...
Definition: Quaternion.h:171
void SetComponents(IT begin, IT end)
Set the axis and then the angle given a pair of pointers or iterators defining the beginning and end ...
Definition: AxisAngle.h:117
Scalar Z() const
Cartesian Z, converting if necessary from internal coordinate system.
Scalar Y() const
Cartesian Y, converting if necessary from internal coordinate system.
Scalar Theta() const
Return Theta Euler angle.
Definition: EulerAngles.h:222
#define M_PI_2
Definition: Math.h:42
double sin(double)
AxisVector Axis() const
accesss to rotation axis
Definition: AxisAngle.h:183
void Rectify()
Re-adjust components to eliminate small deviations from the axis being a unit vector and angles out o...
Definition: AxisAngle.cxx:47
VecExpr< UnaryOp< Fabs< T >, VecExpr< A, T, D >, T >, T, D > fabs(const VecExpr< A, T, D > &rhs)
Scalar Angle() const
Angle of rotation.
Definition: RotationX.h:103
ROOT::R::TRInterface & r
Definition: Object.C:4
Scalar Angle() const
access to rotation angle
Definition: AxisAngle.h:188
#define M_PI
Definition: Rotated.cxx:105
Rotation class representing a 3D rotation about the X axis by the angle of rotation.
Definition: RotationX.h:43
Rotation class with the (3D) rotation represented by a 3x3 orthogonal matrix.
Definition: Rotation3D.h:65
static void correctByPi(double &psi, double &phi)
Scalar CosAngle() const
Definition: RotationX.h:109
void GetComponents(IT begin, IT end) const
Get the axis and then the angle into data specified by an iterator begin and another to the end of th...
Definition: RotationZYX.h:142
double asin(double)
REAL epsilon
Definition: triangle.c:617
void convert(R1 const &, R2 const)
Definition: 3DConversions.h:41
double atan2(double, double)
EulerAngles class describing rotation as three angles (Euler Angles).
Definition: EulerAngles.h:43
static constexpr double s
Namespace for new Math classes and functions.
void SetComponents(const ForeignVector &v1, const ForeignVector &v2, const ForeignVector &v3)
Set components from three orthonormal vectors (which must have methods x(), y() and z()) which will b...
Definition: Rotation3D.h:233
void Rectify()
Re-adjust components to eliminate small deviations from |Q| = 1 orthonormality.
Definition: Quaternion.cxx:35
#define c(i)
Definition: RSha256.hxx:101
Scalar K() const
Definition: Quaternion.h:174
Scalar Angle() const
Angle of rotation.
Definition: RotationZ.h:103
float * q
Definition: THbookFile.cxx:87
void SetComponents(IT begin, IT end)
Set the three Euler angles given a pair of pointers or iterators defining the beginning and end of an...
Definition: RotationZYX.h:125
Rotation3D::Scalar Scalar
Scalar SinAngle() const
Sine or Cosine of the rotation angle.
Definition: RotationX.h:108
void SetComponents(IT begin, IT end)
Set the four components given an iterator to the start of the desired data, and another to the end (4...
Definition: Quaternion.h:112