CMS 3D CMS Logo

Utilities.cc
Go to the documentation of this file.
1 // #include "Math/GenVector/Rotation3D.h"
2 
5 
7 
8 
10 {
11  const Scalar one = 1; // to ensure same precison is used in comparison
12 
14 
15  if (std::abs( rot.zx() ) > one)
16  {
17  edm::LogWarning("Alignment") << "Rounding errors in\n" << rot;
18  }
19 
20  if (std::abs( rot.zx() ) < one)
21  {
22  angles(1) = -std::atan2( rot.zy(), rot.zz() );
23  angles(2) = std::asin( rot.zx() );
24  angles(3) = -std::atan2( rot.yx(), rot.xx() );
25  }
26  else if (rot.zx() >= one)
27  {
28  angles(1) = std::atan2(rot.xy() + rot.yz(), rot.yy() - rot.xz() );
29  angles(2) = std::asin(one);
30  angles(3) = 0;
31  }
32  else if (rot.zx() <= -one)
33  {
34  angles(1) = std::atan2(rot.xy() - rot.yz(), rot.yy() + rot.xz() );
35  angles(2) = std::asin(-one);
36  angles(3) = 0;
37  }
38 
39  return angles;
40 }
41 
43 {
44  Scalar s1 = std::sin(angles[0]), c1 = std::cos(angles[0]);
45  Scalar s2 = std::sin(angles[1]), c2 = std::cos(angles[1]);
46  Scalar s3 = std::sin(angles[2]), c3 = std::cos(angles[2]);
47 
48  return RotationType( c2 * c3, c1 * s3 + s1 * s2 * c3, s1 * s3 - c1 * s2 * c3,
49  -c2 * s3, c1 * c3 - s1 * s2 * s3, s1 * c3 + c1 * s2 * s3,
50  s2, -s1 * c2, c1 * c2);
51 }
52 
53 align::PositionType align::motherPosition(const std::vector<const PositionType*>& dauPos)
54 {
55  unsigned int nDau = dauPos.size();
56 
57  Scalar posX(0.), posY(0.), posZ(0.); // position of mother
58 
59  for (unsigned int i = 0; i < nDau; ++i)
60  {
61  const PositionType* point = dauPos[i];
62 
63  posX += point->x();
64  posY += point->y();
65  posZ += point->z();
66  }
67 
68  Scalar inv = 1. / static_cast<Scalar>(nDau);
69 
70  return PositionType(posX *= inv, posY *= inv, posZ *= inv);
71 }
72 
74  const GlobalVectors& nominal)
75 {
76 // Find the matrix needed to rotate the nominal surface to the current one
77 // using small angle approximation through the equation:
78 //
79 // I * dOmega = dr * r (sum over points)
80 //
81 // where dOmega is a vector of small rotation angles about (x, y, z)-axes,
82 // and I is the inertia tensor defined as
83 //
84 // I_ij = delta_ij * r^2 - r_i * r_j (sum over points)
85 //
86 // delta_ij is the identity matrix. i, j are indices for (x, y, z).
87 //
88 // On the rhs of the first eq, r * dr is the cross product of r and dr.
89 // In this case, r is the nominal vector and dr is the displacement of the
90 // current point from its nominal point (current vector - nominal vector).
91 //
92 // Since the solution of dOmega (by inverting I) gives angles that are small,
93 // we rotate the current surface by -dOmega and repeat the process until the
94 // dOmega^2 is less than a certain tolerance value.
95 // (In other words, we move the current surface by small angular steps till
96 // it matches the nominal surface.)
97 // The full rotation is found by adding up the rotations (given by dOmega)
98 // in each step. (More precisely, the product of all the matrices.)
99 //
100 // Note that, in some cases, if the angular displacement between current and
101 // nominal is pi, the algorithm can return an identity (no rotation).
102 // This is because dr = -r and r * dr is all zero.
103 // This is not a problem since we are dealing with small angles in alignment.
104 
105  static const double tolerance = 1e-12;
106 
107  RotationType rot; // rotation from nominal to current; init to identity
108 
109 // Initial values for dr and I; I is always the same in each step
110 
111  AlgebraicSymMatrix I(3); // inertia tensor
112 
113  GlobalVectors rotated = current; // rotated current vectors in each step
114 
115  unsigned int nPoints = nominal.size();
116 
117  for (unsigned int j = 0; j < nPoints; ++j)
118  {
119  const GlobalVector& r = nominal[j];
120  // Inertial tensor: I_ij = delta_ij * r^2 - r_i * r_j (sum over points)
121 
122  I.fast(1, 1) += r.y() * r.y() + r.z() * r.z();
123  I.fast(2, 2) += r.x() * r.x() + r.z() * r.z();
124  I.fast(3, 3) += r.y() * r.y() + r.x() * r.x();
125  I.fast(2, 1) -= r.x() * r.y(); // row index must be >= col index
126  I.fast(3, 1) -= r.x() * r.z();
127  I.fast(3, 2) -= r.y() * r.z();
128  }
129  int count=0;
130  while (true)
131  {
132  AlgebraicVector rhs(3); // sum of dr * r
133 
134  for (unsigned int j = 0; j < nPoints; ++j)
135  {
136  const GlobalVector& r = nominal[j];
137  const GlobalVector& c = rotated[j];
138 
139  // Cross product of dr * r = c * r (sum over points)
140 
141  rhs(1) += c.y() * r.z() - c.z() * r.y();
142  rhs(2) += c.z() * r.x() - c.x() * r.z();
143  rhs(3) += c.x() * r.y() - c.y() * r.x();
144  }
145 
146  EulerAngles dOmega = CLHEP::solve(I, rhs);
147 
148  rot *= toMatrix(dOmega); // add to rotation
149 
150  if (dOmega.normsq() < tolerance) break; // converges, so exit loop
151  count++;
152  if(count>100000){
153  std::cout<<"diffRot infinite loop: dOmega is "<<dOmega.normsq()<<"\n";
154  break;
155  }
156 
157  // Not yet converge; move current vectors to new positions and find dr
158 
159  for (unsigned int j = 0; j < nPoints; ++j)
160  {
161  rotated[j] = GlobalVector( rot.multiplyInverse( current[j].basicVector() ) );
162  }
163  }
164 
165  return rot;
166 }
167 
169  const GlobalVectors& nominal)
170 {
171  GlobalVector nCM(0,0,0);
172  GlobalVector cCM(0,0,0);
173 
174  unsigned int nPoints = nominal.size();
175 
176  for (unsigned int j = 0; j < nPoints; ++j)
177  {
178  nCM += nominal[j];
179  cCM += current[j];
180  }
181 
182  nCM -= cCM;
183 
184  return nCM /= static_cast<Scalar>(nPoints);
185 }
186 
188 {
189  unsigned int nPoints = theVs.size();
190 
191  GlobalVector CM(0,0,0);
192 
193  for (unsigned int j = 0; j < nPoints; ++j) CM += theVs[j];
194 
195  return CM /= static_cast<Scalar>(nPoints);
196 }
197 
199 {
200 // Use ROOT for better numerical precision but slower.
201 
202 // ROOT::Math::Rotation3D temp( rot.xx(), rot.xy(), rot.xz(),
203 // rot.yx(), rot.yy(), rot.yz(),
204 // rot.zx(), rot.zy(), rot.zz() );
205 //
206 // temp.Rectify();
207 //
208 // Scalar elems[9];
209 //
210 // temp.GetComponents(elems);
211 // rot = RotationType(elems);
212 
213  rot = toMatrix( toAngles(rot) ); // fast rectification but less precise
214 }
215 
216 
219  const RunNumber& defaultRun)
220 {
221  const auto beginValue = cond::timeTypeSpecs[cond::runnumber].beginValue;
222  const auto endValue = cond::timeTypeSpecs[cond::runnumber].endValue;
223  RunRanges uniqueRunRanges;
224  if (!runRanges.empty()) {
225  std::map<RunNumber,RunNumber> uniqueFirstRunNumbers;
226  for (const auto& ipset: runRanges) {
227  const auto RunRangeStrings =
228  ipset.getParameter<std::vector<std::string> >("RunRanges");
229  for (const auto& irange: RunRangeStrings) {
230  if (irange.find(':')==std::string::npos) {
231  long int temp{strtol(irange.c_str(), nullptr, 0)};
232  auto first = (temp != -1) ? temp : beginValue;
233  uniqueFirstRunNumbers[first] = first;
234  } else {
235  edm::LogWarning("BadConfig")
236  << "@SUB=align::makeNonOverlappingRunRanges"
237  << "Config file contains old format for 'RunRangeSelection'. Only "
238  << "the start run number is used internally. The number of the last"
239  << " run is ignored and can besafely removed from the config file.";
240  auto tokens = edm::tokenize(irange, ":");
241  long int temp{strtol(tokens[0].c_str(), nullptr, 0)};
242  auto first = (temp != -1) ? temp : beginValue;
243  uniqueFirstRunNumbers[first] = first;
244  }
245  }
246  }
247 
248  for (const auto& iFirst: uniqueFirstRunNumbers) {
249  uniqueRunRanges.push_back(std::make_pair(iFirst.first, endValue));
250  }
251  for (unsigned int i = 0;i<uniqueRunRanges.size()-1;++i) {
252  uniqueRunRanges[i].second = uniqueRunRanges[i+1].first - 1;
253  }
254  } else {
255  uniqueRunRanges.push_back(std::make_pair(defaultRun, endValue));
256  }
257 
258  return uniqueRunRanges;
259 }
260 
261 
264  const RunNumber& defaultRun) {
265  auto uniqueRunRanges =
266  align::makeNonOverlappingRunRanges(runRanges, defaultRun);
267  if (uniqueRunRanges.empty()) { // create dummy IOV
268  const RunRange runRange(defaultRun,
270  uniqueRunRanges.push_back(runRange);
271  }
272  return uniqueRunRanges;
273 }
T xx() const
const TimeTypeSpecs timeTypeSpecs[]
Definition: Time.cc:22
TkRotation< Scalar > RotationType
Definition: Definitions.h:29
double Scalar
Definition: Definitions.h:27
Time_t beginValue
Definition: Time.h:45
const double tolerance
std::vector< ParameterSet > VParameterSet
Definition: ParameterSet.h:33
std::vector< RunRange > RunRanges
Definition: Utilities.h:40
Sin< T >::type sin(const T &t)
Definition: Sin.h:22
T y() const
Definition: PV3DBase.h:63
T yx() const
GlobalVector diffR(const GlobalVectors &current, const GlobalVectors &nominal)
Definition: Utilities.cc:168
RotationType diffRot(const GlobalVectors &current, const GlobalVectors &nominal)
Definition: Utilities.cc:73
Point3DBase< Scalar, GlobalTag > PositionType
Definition: Definitions.h:30
T zx() const
T xy() const
T zz() const
void rectify(RotationType &)
Correct a rotation matrix for rounding errors.
Definition: Utilities.cc:198
RunRanges makeNonOverlappingRunRanges(const edm::VParameterSet &runRanges, const RunNumber &defaultRun)
Definition: Utilities.cc:218
RunRanges makeUniqueRunRanges(const edm::VParameterSet &runRanges, const RunNumber &defaultRun)
Definition: Utilities.cc:263
T z() const
Definition: PV3DBase.h:64
Cos< T >::type cos(const T &t)
Definition: Cos.h:22
T zy() const
Abs< T >::type abs(const T &t)
Definition: Abs.h:22
const std::complex< double > I
Definition: I.h:8
EulerAngles toAngles(const RotationType &)
Convert rotation matrix to angles about x-, y-, z-axes (frame rotation).
Definition: Utilities.cc:9
T yy() const
GlobalVector centerOfMass(const GlobalVectors &theVs)
Find the CM of a set of points.
Definition: Utilities.cc:187
std::pair< RunNumber, RunNumber > RunRange
Definition: Utilities.h:39
CLHEP::HepVector AlgebraicVector
AlgebraicVector EulerAngles
Definition: Definitions.h:36
std::vector< GlobalVector > GlobalVectors
Definition: Utilities.h:29
T xz() const
RotationType toMatrix(const EulerAngles &)
Convert rotation angles about x-, y-, z-axes to matrix.
Definition: Utilities.cc:42
CLHEP::HepSymMatrix AlgebraicSymMatrix
PositionType motherPosition(const std::vector< const PositionType * > &dauPos)
Find mother&#39;s position from the average of its daughters&#39; positions.
Definition: Utilities.cc:53
Time_t endValue
Definition: Time.h:46
Basic3DVector< T > multiplyInverse(const Basic3DVector< T > &v) const
T x() const
Definition: PV3DBase.h:62
T yz() const
*vegas h *****************************************************used in the default bin number in original ***version of VEGAS is ***a higher bin number might help to derive a more precise ***grade subtle point
Definition: invegas.h:5
Global3DVector GlobalVector
Definition: GlobalVector.h:10
cond::RealTimeType< cond::runnumber >::type RunNumber
Definition: Utilities.h:38
std::vector< std::string > tokenize(std::string const &input, std::string const &separator)
breaks the input string into tokens, delimited by the separator