CommonAlignment/src/Utilities.cc

0001 // #include "Math/GenVector/Rotation3D.h"
0002
0003 #include "FWCore/MessageLogger/interface/MessageLogger.h"
0004 #include "FWCore/Utilities/interface/Parse.h"
0005
0006 #include "Alignment/CommonAlignment/interface/Utilities.h"
0007
0008 align::EulerAngles align::toAngles(const RotationType& rot) {
0009   const Scalar one = 1;  // to ensure same precison is used in comparison
0010
0011   EulerAngles angles(3);
0012
0013   if (std::abs(rot.zx()) > one) {
0014     edm::LogWarning("Alignment") << "Rounding errors in\n" << rot;
0015   }
0016
0017   if (std::abs(rot.zx()) < one) {
0018     angles(1) = -std::atan2(rot.zy(), rot.zz());
0019     angles(2) = std::asin(rot.zx());
0020     angles(3) = -std::atan2(rot.yx(), rot.xx());
0021   } else if (rot.zx() >= one) {
0022     angles(1) = std::atan2(rot.xy() + rot.yz(), rot.yy() - rot.xz());
0023     angles(2) = std::asin(one);
0024     angles(3) = 0;
0025   } else if (rot.zx() <= -one) {
0026     angles(1) = std::atan2(rot.xy() - rot.yz(), rot.yy() + rot.xz());
0027     angles(2) = std::asin(-one);
0028     angles(3) = 0;
0029   }
0030
0031   return angles;
0032 }
0033
0034 align::RotationType align::toMatrix(const EulerAngles& angles) {
0035   Scalar s1 = std::sin(angles[0]), c1 = std::cos(angles[0]);
0036   Scalar s2 = std::sin(angles[1]), c2 = std::cos(angles[1]);
0037   Scalar s3 = std::sin(angles[2]), c3 = std::cos(angles[2]);
0038
0039   return RotationType(c2 * c3,
0040                       c1 * s3 + s1 * s2 * c3,
0041                       s1 * s3 - c1 * s2 * c3,
0042                       -c2 * s3,
0043                       c1 * c3 - s1 * s2 * s3,
0044                       s1 * c3 + c1 * s2 * s3,
0045                       s2,
0046                       -s1 * c2,
0047                       c1 * c2);
0048 }
0049
0050 align::PositionType align::motherPosition(const std::vector<const PositionType*>& dauPos) {
0051   unsigned int nDau = dauPos.size();
0052
0053   Scalar posX(0.), posY(0.), posZ(0.);  // position of mother
0054
0055   for (unsigned int i = 0; i < nDau; ++i) {
0056     const PositionType* point = dauPos[i];
0057
0058     posX += point->x();
0059     posY += point->y();
0060     posZ += point->z();
0061   }
0062
0063   Scalar inv = 1. / static_cast<Scalar>(nDau);
0064
0065   return PositionType(posX * inv, posY * inv, posZ * inv);
0066 }
0067
0068 align::RotationType align::diffRot(const GlobalVectors& current, const GlobalVectors& nominal) {
0069   // Find the matrix needed to rotate the nominal surface to the current one
0070   // using small angle approximation through the equation:
0071   //
0072   //   I * dOmega = dr * r (sum over points)
0073   //
0074   // where dOmega is a vector of small rotation angles about (x, y, z)-axes,
0075   //   and I is the inertia tensor defined as
0076   //
0077   //   I_ij = delta_ij * r^2 - r_i * r_j (sum over points)
0078   //
0079   // delta_ij is the identity matrix. i, j are indices for (x, y, z).
0080   //
0081   // On the rhs of the first eq, r * dr is the cross product of r and dr.
0082   // In this case, r is the nominal vector and dr is the displacement of the
0083   // current point from its nominal point (current vector - nominal vector).
0084   //
0085   // Since the solution of dOmega (by inverting I) gives angles that are small,
0086   // we rotate the current surface by -dOmega and repeat the process until the
0087   // dOmega^2 is less than a certain tolerance value.
0088   // (In other words, we move the current surface by small angular steps till
0089   // it matches the nominal surface.)
0090   // The full rotation is found by adding up the rotations (given by dOmega)
0091   // in each step. (More precisely, the product of all the matrices.)
0092   //
0093   // Note that, in some cases, if the angular displacement between current and
0094   // nominal is pi, the algorithm can return an identity (no rotation).
0095   // This is because dr = -r and r * dr is all zero.
0096   // This is not a problem since we are dealing with small angles in alignment.
0097
0098   static const double tolerance = 1e-12;
0099
0100   RotationType rot;  // rotation from nominal to current; init to identity
0101
0102   // Initial values for dr and I; I is always the same in each step
0103
0104   AlgebraicSymMatrix I(3);  // inertia tensor
0105
0106   GlobalVectors rotated = current;  // rotated current vectors in each step
0107
0108   unsigned int nPoints = nominal.size();
0109
0110   for (unsigned int j = 0; j < nPoints; ++j) {
0111     const GlobalVector& r = nominal[j];
0112     // Inertial tensor: I_ij = delta_ij * r^2 - r_i * r_j (sum over points)
0113
0114     I.fast(1, 1) += r.y() * r.y() + r.z() * r.z();
0115     I.fast(2, 2) += r.x() * r.x() + r.z() * r.z();
0116     I.fast(3, 3) += r.y() * r.y() + r.x() * r.x();
0117     I.fast(2, 1) -= r.x() * r.y();  // row index must be >= col index
0118     I.fast(3, 1) -= r.x() * r.z();
0119     I.fast(3, 2) -= r.y() * r.z();
0120   }
0121   int count = 0;
0122   while (true) {
0123     AlgebraicVector rhs(3);  // sum of dr * r
0124
0125     for (unsigned int j = 0; j < nPoints; ++j) {
0126       const GlobalVector& r = nominal[j];
0127       const GlobalVector& c = rotated[j];
0128
0129       // Cross product of dr * r = c * r (sum over points)
0130
0131       rhs(1) += c.y() * r.z() - c.z() * r.y();
0132       rhs(2) += c.z() * r.x() - c.x() * r.z();
0133       rhs(3) += c.x() * r.y() - c.y() * r.x();
0134     }
0135
0136     EulerAngles dOmega = CLHEP::solve(I, rhs);
0137
0138     rot *= toMatrix(dOmega);  // add to rotation
0139
0140     if (dOmega.normsq() < tolerance)
0141       break;  // converges, so exit loop
0142     count++;
0143     if (count > 100000) {
0144       std::cout << "diffRot infinite loop: dOmega is " << dOmega.normsq() << "\n";
0145       break;
0146     }
0147
0148     // Not yet converge; move current vectors to new positions and find dr
0149
0150     for (unsigned int j = 0; j < nPoints; ++j) {
0151       rotated[j] = GlobalVector(rot.multiplyInverse(current[j].basicVector()));
0152     }
0153   }
0154
0155   return rot;
0156 }
0157
0158 align::GlobalVector align::diffR(const GlobalVectors& current, const GlobalVectors& nominal) {
0159   GlobalVector nCM(0, 0, 0);
0160   GlobalVector cCM(0, 0, 0);
0161
0162   unsigned int nPoints = nominal.size();
0163
0164   for (unsigned int j = 0; j < nPoints; ++j) {
0165     nCM += nominal[j];
0166     cCM += current[j];
0167   }
0168
0169   nCM -= cCM;
0170
0171   return nCM /= static_cast<Scalar>(nPoints);
0172 }
0173
0174 align::GlobalVector align::centerOfMass(const GlobalVectors& theVs) {
0175   unsigned int nPoints = theVs.size();
0176
0177   GlobalVector CM(0, 0, 0);
0178
0179   for (unsigned int j = 0; j < nPoints; ++j)
0180     CM += theVs[j];
0181
0182   return CM /= static_cast<Scalar>(nPoints);
0183 }
0184
0185 void align::rectify(RotationType& rot) {
0186   // Use ROOT for better numerical precision but slower.
0187
0188   //   ROOT::Math::Rotation3D temp( rot.xx(), rot.xy(), rot.xz(),
0189   //                                rot.yx(), rot.yy(), rot.yz(),
0190   //                                rot.zx(), rot.zy(), rot.zz() );
0191   //
0192   //   temp.Rectify();
0193   //
0194   //   Scalar elems[9];
0195   //
0196   //   temp.GetComponents(elems);
0197   //   rot = RotationType(elems);
0198
0199   rot = toMatrix(toAngles(rot));  // fast rectification but less precise
0200 }
0201
0202 align::RunRanges align::makeNonOverlappingRunRanges(const edm::VParameterSet& runRanges, const RunNumber& defaultRun) {
0203   const auto beginValue = cond::timeTypeSpecs[cond::runnumber].beginValue;
0204   const auto endValue = cond::timeTypeSpecs[cond::runnumber].endValue;
0205   RunRanges uniqueRunRanges;
0206   if (!runRanges.empty()) {
0207     std::map<RunNumber, RunNumber> uniqueFirstRunNumbers;
0208     for (const auto& ipset : runRanges) {
0209       const auto RunRangeStrings = ipset.getParameter<std::vector<std::string> >("RunRanges");
0210       for (const auto& irange : RunRangeStrings) {
0211         if (irange.find(':') == std::string::npos) {
0212           long int temp{strtol(irange.c_str(), nullptr, 0)};
0213           auto first = (temp != -1) ? temp : beginValue;
0214           uniqueFirstRunNumbers[first] = first;
0215         } else {
0216           edm::LogWarning("BadConfig") << "@SUB=align::makeNonOverlappingRunRanges"
0217                                        << "Config file contains old format for 'RunRangeSelection'. Only "
0218                                        << "the start run number is used internally. The number of the last"
0219                                        << " run is ignored and can besafely removed from the config file.";
0220           auto tokens = edm::tokenize(irange, ":");
0221           long int temp{strtol(tokens[0].c_str(), nullptr, 0)};
0222           auto first = (temp != -1) ? temp : beginValue;
0223           uniqueFirstRunNumbers[first] = first;
0224         }
0225       }
0226     }
0227
0228     for (const auto& iFirst : uniqueFirstRunNumbers) {
0229       uniqueRunRanges.push_back(std::make_pair(iFirst.first, endValue));
0230     }
0231     for (unsigned int i = 0; i < uniqueRunRanges.size() - 1; ++i) {
0232       uniqueRunRanges[i].second = uniqueRunRanges[i + 1].first - 1;
0233     }
0234   } else {
0235     uniqueRunRanges.push_back(std::make_pair(defaultRun, endValue));
0236   }
0237
0238   return uniqueRunRanges;
0239 }
0240
0241 align::RunRanges align::makeUniqueRunRanges(const edm::VParameterSet& runRanges, const RunNumber& defaultRun) {
0242   auto uniqueRunRanges = align::makeNonOverlappingRunRanges(runRanges, defaultRun);
0243   if (uniqueRunRanges.empty()) {  // create dummy IOV
0244     const RunRange runRange(defaultRun, cond::timeTypeSpecs[cond::runnumber].endValue);
0245     uniqueRunRanges.push_back(runRange);
0246   }
0247   return uniqueRunRanges;
0248 }