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.T());
100 
101 }
102 
103 
104 //__________________________________________________________________________________________________
105 AlgebraicMatrix
106 FrameToFrameDerivative::derivativePosPos(const AlgebraicMatrix &RotDet,
107  const AlgebraicMatrix &RotRot) const
108 {
109 
110  return RotDet * RotRot.T();
111 
112 }
113 
114 
115 //__________________________________________________________________________________________________
116 AlgebraicMatrix
117 FrameToFrameDerivative::derivativePosRot(const AlgebraicMatrix &RotDet,
118  const AlgebraicMatrix &RotRot,
119  const AlgebraicVector &S) const
120 {
121 
122  AlgebraicVector dEulerA(3);
123  AlgebraicVector dEulerB(3);
124  AlgebraicVector dEulerC(3);
125  AlgebraicMatrix RotDa(3,3);
126  AlgebraicMatrix RotDb(3,3);
127  AlgebraicMatrix RotDc(3,3);
128 
129  RotDa[1][2] = 1; RotDa[2][1] = -1;
130  RotDb[0][2] = -1; RotDb[2][0] = 1; // New beta sign
131  RotDc[0][1] = 1; RotDc[1][0] = -1;
132 
133  dEulerA = RotDet*( RotRot.T()*RotDa*RotRot*S );
134  dEulerB = RotDet*( RotRot.T()*RotDb*RotRot*S );
135  dEulerC = RotDet*( RotRot.T()*RotDc*RotRot*S );
136 
137  AlgebraicMatrix eulerDeriv(3,3);
138  eulerDeriv[0][0] = dEulerA[0];
139  eulerDeriv[1][0] = dEulerA[1];
140  eulerDeriv[2][0] = dEulerA[2];
141  eulerDeriv[0][1] = dEulerB[0];
142  eulerDeriv[1][1] = dEulerB[1];
143  eulerDeriv[2][1] = dEulerB[2];
144  eulerDeriv[0][2] = dEulerC[0];
145  eulerDeriv[1][2] = dEulerC[1];
146  eulerDeriv[2][2] = dEulerC[2];
147 
148  return eulerDeriv;
149 
150 }
151 
152 
153 //__________________________________________________________________________________________________
154 AlgebraicMatrix
155 FrameToFrameDerivative::derivativeRotRot(const AlgebraicMatrix &RotDet,
156  const AlgebraicMatrix &RotRot) const
157 {
158 
159  AlgebraicVector dEulerA(3);
160  AlgebraicVector dEulerB(3);
161  AlgebraicVector dEulerC(3);
162  AlgebraicMatrix RotDa(3,3);
163  AlgebraicMatrix RotDb(3,3);
164  AlgebraicMatrix RotDc(3,3);
165 
166  RotDa[1][2] = 1; RotDa[2][1] = -1;
167  RotDb[0][2] = -1; RotDb[2][0] = 1; // New beta sign
168  RotDc[0][1] = 1; RotDc[1][0] = -1;
169 
170  dEulerA = linearEulerAngles( RotDet*RotRot.T()*RotDa*RotRot*RotDet.T() );
171  dEulerB = linearEulerAngles( RotDet*RotRot.T()*RotDb*RotRot*RotDet.T() );
172  dEulerC = linearEulerAngles( RotDet*RotRot.T()*RotDc*RotRot*RotDet.T() );
173 
174  AlgebraicMatrix eulerDeriv(3,3);
175 
176  eulerDeriv[0][0] = dEulerA[0];
177  eulerDeriv[1][0] = dEulerA[1];
178  eulerDeriv[2][0] = dEulerA[2];
179  eulerDeriv[0][1] = dEulerB[0];
180  eulerDeriv[1][1] = dEulerB[1];
181  eulerDeriv[2][1] = dEulerB[2];
182  eulerDeriv[0][2] = dEulerC[0];
183  eulerDeriv[1][2] = dEulerC[1];
184  eulerDeriv[2][2] = dEulerC[2];
185 
186  return eulerDeriv;
187 
188 }
189 
190 
191 
192 //__________________________________________________________________________________________________
193 AlgebraicVector
194 FrameToFrameDerivative::linearEulerAngles(const AlgebraicMatrix &rotDelta ) const
195 {
196 
197  AlgebraicMatrix eulerAB(3,3);
198  AlgebraicVector aB(3);
199  eulerAB[0][1] = 1;
200  eulerAB[1][0] = -1; // New beta sign
201  aB[2] = 1;
202 
203  AlgebraicMatrix eulerC(3,3);
204  AlgebraicVector C(3);
205  eulerC[2][0] = 1;
206  C[1] = 1;
207 
208  AlgebraicVector eulerAngles(3);
209  eulerAngles = eulerAB*rotDelta*aB + eulerC*rotDelta*C;
210  return eulerAngles;
211 
212 }
213 
AlgebraicMatrix66
ROOT::Math::SMatrix< double, 6, 6, ROOT::Math::MatRepStd< double, 6, 6 > > AlgebraicMatrix66
Definition: AlgebraicROOTObjects.h:60
PV3DBase::y
T y() const
Definition: PV3DBase.h:57
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:61
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:58
FrameToFrameDerivative::frameToFrameDerivative
AlgebraicMatrix frameToFrameDerivative(const Alignable *object, const Alignable *composedObject) const
Return the derivative DeltaFrame(object)/DeltaFrame(composedobject)
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:155
FrameToFrameDerivative::derivativePosRot
AlgebraicMatrix derivativePosRot(const AlgebraicMatrix &RotDet, const AlgebraicMatrix &RotRot, const AlgebraicVector &S) const
Calculates the derivative DPos/DRot.
Definition: FrameToFrameDerivative.cc:117
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:56
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:106
FrameToFrameDerivative::linearEulerAngles
AlgebraicVector linearEulerAngles(const AlgebraicMatrix &rotDelta) const
Gets linear approximated euler Angles.
Definition: FrameToFrameDerivative.cc:194
Generated for CMSSW Reference Manual by doxygen 1.8.5