 ROOT   6.08/07 Reference Guide
Rotation3D.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 // Header file for class Rotation in 3 dimensions, represented by 3x3 matrix
12 //
13 // Created by: Mark Fischler Tues July 5 2005
14 //
16
17 #include <cmath>
18 #include <algorithm>
19
22
23 namespace ROOT {
24
25 namespace Math {
26
27 // ========== Constructors and Assignment =====================
28
30 {
31  // constructor of a identity rotation
32  fM[kXX] = 1.0; fM[kXY] = 0.0; fM[kXZ] = 0.0;
33  fM[kYX] = 0.0; fM[kYY] = 1.0; fM[kYZ] = 0.0;
34  fM[kZX] = 0.0; fM[kZY] = 0.0; fM[kZZ] = 1.0;
35 }
36
37
39 {
40  // rectify rotation matrix (make orthogonal)
41  // The "nearest" orthogonal matrix X to a nearly-orthogonal matrix A
42  // (in the sense that X is exaclty orthogonal and the sum of the squares
43  // of the element differences X-A is as small as possible) is given by
44  // X = A * inverse(sqrt(A.transpose()*A.inverse())).
45
46  // Step 1 -- form symmetric M = A.transpose * A
47
48  double m11 = fM[kXX]*fM[kXX] + fM[kYX]*fM[kYX] + fM[kZX]*fM[kZX];
49  double m12 = fM[kXX]*fM[kXY] + fM[kYX]*fM[kYY] + fM[kZX]*fM[kZY];
50  double m13 = fM[kXX]*fM[kXZ] + fM[kYX]*fM[kYZ] + fM[kZX]*fM[kZZ];
51  double m22 = fM[kXY]*fM[kXY] + fM[kYY]*fM[kYY] + fM[kZY]*fM[kZY];
52  double m23 = fM[kXY]*fM[kXZ] + fM[kYY]*fM[kYZ] + fM[kZY]*fM[kZZ];
53  double m33 = fM[kXZ]*fM[kXZ] + fM[kYZ]*fM[kYZ] + fM[kZZ]*fM[kZZ];
54
55  // Step 2 -- find lower-triangular U such that U * U.transpose = M
56
57  double u11 = std::sqrt(m11);
58  double u21 = m12/u11;
59  double u31 = m13/u11;
60  double u22 = std::sqrt(m22-u21*u21);
61  double u32 = (m23-m12*m13/m11)/u22;
62  double u33 = std::sqrt(m33 - u31*u31 - u32*u32);
63
64
65  // Step 3 -- find V such that V*V = U. U is also lower-triangular
66
67  double v33 = 1/u33;
68  double v32 = -v33*u32/u22;
69  double v31 = -(v32*u21+v33*u31)/u11;
70  double v22 = 1/u22;
71  double v21 = -v22*u21/u11;
72  double v11 = 1/u11;
73
74
75  // Step 4 -- N = V.transpose * V is inverse(sqrt(A.transpose()*A.inverse()))
76
77  double n11 = v11*v11 + v21*v21 + v31*v31;
78  double n12 = v11*v21 + v21*v22 + v31*v32;
79  double n13 = v11*v31 + v21*v32 + v31*v33;
80  double n22 = v21*v21 + v22*v22 + v32*v32;
81  double n23 = v21*v31 + v22*v32 + v32*v33;
82  double n33 = v31*v31 + v32*v32 + v33*v33;
83
84
85  // Step 5 -- The new matrix is A * N
86
87  double mA;
88  std::copy(fM, &fM, mA);
89
90  fM[kXX] = mA[kXX]*n11 + mA[kXY]*n12 + mA[kXZ]*n13;
91  fM[kXY] = mA[kXX]*n12 + mA[kXY]*n22 + mA[kXZ]*n23;
92  fM[kXZ] = mA[kXX]*n13 + mA[kXY]*n23 + mA[kXZ]*n33;
93  fM[kYX] = mA[kYX]*n11 + mA[kYY]*n12 + mA[kYZ]*n13;
94  fM[kYY] = mA[kYX]*n12 + mA[kYY]*n22 + mA[kYZ]*n23;
95  fM[kYZ] = mA[kYX]*n13 + mA[kYY]*n23 + mA[kYZ]*n33;
96  fM[kZX] = mA[kZX]*n11 + mA[kZY]*n12 + mA[kZZ]*n13;
97  fM[kZY] = mA[kZX]*n12 + mA[kZY]*n22 + mA[kZZ]*n23;
98  fM[kZZ] = mA[kZX]*n13 + mA[kZY]*n23 + mA[kZZ]*n33;
99
100
101 } // Rectify()
102
103
104 static inline void swap(double & a, double & b) {
105  // swap two values
106  double t=b; b=a; a=t;
107 }
108
110  // invert a rotation
111  swap (fM[kXY], fM[kYX]);
112  swap (fM[kXZ], fM[kZX]);
113  swap (fM[kYZ], fM[kZY]);
114 }
115
116
118  // combine with an AxisAngle rotation
119  return operator* ( Rotation3D(a) );
120 }
121
123  // combine with an EulerAngles rotation
124  return operator* ( Rotation3D(e) );
125 }
126
128  // combine with a Quaternion rotation
129  return operator* ( Rotation3D(q) );
130 }
131
133  // combine with a RotastionZYX rotation
134  return operator* ( Rotation3D(r) );
135 }
136
137 std::ostream & operator<< (std::ostream & os, const Rotation3D & r) {
138  // TODO - this will need changing for machine-readable issues
139  // and even the human readable form needs formatting improvements
140  double m;
141  r.GetComponents(m, m+9);
142  os << "\n" << m << " " << m << " " << m;
143  os << "\n" << m << " " << m << " " << m;
144  os << "\n" << m << " " << m << " " << m << "\n";
145  return os;
146 }
147
148 } //namespace Math
149 } //namespace ROOT
This namespace contains pre-defined functions to be used in conjuction with TExecutor::Map and TExecu...
Definition: StringConv.hxx:21
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 with the (3D) rotation represented by angles describing first a rotation of an angle p...
Definition: RotationZYX.h:71
TArc * a
Definition: textangle.C:12
Rotation class with the (3D) rotation represented by a unit quaternion (u, i, j, k).
Definition: Quaternion.h:47
std::ostream & operator<<(std::ostream &os, const AxisAngle &a)
Stream Output and Input.
Definition: AxisAngle.cxx:91
AxisAngle class describing rotation represented with direction axis (3D Vector) and an angle of rotat...
Definition: AxisAngle.h:41
double sqrt(double)
static void swap(double &a, double &b)
Definition: Rotation3D.cxx:104
void Rectify()
Re-adjust components to eliminate small deviations from perfect orthonormality.
Definition: Rotation3D.cxx:38
AVector operator*(const AVector &v) const
Overload operator * for rotation on a vector.
Definition: Rotation3D.h:407
TRandom2 r(17)
Rotation class with the (3D) rotation represented by a 3x3 orthogonal matrix.
Definition: Rotation3D.h:65
TMarker * m
Definition: textangle.C:8
EulerAngles class describing rotation as three angles (Euler Angles).
Definition: EulerAngles.h:43
you should not use this method at all Int_t Int_t Double_t Double_t Double_t e
Definition: TRolke.cxx:630
Namespace for new Math classes and functions.
void Invert()
Invert a rotation in place.
Definition: Rotation3D.cxx:109
Rotation3D()
Default constructor (identity rotation)
Definition: Rotation3D.cxx:29
you should not use this method at all Int_t Int_t Double_t Double_t Double_t Int_t Double_t Double_t Double_t Double_t b
Definition: TRolke.cxx:630
float * q
Definition: THbookFile.cxx:87