Go to the documentation of this file.00001
00002
00003 #include "FWCore/MessageLogger/interface/MessageLogger.h"
00004
00005 #include "Alignment/CommonAlignment/interface/Utilities.h"
00006
00007 align::EulerAngles align::toAngles(const RotationType& rot)
00008 {
00009 const Scalar one = 1;
00010
00011 EulerAngles angles(3);
00012
00013 if (std::abs( rot.zx() ) > one)
00014 {
00015 edm::LogWarning("Alignment") << "Rounding errors in\n" << rot;
00016 }
00017
00018 if (std::abs( rot.zx() ) < one)
00019 {
00020 angles(1) = -std::atan2( rot.zy(), rot.zz() );
00021 angles(2) = std::asin( rot.zx() );
00022 angles(3) = -std::atan2( rot.yx(), rot.xx() );
00023 }
00024 else if (rot.zx() >= one)
00025 {
00026 angles(1) = std::atan2(rot.xy() + rot.yz(), rot.yy() - rot.xz() );
00027 angles(2) = std::asin(one);
00028 angles(3) = 0;
00029 }
00030 else if (rot.zx() <= -one)
00031 {
00032 angles(1) = std::atan2(rot.xy() - rot.yz(), rot.yy() + rot.xz() );
00033 angles(2) = std::asin(-one);
00034 angles(3) = 0;
00035 }
00036
00037 return angles;
00038 }
00039
00040 align::RotationType align::toMatrix(const EulerAngles& angles)
00041 {
00042 Scalar s1 = std::sin(angles[0]), c1 = std::cos(angles[0]);
00043 Scalar s2 = std::sin(angles[1]), c2 = std::cos(angles[1]);
00044 Scalar s3 = std::sin(angles[2]), c3 = std::cos(angles[2]);
00045
00046 return RotationType( c2 * c3, c1 * s3 + s1 * s2 * c3, s1 * s3 - c1 * s2 * c3,
00047 -c2 * s3, c1 * c3 - s1 * s2 * s3, s1 * c3 + c1 * s2 * s3,
00048 s2, -s1 * c2, c1 * c2);
00049 }
00050
00051 align::PositionType align::motherPosition(const std::vector<const PositionType*>& dauPos)
00052 {
00053 unsigned int nDau = dauPos.size();
00054
00055 Scalar posX(0.), posY(0.), posZ(0.);
00056
00057 for (unsigned int i = 0; i < nDau; ++i)
00058 {
00059 const PositionType* point = dauPos[i];
00060
00061 posX += point->x();
00062 posY += point->y();
00063 posZ += point->z();
00064 }
00065
00066 Scalar inv = 1. / static_cast<Scalar>(nDau);
00067
00068 return PositionType(posX *= inv, posY *= inv, posZ *= inv);
00069 }
00070
00071 align::RotationType align::diffRot(const GlobalVectors& current,
00072 const GlobalVectors& nominal)
00073 {
00074
00075
00076
00077
00078
00079
00080
00081
00082
00083
00084
00085
00086
00087
00088
00089
00090
00091
00092
00093
00094
00095
00096
00097
00098
00099
00100
00101
00102
00103 static const double tolerance = 1e-12;
00104
00105 RotationType rot;
00106
00107
00108
00109 AlgebraicSymMatrix I(3);
00110
00111 GlobalVectors rotated = current;
00112
00113 unsigned int nPoints = nominal.size();
00114
00115 for (unsigned int j = 0; j < nPoints; ++j)
00116 {
00117 const GlobalVector& r = nominal[j];
00118
00119
00120 I.fast(1, 1) += r.y() * r.y() + r.z() * r.z();
00121 I.fast(2, 2) += r.x() * r.x() + r.z() * r.z();
00122 I.fast(3, 3) += r.y() * r.y() + r.x() * r.x();
00123 I.fast(2, 1) -= r.x() * r.y();
00124 I.fast(3, 1) -= r.x() * r.z();
00125 I.fast(3, 2) -= r.y() * r.z();
00126 }
00127 int count=0;
00128 while (true)
00129 {
00130 AlgebraicVector rhs(3);
00131
00132 for (unsigned int j = 0; j < nPoints; ++j)
00133 {
00134 const GlobalVector& r = nominal[j];
00135 const GlobalVector& c = rotated[j];
00136
00137
00138
00139 rhs(1) += c.y() * r.z() - c.z() * r.y();
00140 rhs(2) += c.z() * r.x() - c.x() * r.z();
00141 rhs(3) += c.x() * r.y() - c.y() * r.x();
00142 }
00143
00144 EulerAngles dOmega = CLHEP::solve(I, rhs);
00145
00146 rot *= toMatrix(dOmega);
00147
00148 if (dOmega.normsq() < tolerance) break;
00149 count++;
00150 if(count>100000){
00151 std::cout<<"diffRot infinite loop: dOmega is "<<dOmega.normsq()<<"\n";
00152 break;
00153 }
00154
00155
00156
00157 for (unsigned int j = 0; j < nPoints; ++j)
00158 {
00159 rotated[j] = GlobalVector( rot.multiplyInverse( current[j].basicVector() ) );
00160 }
00161 }
00162
00163 return rot;
00164 }
00165
00166 align::GlobalVector align::diffR(const GlobalVectors& current,
00167 const GlobalVectors& nominal)
00168 {
00169 GlobalVector nCM(0,0,0);
00170 GlobalVector cCM(0,0,0);
00171
00172 unsigned int nPoints = nominal.size();
00173
00174 for (unsigned int j = 0; j < nPoints; ++j)
00175 {
00176 nCM += nominal[j];
00177 cCM += current[j];
00178 }
00179
00180 nCM -= cCM;
00181
00182 return nCM /= static_cast<Scalar>(nPoints);
00183 }
00184
00185 align::GlobalVector align::centerOfMass(const GlobalVectors& theVs)
00186 {
00187 unsigned int nPoints = theVs.size();
00188
00189 GlobalVector CM(0,0,0);
00190
00191 for (unsigned int j = 0; j < nPoints; ++j) CM += theVs[j];
00192
00193 return CM /= static_cast<Scalar>(nPoints);
00194 }
00195
00196 void align::rectify(RotationType& rot)
00197 {
00198
00199
00200
00201
00202
00203
00204
00205
00206
00207
00208
00209
00210
00211 rot = toMatrix( toAngles(rot) );
00212 }