Logo ROOT   master
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[9];
88  std::copy(fM, &fM[9], 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[9];
141  r.GetComponents(m, m+9);
142  os << "\n" << m[0] << " " << m[1] << " " << m[2];
143  os << "\n" << m[3] << " " << m[4] << " " << m[5];
144  os << "\n" << m[6] << " " << m[7] << " " << m[8] << "\n";
145  return os;
146 }
147 
148 } //namespace Math
149 } //namespace ROOT
auto * m
Definition: textangle.C:8
Returns the available number of logical cores.
Definition: RNumpyDS.hxx:30
Rotation class with the (3D) rotation represented by angles describing first a rotation of an angle p...
Definition: RotationZYX.h:61
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:401
ROOT::R::TRInterface & r
Definition: Object.C:4
auto * a
Definition: textangle.C:12
Rotation class with the (3D) rotation represented by a 3x3 orthogonal matrix.
Definition: Rotation3D.h:65
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