CMS 3D CMS Logo

/data/doxygen/doxygen-1.7.3/gen/CMSSW_4_2_8/src/Alignment/CommonAlignment/src/Utilities.cc

Go to the documentation of this file.
00001 // #include "Math/GenVector/Rotation3D.h"
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; // to ensure same precison is used in comparison
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.); // position of mother
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 // Find the matrix needed to rotate the nominal surface to the current one
00075 // using small angle approximation through the equation:
00076 //
00077 //   I * dOmega = dr * r (sum over points)
00078 //
00079 // where dOmega is a vector of small rotation angles about (x, y, z)-axes,
00080 //   and I is the inertia tensor defined as
00081 //
00082 //   I_ij = delta_ij * r^2 - r_i * r_j (sum over points)
00083 //
00084 // delta_ij is the identity matrix. i, j are indices for (x, y, z).
00085 //
00086 // On the rhs of the first eq, r * dr is the cross product of r and dr.
00087 // In this case, r is the nominal vector and dr is the displacement of the
00088 // current point from its nominal point (current vector - nominal vector).
00089 // 
00090 // Since the solution of dOmega (by inverting I) gives angles that are small,
00091 // we rotate the current surface by -dOmega and repeat the process until the
00092 // dOmega^2 is less than a certain tolerance value.
00093 // (In other words, we move the current surface by small angular steps till
00094 // it matches the nominal surface.)
00095 // The full rotation is found by adding up the rotations (given by dOmega)
00096 // in each step. (More precisely, the product of all the matrices.)
00097 //
00098 // Note that, in some cases, if the angular displacement between current and
00099 // nominal is pi, the algorithm can return an identity (no rotation).
00100 // This is because dr = -r and r * dr is all zero.
00101 // This is not a problem since we are dealing with small angles in alignment.
00102 
00103   static const double tolerance = 1e-12;
00104 
00105   RotationType rot; // rotation from nominal to current; init to identity
00106 
00107 // Initial values for dr and I; I is always the same in each step
00108 
00109   AlgebraicSymMatrix I(3); // inertia tensor
00110 
00111   GlobalVectors rotated = current; // rotated current vectors in each step
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   // Inertial tensor: I_ij = delta_ij * r^2 - r_i * r_j (sum over points)
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(); // row index must be >= col index
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); // sum of dr * r
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     // Cross product of dr * r = c * r (sum over points)
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); // add to rotation
00147 
00148     if (dOmega.normsq() < tolerance) break; // converges, so exit loop
00149     count++;
00150     if(count>100000){
00151        std::cout<<"diffRot infinite loop: dOmega is "<<dOmega.normsq()<<"\n";
00152        break;
00153     }
00154 
00155   // Not yet converge; move current vectors to new positions and find dr
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 // Use ROOT for better numerical precision but slower.
00199 
00200 //   ROOT::Math::Rotation3D temp( rot.xx(), rot.xy(), rot.xz(),
00201 //                                rot.yx(), rot.yy(), rot.yz(),
00202 //                                rot.zx(), rot.zy(), rot.zz() );
00203 // 
00204 //   temp.Rectify();
00205 // 
00206 //   Scalar elems[9];
00207 // 
00208 //   temp.GetComponents(elems);
00209 //   rot = RotationType(elems);
00210 
00211   rot = toMatrix( toAngles(rot) ); // fast rectification but less precise
00212 }