CMS 3D CMS Logo
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Macros Pages
FrameToFrameDerivative.cc
Go to the documentation of this file.
1 
7 #include "Alignment/CommonAlignmentParametrization/interface/FrameToFrameDerivative.h"
8 
9 #include "Alignment/CommonAlignment/interface/Alignable.h"
10 
11 // already in header: #include "DataFormats/Math/interface/AlgebraicROOTObjects.h"
12 #include "DataFormats/CLHEP/interface/Migration.h"
13 
14 //__________________________________________________________________________________________________
15 AlgebraicMatrix
16 FrameToFrameDerivative::frameToFrameDerivative(const Alignable* object,
17  const Alignable* composedObject) const
18 {
19 
20  return getDerivative( object->globalRotation(),
21  composedObject->globalRotation(),
22  composedObject->globalPosition() - object->globalPosition() );
23 
24 }
25 
26 //__________________________________________________________________________________________________
27 AlgebraicMatrix66
28 FrameToFrameDerivative::getDerivative(const align::RotationType &objectRot,
29  const align::RotationType &composeRot,
30  const align::GlobalPoint &objectPos,
31  const align::GlobalPoint &composePos) const
32 {
33  return asSMatrix<6,6>(this->getDerivative(objectRot, composeRot, composePos - objectPos));
34 }
35 
36 //__________________________________________________________________________________________________
37 AlgebraicMatrix
38 FrameToFrameDerivative::getDerivative(const align::RotationType &objectRot,
39  const align::RotationType &composeRot,
40  const align::GlobalVector &posVec) const
41 {
42 
43  AlgebraicMatrix rotDet = transform(objectRot);
44  AlgebraicMatrix rotCompO = transform(composeRot);
45 
46  AlgebraicVector diffVec(3);
47 
48  diffVec(1) = posVec.x();
49  diffVec(2) = posVec.y();
50  diffVec(3) = posVec.z();
51 
52  AlgebraicMatrix derivative(6,6);
53 
54  AlgebraicMatrix derivAA(3,3);
55  AlgebraicMatrix derivAB(3,3);
56  AlgebraicMatrix derivBB(3,3);
57 
58  derivAA = derivativePosPos( rotDet, rotCompO );
59  derivAB = derivativePosRot( rotDet, rotCompO, diffVec );
60  derivBB = derivativeRotRot( rotDet, rotCompO );
61 
62  derivative[0][0] = derivAA[0][0];
63  derivative[0][1] = derivAA[0][1];
64  derivative[0][2] = derivAA[0][2];
65  derivative[0][3] = derivAB[0][0];
66  derivative[0][4] = derivAB[0][1];
67  derivative[0][5] = derivAB[0][2];
68  derivative[1][0] = derivAA[1][0];
69  derivative[1][1] = derivAA[1][1];
70  derivative[1][2] = derivAA[1][2];
71  derivative[1][3] = derivAB[1][0];
72  derivative[1][4] = derivAB[1][1];
73  derivative[1][5] = derivAB[1][2];
74  derivative[2][0] = derivAA[2][0];
75  derivative[2][1] = derivAA[2][1];
76  derivative[2][2] = derivAA[2][2];
77  derivative[2][3] = derivAB[2][0];
78  derivative[2][4] = derivAB[2][1];
79  derivative[2][5] = derivAB[2][2];
80  derivative[3][0] = 0;
81  derivative[3][1] = 0;
82  derivative[3][2] = 0;
83  derivative[3][3] = derivBB[0][0];
84  derivative[3][4] = derivBB[0][1];
85  derivative[3][5] = derivBB[0][2];
86  derivative[4][0] = 0;
87  derivative[4][1] = 0;
88  derivative[4][2] = 0;
89  derivative[4][3] = derivBB[1][0];
90  derivative[4][4] = derivBB[1][1];
91  derivative[4][5] = derivBB[1][2];
92  derivative[5][0] = 0;
93  derivative[5][1] = 0;
94  derivative[5][2] = 0;
95  derivative[5][3] = derivBB[2][0];
96  derivative[5][4] = derivBB[2][1];
97  derivative[5][5] = derivBB[2][2];
98 
99  return derivative;
100 }
101 
102 
103 //__________________________________________________________________________________________________
104 AlgebraicMatrix
105 FrameToFrameDerivative::derivativePosPos(const AlgebraicMatrix &RotDet,
106  const AlgebraicMatrix &RotRot) const
107 {
108 
109  return RotDet * RotRot.T();
110 
111 }
112 
113 
114 //__________________________________________________________________________________________________
115 AlgebraicMatrix
116 FrameToFrameDerivative::derivativePosRot(const AlgebraicMatrix &RotDet,
117  const AlgebraicMatrix &RotRot,
118  const AlgebraicVector &S) const
119 {
120 
121  AlgebraicVector dEulerA(3);
122  AlgebraicVector dEulerB(3);
123  AlgebraicVector dEulerC(3);
124  AlgebraicMatrix RotDa(3,3);
125  AlgebraicMatrix RotDb(3,3);
126  AlgebraicMatrix RotDc(3,3);
127 
128  RotDa[1][2] = 1; RotDa[2][1] = -1;
129  RotDb[0][2] = -1; RotDb[2][0] = 1; // New beta sign
130  RotDc[0][1] = 1; RotDc[1][0] = -1;
131 
132  dEulerA = RotDet*( RotRot.T()*RotDa*RotRot*S );
133  dEulerB = RotDet*( RotRot.T()*RotDb*RotRot*S );
134  dEulerC = RotDet*( RotRot.T()*RotDc*RotRot*S );
135 
136  AlgebraicMatrix eulerDeriv(3,3);
137  eulerDeriv[0][0] = dEulerA[0];
138  eulerDeriv[1][0] = dEulerA[1];
139  eulerDeriv[2][0] = dEulerA[2];
140  eulerDeriv[0][1] = dEulerB[0];
141  eulerDeriv[1][1] = dEulerB[1];
142  eulerDeriv[2][1] = dEulerB[2];
143  eulerDeriv[0][2] = dEulerC[0];
144  eulerDeriv[1][2] = dEulerC[1];
145  eulerDeriv[2][2] = dEulerC[2];
146 
147  return eulerDeriv;
148 
149 }
150 
151 
152 //__________________________________________________________________________________________________
153 AlgebraicMatrix
154 FrameToFrameDerivative::derivativeRotRot(const AlgebraicMatrix &RotDet,
155  const AlgebraicMatrix &RotRot) const
156 {
157 
158  AlgebraicVector dEulerA(3);
159  AlgebraicVector dEulerB(3);
160  AlgebraicVector dEulerC(3);
161  AlgebraicMatrix RotDa(3,3);
162  AlgebraicMatrix RotDb(3,3);
163  AlgebraicMatrix RotDc(3,3);
164 
165  RotDa[1][2] = 1; RotDa[2][1] = -1;
166  RotDb[0][2] = -1; RotDb[2][0] = 1; // New beta sign
167  RotDc[0][1] = 1; RotDc[1][0] = -1;
168 
169  dEulerA = linearEulerAngles( RotDet*RotRot.T()*RotDa*RotRot*RotDet.T() );
170  dEulerB = linearEulerAngles( RotDet*RotRot.T()*RotDb*RotRot*RotDet.T() );
171  dEulerC = linearEulerAngles( RotDet*RotRot.T()*RotDc*RotRot*RotDet.T() );
172 
173  AlgebraicMatrix eulerDeriv(3,3);
174 
175  eulerDeriv[0][0] = dEulerA[0];
176  eulerDeriv[1][0] = dEulerA[1];
177  eulerDeriv[2][0] = dEulerA[2];
178  eulerDeriv[0][1] = dEulerB[0];
179  eulerDeriv[1][1] = dEulerB[1];
180  eulerDeriv[2][1] = dEulerB[2];
181  eulerDeriv[0][2] = dEulerC[0];
182  eulerDeriv[1][2] = dEulerC[1];
183  eulerDeriv[2][2] = dEulerC[2];
184 
185  return eulerDeriv;
186 
187 }
188 
189 
190 
191 //__________________________________________________________________________________________________
192 AlgebraicVector
193 FrameToFrameDerivative::linearEulerAngles(const AlgebraicMatrix &rotDelta ) const
194 {
195 
196  AlgebraicMatrix eulerAB(3,3);
197  AlgebraicVector aB(3);
198  eulerAB[0][1] = 1;
199  eulerAB[1][0] = -1; // New beta sign
200  aB[2] = 1;
201 
202  AlgebraicMatrix eulerC(3,3);
203  AlgebraicVector C(3);
204  eulerC[2][0] = 1;
205  C[1] = 1;
206 
207  AlgebraicVector eulerAngles(3);
208  eulerAngles = eulerAB*rotDelta*aB + eulerC*rotDelta*C;
209  return eulerAngles;
210 
211 }
212 
AlgebraicMatrix66
ROOT::Math::SMatrix< double, 6, 6, ROOT::Math::MatRepStd< double, 6, 6 > > AlgebraicMatrix66
Definition: AlgebraicROOTObjects.h:61
PV3DBase::y
T y() const
Definition: PV3DBase.h:63
Alignable::globalRotation
const RotationType & globalRotation() const
Return the global orientation of the object.
Definition: Alignable.h:132
FrameToFrameDerivative::transform
static AlgebraicMatrix transform(const align::RotationType &)
Helper to transform from RotationType to AlgebraicMatrix.
Definition: FrameToFrameDerivative.h:77
FrameToFrameDerivative::getDerivative
AlgebraicMatrix66 getDerivative(const align::RotationType &objectRot, const align::RotationType &composeRot, const align::GlobalPoint &objectPos, const align::GlobalPoint &composePos) const
Definition: FrameToFrameDerivative.cc:28
AlgebraicMatrix
CLHEP::HepMatrix AlgebraicMatrix
Definition: AlgebraicObjects.h:13
PV3DBase::z
T z() const
Definition: PV3DBase.h:64
FrameToFrameDerivative::frameToFrameDerivative
AlgebraicMatrix frameToFrameDerivative(const Alignable *object, const Alignable *composedObject) const
Definition: FrameToFrameDerivative.cc:16
AlgebraicVector
CLHEP::HepVector AlgebraicVector
Definition: AlgebraicObjects.h:12
FrameToFrameDerivative::derivativeRotRot
AlgebraicMatrix derivativeRotRot(const AlgebraicMatrix &RotDet, const AlgebraicMatrix &RotRot) const
Calculates the derivative DRot/DRot.
Definition: FrameToFrameDerivative.cc:154
FrameToFrameDerivative::derivativePosRot
AlgebraicMatrix derivativePosRot(const AlgebraicMatrix &RotDet, const AlgebraicMatrix &RotRot, const AlgebraicVector &S) const
Calculates the derivative DPos/DRot.
Definition: FrameToFrameDerivative.cc:116
Alignable::globalPosition
const PositionType & globalPosition() const
Return the global position of the object.
Definition: Alignable.h:129
PV3DBase::x
T x() const
Definition: PV3DBase.h:62
funct::derivative
A derivative(const A &_)
Definition: Derivative.h:18
FrameToFrameDerivative::derivativePosPos
AlgebraicMatrix derivativePosPos(const AlgebraicMatrix &RotDet, const AlgebraicMatrix &RotRot) const
Calculates the derivative DPos/DPos.
Definition: FrameToFrameDerivative.cc:105
FrameToFrameDerivative::linearEulerAngles
AlgebraicVector linearEulerAngles(const AlgebraicMatrix &rotDelta) const
Gets linear approximated euler Angles.
Definition: FrameToFrameDerivative.cc:193
Generated for CMSSW Reference Manual by doxygen 1.8.5