CommonAlignmentParametrization/src/FrameToFrameDerivative.cc

0001 /** \file FrameToFrameDerivative.cc
0002  *
0003  *  $Date: 2007/03/12 21:28:48 $
0004  *  $Revision: 1.5 $
0005  */
0006
0007 #include "Alignment/CommonAlignmentParametrization/interface/FrameToFrameDerivative.h"
0008
0009 #include "Alignment/CommonAlignment/interface/Alignable.h"
0010
0011 // already in header: #include
0012 // "DataFormats/Math/interface/AlgebraicROOTObjects.h"
0013 #include "DataFormats/CLHEP/interface/Migration.h"
0014
0015 //__________________________________________________________________________________________________
0016 AlgebraicMatrix FrameToFrameDerivative::frameToFrameDerivative(const Alignable *object,
0017                                                                const Alignable *composedObject) const {
0018   return getDerivative(object->globalRotation(),
0019                        composedObject->globalRotation(),
0020                        composedObject->globalPosition() - object->globalPosition());
0021 }
0022
0023 //__________________________________________________________________________________________________
0024 AlgebraicMatrix66 FrameToFrameDerivative::getDerivative(const align::RotationType &objectRot,
0025                                                         const align::RotationType &composeRot,
0026                                                         const align::GlobalPoint &objectPos,
0027                                                         const align::GlobalPoint &composePos) const {
0028   return asSMatrix<6, 6>(this->getDerivative(objectRot, composeRot, composePos - objectPos));
0029 }
0030
0031 //__________________________________________________________________________________________________
0032 AlgebraicMatrix FrameToFrameDerivative::getDerivative(const align::RotationType &objectRot,
0033                                                       const align::RotationType &composeRot,
0034                                                       const align::GlobalVector &posVec) const {
0035   AlgebraicMatrix rotDet = transform(objectRot);
0036   AlgebraicMatrix rotCompO = transform(composeRot);
0037
0038   AlgebraicVector diffVec(3);
0039
0040   diffVec(1) = posVec.x();
0041   diffVec(2) = posVec.y();
0042   diffVec(3) = posVec.z();
0043
0044   AlgebraicMatrix derivative(6, 6);
0045
0046   AlgebraicMatrix derivAA(3, 3);
0047   AlgebraicMatrix derivAB(3, 3);
0048   AlgebraicMatrix derivBB(3, 3);
0049
0050   derivAA = derivativePosPos(rotDet, rotCompO);
0051   derivAB = derivativePosRot(rotDet, rotCompO, diffVec);
0052   derivBB = derivativeRotRot(rotDet, rotCompO);
0053
0054   derivative[0][0] = derivAA[0][0];
0055   derivative[0][1] = derivAA[0][1];
0056   derivative[0][2] = derivAA[0][2];
0057   derivative[0][3] = derivAB[0][0];
0058   derivative[0][4] = derivAB[0][1];
0059   derivative[0][5] = derivAB[0][2];
0060   derivative[1][0] = derivAA[1][0];
0061   derivative[1][1] = derivAA[1][1];
0062   derivative[1][2] = derivAA[1][2];
0063   derivative[1][3] = derivAB[1][0];
0064   derivative[1][4] = derivAB[1][1];
0065   derivative[1][5] = derivAB[1][2];
0066   derivative[2][0] = derivAA[2][0];
0067   derivative[2][1] = derivAA[2][1];
0068   derivative[2][2] = derivAA[2][2];
0069   derivative[2][3] = derivAB[2][0];
0070   derivative[2][4] = derivAB[2][1];
0071   derivative[2][5] = derivAB[2][2];
0072   derivative[3][0] = 0;
0073   derivative[3][1] = 0;
0074   derivative[3][2] = 0;
0075   derivative[3][3] = derivBB[0][0];
0076   derivative[3][4] = derivBB[0][1];
0077   derivative[3][5] = derivBB[0][2];
0078   derivative[4][0] = 0;
0079   derivative[4][1] = 0;
0080   derivative[4][2] = 0;
0081   derivative[4][3] = derivBB[1][0];
0082   derivative[4][4] = derivBB[1][1];
0083   derivative[4][5] = derivBB[1][2];
0084   derivative[5][0] = 0;
0085   derivative[5][1] = 0;
0086   derivative[5][2] = 0;
0087   derivative[5][3] = derivBB[2][0];
0088   derivative[5][4] = derivBB[2][1];
0089   derivative[5][5] = derivBB[2][2];
0090
0091   return derivative;
0092 }
0093
0094 //__________________________________________________________________________________________________
0095 AlgebraicMatrix FrameToFrameDerivative::derivativePosPos(const AlgebraicMatrix &RotDet,
0096                                                          const AlgebraicMatrix &RotRot) const {
0097   return RotDet * RotRot.T();
0098 }
0099
0100 //__________________________________________________________________________________________________
0101 AlgebraicMatrix FrameToFrameDerivative::derivativePosRot(const AlgebraicMatrix &RotDet,
0102                                                          const AlgebraicMatrix &RotRot,
0103                                                          const AlgebraicVector &S) const {
0104   AlgebraicVector dEulerA(3);
0105   AlgebraicVector dEulerB(3);
0106   AlgebraicVector dEulerC(3);
0107   AlgebraicMatrix RotDa(3, 3);
0108   AlgebraicMatrix RotDb(3, 3);
0109   AlgebraicMatrix RotDc(3, 3);
0110
0111   RotDa[1][2] = 1;
0112   RotDa[2][1] = -1;
0113   RotDb[0][2] = -1;
0114   RotDb[2][0] = 1;  // New beta sign
0115   RotDc[0][1] = 1;
0116   RotDc[1][0] = -1;
0117
0118   dEulerA = RotDet * (RotRot.T() * RotDa * RotRot * S);
0119   dEulerB = RotDet * (RotRot.T() * RotDb * RotRot * S);
0120   dEulerC = RotDet * (RotRot.T() * RotDc * RotRot * S);
0121
0122   AlgebraicMatrix eulerDeriv(3, 3);
0123   eulerDeriv[0][0] = dEulerA[0];
0124   eulerDeriv[1][0] = dEulerA[1];
0125   eulerDeriv[2][0] = dEulerA[2];
0126   eulerDeriv[0][1] = dEulerB[0];
0127   eulerDeriv[1][1] = dEulerB[1];
0128   eulerDeriv[2][1] = dEulerB[2];
0129   eulerDeriv[0][2] = dEulerC[0];
0130   eulerDeriv[1][2] = dEulerC[1];
0131   eulerDeriv[2][2] = dEulerC[2];
0132
0133   return eulerDeriv;
0134 }
0135
0136 //__________________________________________________________________________________________________
0137 AlgebraicMatrix FrameToFrameDerivative::derivativeRotRot(const AlgebraicMatrix &RotDet,
0138                                                          const AlgebraicMatrix &RotRot) const {
0139   AlgebraicVector dEulerA(3);
0140   AlgebraicVector dEulerB(3);
0141   AlgebraicVector dEulerC(3);
0142   AlgebraicMatrix RotDa(3, 3);
0143   AlgebraicMatrix RotDb(3, 3);
0144   AlgebraicMatrix RotDc(3, 3);
0145
0146   RotDa[1][2] = 1;
0147   RotDa[2][1] = -1;
0148   RotDb[0][2] = -1;
0149   RotDb[2][0] = 1;  // New beta sign
0150   RotDc[0][1] = 1;
0151   RotDc[1][0] = -1;
0152
0153   dEulerA = linearEulerAngles(RotDet * RotRot.T() * RotDa * RotRot * RotDet.T());
0154   dEulerB = linearEulerAngles(RotDet * RotRot.T() * RotDb * RotRot * RotDet.T());
0155   dEulerC = linearEulerAngles(RotDet * RotRot.T() * RotDc * RotRot * RotDet.T());
0156
0157   AlgebraicMatrix eulerDeriv(3, 3);
0158
0159   eulerDeriv[0][0] = dEulerA[0];
0160   eulerDeriv[1][0] = dEulerA[1];
0161   eulerDeriv[2][0] = dEulerA[2];
0162   eulerDeriv[0][1] = dEulerB[0];
0163   eulerDeriv[1][1] = dEulerB[1];
0164   eulerDeriv[2][1] = dEulerB[2];
0165   eulerDeriv[0][2] = dEulerC[0];
0166   eulerDeriv[1][2] = dEulerC[1];
0167   eulerDeriv[2][2] = dEulerC[2];
0168
0169   return eulerDeriv;
0170 }
0171
0172 //__________________________________________________________________________________________________
0173 AlgebraicVector FrameToFrameDerivative::linearEulerAngles(const AlgebraicMatrix &rotDelta) const {
0174   AlgebraicMatrix eulerAB(3, 3);
0175   AlgebraicVector aB(3);
0176   eulerAB[0][1] = 1;
0177   eulerAB[1][0] = -1;  // New beta sign
0178   aB[2] = 1;
0179
0180   AlgebraicMatrix eulerC(3, 3);
0181   AlgebraicVector C(3);
0182   eulerC[2][0] = 1;
0183   C[1] = 1;
0184
0185   AlgebraicVector eulerAngles(3);
0186   eulerAngles = eulerAB * rotDelta * aB + eulerC * rotDelta * C;
0187   return eulerAngles;
0188 }