 ROOT   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
23namespace ROOT {
24
25namespace 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
104static 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
137std::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
ROOT::R::TRInterface & r
Definition: Object.C:4
#define b(i)
Definition: RSha256.hxx:100
#define e(i)
Definition: RSha256.hxx:103
float * q
Definition: THbookFile.cxx:87
double sqrt(double)
AxisAngle class describing rotation represented with direction axis (3D Vector) and an angle of rotat...
Definition: AxisAngle.h:41
EulerAngles class describing rotation as three angles (Euler Angles).
Definition: EulerAngles.h:43
Rotation class with the (3D) rotation represented by a unit quaternion (u, i, j, k).
Definition: Quaternion.h:47
Rotation class with the (3D) rotation represented by a 3x3 orthogonal matrix.
Definition: Rotation3D.h:65
void Rectify()
Re-adjust components to eliminate small deviations from perfect orthonormality.
Definition: Rotation3D.cxx:38
void Invert()
Invert a rotation in place.
Definition: Rotation3D.cxx:109
AVector operator*(const AVector &v) const
Overload operator * for rotation on a vector.
Definition: Rotation3D.h:401
Rotation3D()
Default constructor (identity rotation)
Definition: Rotation3D.cxx:29
Rotation class with the (3D) rotation represented by angles describing first a rotation of an angle p...
Definition: RotationZYX.h:61
Namespace for new Math classes and functions.
std::ostream & operator<<(std::ostream &os, const AxisAngle &a)
Stream Output and Input.
Definition: AxisAngle.cxx:91
static void swap(double &a, double &b)
Definition: Rotation3D.cxx:104
tbb::task_arena is an alias of tbb::interface7::task_arena, which doesn't allow to forward declare tb...
Definition: StringConv.hxx:21
auto * m
Definition: textangle.C:8
auto * a
Definition: textangle.C:12