ATLAS Offline Software
VP1LinAlgUtils.cxx
Go to the documentation of this file.
1 /*
2  Copyright (C) 2002-2017 CERN for the benefit of the ATLAS collaboration
3 */
4 
5 
7 // //
8 // Implementation of class VP1LinAlgUtils //
9 // //
10 // Author: Thomas H. Kittelmann (Thomas.Kittelmann@cern.ch) //
11 // Initial version: March 2008 //
12 // //
14 
15 #include "VP1Utils/VP1LinAlgUtils.h"
16 #include "VP1Base/VP1Msg.h"
17 
18 #include <Inventor/C/errors/debugerror.h>
19 #include <Inventor/nodes/SoTransform.h>
20 
21 
22 #include <math.h>
23 
24 
25 class VP1LinAlgUtils::Imp {
26 public:
27  static double atlasR;
28  static bool isBad( const double& x, const QString& ctxStr, const double& limit = 1.0e20 )
29  {
30  if (x==x && fabs(x) < limit)
31  return false;
32  if (!ctxStr.isEmpty())
33  VP1Msg::messageDebug("VP1LinAlgUtils ("+ctxStr+") ERROR: Saw suspicious number "+VP1Msg::str(x));
34  return true;
35  }
36 };
37 double VP1LinAlgUtils::Imp::atlasR = 1.0*CLHEP::km;
38 
39 //______________________________________________________________________________________
40 SoTransform * VP1LinAlgUtils::toSoTransform(const HepGeom::Transform3D & transformation, SoTransform * tin)
41 {
42  HepGeom::Vector3D<double> translation = transformation.getTranslation();
43  CLHEP::HepRotation rotation = transformation.getRotation();
44  SoTransform *myTransform = tin ? tin : new SoTransform();
45  myTransform->translation = SbVec3f(translation.x(),
46  translation.y(),
47  translation.z());
48  myTransform->rotation = SbRotation(SbMatrix(rotation.xx(),rotation.yx(),rotation.zx(),0,
49  rotation.xy(),rotation.yy(),rotation.zy(),0,
50  rotation.xz(),rotation.yz(),rotation.zz(),0,
51  0,0,0,1));
52  if (VP1Msg::verbose()) {
53  bool inputok = isSane( transformation, "input HepGeom::Transform3D" );
54  bool outputok = isSane( myTransform, "output SoTransform" );
55  if (!inputok)
56  VP1Msg::messageVerbose("VP1LinAlgUtils::toSoTransform ERROR: Problems observed in input HepGeom::Transform3D");
57  if (!outputok)
58  VP1Msg::messageVerbose("VP1LinAlgUtils::toSoTransform ERROR: Problems observed in output SoTransform");
59  if (inputok!=outputok)
60  VP1Msg::messageVerbose("VP1LinAlgUtils::toSoTransform Error introduced in HepGeom::Transform3D -> SoTransform conversion!");
61  }
62 
63  return myTransform;
64 }
65 
66 //______________________________________________________________________________________
67 SoTransform * VP1LinAlgUtils::toSoTransform(const Amg::Transform3D & transformation, SoTransform * tin)
68 {
69  Amg::Vector3D translation = transformation.translation();
70  Amg::RotationMatrix3D rotation = transformation.rotation();
71 
72  SoTransform *myTransform = tin ? tin : new SoTransform();
73  myTransform->translation = SbVec3f(translation.x(),
74  translation.y(),
75  translation.z());
76 // myTransform->rotation = SbRotation(SbMatrix(rotation.xx(),rotation.yx(),rotation.zx(),0,
77 // rotation.xy(),rotation.yy(),rotation.zy(),0,
78 // rotation.xz(),rotation.yz(),rotation.zz(),0,
79 // 0,0,0,1));
80  myTransform->rotation = SbRotation(SbMatrix(rotation(0,0),rotation(1,0),rotation(2,0),0,
81  rotation(0,1),rotation(1,1),rotation(2,1),0,
82  rotation(0,2),rotation(1,2),rotation(2,2),0,
83  0,0,0,1));
84 
85  // FIXME: to be updated to Eigen
86  if (VP1Msg::verbose()) {
87  bool inputok = isSane( transformation, "input HepGeom::Transform3D" );
88  bool outputok = isSane( myTransform, "output SoTransform" );
89  if (!inputok)
90  VP1Msg::messageVerbose("VP1LinAlgUtils::toSoTransform ERROR: Problems observed in input HepGeom::Transform3D");
91  if (!outputok)
92  VP1Msg::messageVerbose("VP1LinAlgUtils::toSoTransform ERROR: Problems observed in output SoTransform");
93  if (inputok!=outputok)
94  VP1Msg::messageVerbose("VP1LinAlgUtils::toSoTransform Error introduced in HepGeom::Transform3D -> SoTransform conversion!");
95  }
96 
97  return myTransform;
98 }
99 
100 //______________________________________________________________________________________
101 void VP1LinAlgUtils::transformToMatrix(SoTransform * xf, SbMatrix& result) {
102  result.makeIdentity();
103  result.setTransform(xf->translation.getValue(), xf->rotation.getValue(),
104  xf->scaleFactor.getValue(), xf->scaleOrientation.getValue(), xf->center.getValue());
105  if (VP1Msg::verbose()) {
106  bool inputok = isSane( xf, "input SoTransform" );
107  bool outputok = isSane( result, "output SbMatrix" );
108  if (!inputok)
109  VP1Msg::messageVerbose("VP1LinAlgUtils::transformToMatrix(SoTransform*,..) ERROR: Problems observed in input SoTransform");
110  if (!outputok)
111  VP1Msg::messageVerbose("VP1LinAlgUtils::transformToMatrix(SoTransform*,..) ERROR: Problems observed in output SbMatrix");
112  if (inputok!=outputok)
113  VP1Msg::messageVerbose("VP1LinAlgUtils::transformToMatrix(SoTransform*,..) Error introduced in SoTransform -> SbMatrix conversion!");
114  }
115 }
116 
117 //______________________________________________________________________________________
118 void VP1LinAlgUtils::transformToMatrix(const HepGeom::Transform3D & heptr, SbMatrix& result)
119 {
120  result = SbMatrix(heptr.xx(),heptr.yx(),heptr.zx(),0,
121  heptr.xy(),heptr.yy(),heptr.zy(),0,
122  heptr.xz(),heptr.yz(),heptr.zz(),0,
123  heptr.dx(),heptr.dy(),heptr.dz(),1);
124  if (VP1Msg::verbose()) {
125  bool inputok = isSane( heptr, "input HepGeom::Transform3D" );
126  bool outputok = isSane( result, "output SbMatrix" );
127  if (!inputok)
128  VP1Msg::messageVerbose("VP1LinAlgUtils::transformToMatrix(HepGeom::Transform3D,..) ERROR: Problems observed in input HepGeom::Transform3D");
129  if (!outputok)
130  VP1Msg::messageVerbose("VP1LinAlgUtils::transformToMatrix(HepGeom::Transform3D,..) ERROR: Problems observed in output SbMatrix");
131  if (inputok!=outputok)
132  VP1Msg::messageVerbose("VP1LinAlgUtils::transformToMatrix(HepGeom::Transform3D,..) Error introduced in HepGeom::Transform3D -> SbMatrix conversion!");
133  }
134 }
135 
136 //______________________________________________________________________________________
137 SoTransform * VP1LinAlgUtils::toSoTransform(const SbMatrix& matr, SoTransform * tin)
138 {
139  SbVec3f translation, scaleFactor;
140  SbRotation rotation, scaleOrientation;
141  matr.getTransform(translation, rotation, scaleFactor, scaleOrientation, SbVec3f(0,0,0));
142  SoTransform * t = tin ? tin : new SoTransform;
143  t->translation.setValue(translation);
144  t->rotation.setValue(rotation);
145  t->scaleFactor.setValue(scaleFactor);
146  t->scaleOrientation.setValue(scaleOrientation);
147  t->center.setValue(0,0,0);
148  if (VP1Msg::verbose()) {
149  bool inputok = isSane( matr, "input SbMatrix" );
150  bool outputok = isSane( t, "output SoTransform" );
151  if (!inputok)
152  VP1Msg::messageVerbose("VP1LinAlgUtils::toSoTransform ERROR: Problems observed in input SoTransform");
153  if (!outputok)
154  VP1Msg::messageVerbose("VP1LinAlgUtils::toSoTransform ERROR: Problems observed in output SbMatrix");
155  if (inputok!=outputok)
156  VP1Msg::messageVerbose("VP1LinAlgUtils::toSoTransform Error introduced in SoTransform -> SbMatrix conversion!");
157  }
158  return t;
159 }
160 
161 //______________________________________________________________________________________
162 void VP1LinAlgUtils::decodeTransformation( const SbMatrix& matr,
163  float& translation_x, float& translation_y, float& translation_z,
164  float& rotaxis_x, float& rotaxis_y, float& rotaxis_z, float& rotangle_radians )
165 {
166  SbVec3f translation, scaleFactor, rotaxis;
167  SbRotation rotation, scaleOrientation;
168  matr.getTransform (translation, rotation,scaleFactor,scaleOrientation,SbVec3f(0,0,0));
169  translation.getValue(translation_x,translation_y,translation_z);
170  rotation.getValue(rotaxis,rotangle_radians);
171  rotaxis.getValue(rotaxis_x,rotaxis_y,rotaxis_z);
172  if (VP1Msg::verbose()) {
173  if (!isSane( matr, "input SbMatrix" ))
174  VP1Msg::messageVerbose("VP1LinAlgUtils::decodeTransformation ERROR: Problems observed in output SbMatrix");
175  }
176 }
177 
178 #ifdef BAD
179 #undef BAD
180 #endif
181 #define BAD(x) { ok=false; if (!ctxStr.isEmpty()) VP1Msg::messageVerbose("VP1LinAlgUtils ("+ctxStr+") ERROR: "+QString(x)); };
182 
183 //______________________________________________________________________________________
184 bool VP1LinAlgUtils::isSane( const HepGeom::Transform3D & t, const QString& ctxStr )
185 {
186 
187  HepGeom::Vector3D<double> translation = t.getTranslation();
188  CLHEP::HepRotation rotation = t.getRotation();
189  CLHEP::Hep3Vector rotationAxis;
190  double rotationAngle;
191  rotation.getAngleAxis(rotationAngle,rotationAxis);
192 
193  bool ok(true);
194 
195  if (Imp::isBad(t.xx(),ctxStr)||Imp::isBad(t.xy(),ctxStr)||Imp::isBad(t.xz(),ctxStr)
196  ||Imp::isBad(t.yx(),ctxStr)||Imp::isBad(t.yy(),ctxStr)||Imp::isBad(t.yz(),ctxStr)
197  ||Imp::isBad(t.zx(),ctxStr)||Imp::isBad(t.zy(),ctxStr)||Imp::isBad(t.zz(),ctxStr)
198  ||Imp::isBad(t.dx(),ctxStr)||Imp::isBad(t.dy(),ctxStr)||Imp::isBad(t.dz(),ctxStr))
199  BAD("Problem in raw 4x4 transformation matrix!");
200  if (Imp::isBad(translation.x(),ctxStr)||Imp::isBad(translation.y(),ctxStr)||Imp::isBad(translation.z(),ctxStr))
201  BAD("Problem in translation!");
202  if (Imp::isBad(rotation.xx(),ctxStr)||Imp::isBad(rotation.xy(),ctxStr)||Imp::isBad(rotation.xz(),ctxStr)
203  ||Imp::isBad(rotation.yx(),ctxStr)||Imp::isBad(rotation.yy(),ctxStr)||Imp::isBad(rotation.yz(),ctxStr)
204  ||Imp::isBad(rotation.zx(),ctxStr)||Imp::isBad(rotation.zy(),ctxStr)||Imp::isBad(rotation.zz(),ctxStr))
205  BAD("Problem in rotation");
206  if (Imp::isBad(rotationAxis.x(),ctxStr)||Imp::isBad(rotationAxis.y(),ctxStr)||Imp::isBad(rotationAxis.z(),ctxStr))
207  BAD("Problem in rotationAxis");
208  if (Imp::isBad(rotationAngle,ctxStr))
209  BAD("Problem in rotationAngle");
210 
211  //Check that input rotation has det=1
212  const double det = t.xx()*t.yy()*t.zz()
213  +t.zx()*t.xy()*t.yz()
214  +t.xz()*t.yx()*t.zy()
215  -t.xx()*t.zy()*t.yz()
216  -t.yx()*t.xy()*t.zz()
217  -t.zx()*t.yy()*t.xz();
218  const double eps = 1.0e-5;
219  if (det<1.0-eps||det>1.0+eps)
220  BAD("Determinant of rotation part is not consistent with 1 (det="+QString::number(det)+")!");
221 
222  return ok;
223 }
224 
225 
226 //______________________________________________________________________________________
227 bool VP1LinAlgUtils::isSane( const Amg::Transform3D & t, const QString& ctxStr )
228 {
229  Amg::Vector3D translation = t.translation();
230 
231  Amg::RotationMatrix3D rotation = t.rotation();
232 
233  double rotationAngle;
234  Amg::Vector3D rotationAxis;
235 
236  Amg::getAngleAxisFromRotation(rotation, rotationAngle, rotationAxis);
237 
238 // double xx = rotation(0,0);
239 // double yy = rotation(1,1);
240 // double zz = rotation(2,2);
241 //
242 // double cosa = 0.5 * (xx + yy + zz - 1);
243 // double cosa1 = 1 - cosa;
244 //
245 // if (cosa1 <= 0) {
246 // rotationAngle = 0;
247 // rotationAxis = Amg::Vector3D(0,0,1);
248 // }
249 // else{
250 // double x=0, y=0, z=0;
251 // if (xx > cosa) x = sqrt((xx-cosa)/cosa1);
252 // if (yy > cosa) y = sqrt((yy-cosa)/cosa1);
253 // if (zz > cosa) z = sqrt((zz-cosa)/cosa1);
254 // if (rotation(2,1) < rotation(1,2)) x = -x;
255 // if (rotation(0,2) < rotation(2,0)) y = -y;
256 // if (rotation(1,0) < rotation(0,1)) z = -z;
257 // rotationAngle = (cosa < -1.) ? acos(-1.) : acos(cosa);
258 // rotationAxis = Amg::Vector3D(x,y,z);
259 // }
260 
261 
262 
263 
264  bool ok(true);
265 
266  if (Imp::isBad(t(0,0),ctxStr)||Imp::isBad(t(0,1),ctxStr)||Imp::isBad(t(0,2),ctxStr)
267  ||Imp::isBad(t(1,0),ctxStr)||Imp::isBad(t(1,1),ctxStr)||Imp::isBad(t(1,2),ctxStr)
268  ||Imp::isBad(t(2,0),ctxStr)||Imp::isBad(t(2,1),ctxStr)||Imp::isBad(t(2,2),ctxStr)
269 // ||Imp::isBad(t.dx(),ctxStr)||Imp::isBad(t.dy(),ctxStr)||Imp::isBad(t.dz(),ctxStr))
270  ||Imp::isBad(t(0,3),ctxStr)||Imp::isBad(t(1,3),ctxStr)||Imp::isBad(t(2,3),ctxStr))
271  BAD("Problem in raw 4x4 transformation matrix!");
272  if (Imp::isBad(translation.x(),ctxStr)||Imp::isBad(translation.y(),ctxStr)||Imp::isBad(translation.z(),ctxStr))
273  BAD("Problem in translation!");
274  if (Imp::isBad(rotation(0,0),ctxStr)||Imp::isBad(rotation(0,1),ctxStr)||Imp::isBad(rotation(0,2),ctxStr)
275  ||Imp::isBad(rotation(1,0),ctxStr)||Imp::isBad(rotation(1,1),ctxStr)||Imp::isBad(rotation(1,2),ctxStr)
276  ||Imp::isBad(rotation(2,0),ctxStr)||Imp::isBad(rotation(2,1),ctxStr)||Imp::isBad(rotation(2,2),ctxStr))
277  BAD("Problem in rotation");
278  if (Imp::isBad(rotationAxis.x(),ctxStr)||Imp::isBad(rotationAxis.y(),ctxStr)||Imp::isBad(rotationAxis.z(),ctxStr))
279  BAD("Problem in rotationAxis");
280  if (Imp::isBad(rotationAngle,ctxStr))
281  BAD("Problem in rotationAngle");
282 
283  //Check that input rotation has det=1
284 // const double det = t.xx()*t.yy()*t.zz()
285 // +t.zx()*t.xy()*t.yz()
286 // +t.xz()*t.yx()*t.zy()
287 // -t.xx()*t.zy()*t.yz()
288 // -t.yx()*t.xy()*t.zz()
289 // -t.zx()*t.yy()*t.xz();
290  const double det = t.matrix().determinant();
291  const double eps = 1.0e-5;
292  if ( det < 1.0-eps || det > 1.0+eps )
293  BAD("Determinant of rotation part is not consistent with 1 (det="+QString::number(det)+")!");
294 
295  return ok;
296 }
297 
298 
299 //______________________________________________________________________________________
300 bool VP1LinAlgUtils::isSane( const SoTransform * t, const QString& ctxStr )
301 {
302  if (!t) {
303  if (!ctxStr.isEmpty())
304  VP1Msg::messageVerbose("VP1LinAlgUtils::toSoTransform ("+ctxStr+") ERROR: NULL SoTransform!");
305  return false;
306  }
307 
308  bool ok(true);
309  float q0,q1, q2,q3;
310  t->rotation.getValue().getValue(q0,q1, q2,q3);
311  if (Imp::isBad(q0,ctxStr)||Imp::isBad(q1,ctxStr)||Imp::isBad(q2,ctxStr)||Imp::isBad(q3,ctxStr))
312  BAD("Problem in quarternion representation of rotation!");
313  t->scaleOrientation.getValue().getValue(q0,q1, q2,q3);
314  if (Imp::isBad(q0,ctxStr)||Imp::isBad(q1,ctxStr)||Imp::isBad(q2,ctxStr)||Imp::isBad(q3,ctxStr))
315  BAD("Problem in quarternion representation of scaleOrientation!");
316  float x,y,z;
317  t->translation.getValue().getValue(x,y,z);
318  if (Imp::isBad(x,ctxStr,Imp::atlasR)||Imp::isBad(y,ctxStr,Imp::atlasR)||Imp::isBad(z,ctxStr,Imp::atlasR))
319  BAD("Problem in translation!");
320  t->scaleFactor.getValue().getValue(x,y,z);
321  if (Imp::isBad(x,ctxStr,Imp::atlasR)||Imp::isBad(y,ctxStr,Imp::atlasR)||Imp::isBad(z,ctxStr,Imp::atlasR))
322  BAD("Problem in scaleFactor!");
323  t->center.getValue().getValue(x,y,z);
324  if (Imp::isBad(x,ctxStr,Imp::atlasR)||Imp::isBad(y,ctxStr,Imp::atlasR)||Imp::isBad(z,ctxStr,Imp::atlasR))
325  BAD("Problem in center!");
326 
327  return ok;
328 }
329 
330 //______________________________________________________________________________________
331 bool VP1LinAlgUtils::isSane( const SbMatrix& matr, const QString& ctxStr )
332 {
333  bool ok(true);
334 
335  bool baseok(true);
336  for (int i = 0; i < 4; ++i)
337  for (int j = 0; j < 4; ++j)
338  if (Imp::isBad(matr[i][j],ctxStr)) {
339  baseok=false;
340  break;
341  }
342  if (!baseok)
343  BAD("Problem in raw 4x4 matrix!");
344 
345  SbVec3f translation, scaleFactor, rotaxis;
346  SbRotation rotation, scaleOrientation;
347  matr.getTransform(translation, rotation,scaleFactor,scaleOrientation,SbVec3f(0,0,0));
348  float q0,q1, q2,q3;
349  rotation.getValue(q0,q1, q2,q3);
350  if (Imp::isBad(q0,ctxStr)||Imp::isBad(q1,ctxStr)||Imp::isBad(q2,ctxStr)||Imp::isBad(q3,ctxStr))
351  BAD("Problem in quarternion representation of rotation!");
352  scaleOrientation.getValue(q0,q1, q2,q3);
353  if (Imp::isBad(q0,ctxStr)||Imp::isBad(q1,ctxStr)||Imp::isBad(q2,ctxStr)||Imp::isBad(q3,ctxStr))
354  BAD("Problem in quarternion representation of scaleOrientation!");
355  float x,y,z;
356  translation.getValue(x,y,z);
357  if (Imp::isBad(x,ctxStr,Imp::atlasR)||Imp::isBad(y,ctxStr,Imp::atlasR)||Imp::isBad(z,ctxStr,Imp::atlasR))
358  BAD("Problem in translation!");
359  scaleFactor.getValue(x,y,z);
360  if (Imp::isBad(x,ctxStr,Imp::atlasR)||Imp::isBad(y,ctxStr,Imp::atlasR)||Imp::isBad(z,ctxStr,Imp::atlasR))
361  BAD("Problem in scaleFactor!");
362 
363  const float eps = 1.0e-5;
364  const float det3 = matr.det3();
365  if (det3<1.0-eps||det3>1.0+eps)
366  BAD("Determinant of rotation part is not consistent with 1 (det3="+QString::number(det3)+")!");
367  const float det4 = matr.det4();
368  if (det4<1.0-eps||det4>1.0+eps)
369  BAD("Determinant of rotation part is not consistent with 1 (det4="+QString::number(det4)+")!");
370  return ok;
371 }
372 
373 //_____________________________________________________________________________________
374 double VP1LinAlgUtils::phiFromXY(const double& x, const double&y )
375 {
376  //Special case: Returns 0 if both x and y are 0
377  double phi = (x == 0.0f)? (y==0.0f?0.0:0.5*M_PI) : std::abs(atan(std::abs(y/static_cast<double>(x))));
378  if (x>=0.0f&&y<0.0f)
379  phi = 2*M_PI-phi;
380  else if (x<0.0f&&y>=0.0f)
381  phi = M_PI-phi;
382  else if (x<0.0f&&y<0.0f)
383  phi = M_PI+phi;
384  return phi;
385 }
386 
387 
388 //_____________________________________________________________________________________
389 /* http://www.mycplus.com/tutorial.asp?TID=80 */
390 void VP1LinAlgUtils::distPointLineParam(const Amg::Vector3D& point, const Amg::Vector3D& point0, const Amg::Vector3D& point1, double& s)
391 {
392 // s = ((point - point0)*(point1 - point0)) / (((point1 - point0)*(point1 - point0)));
393 
394  s = ( ((point)[0]-(point0)[0])*((point1)[0]-(point0)[0]) +
395  ((point)[1]-(point0)[1])*((point1)[1]-(point0)[1]) +
396  ((point)[2]-(point0)[2])*((point1)[2]-(point0)[2]) ) /
397  ( ((point1)[0]-(point0)[0])*((point1)[0]-(point0)[0]) +
398  ((point1)[1]-(point0)[1])*((point1)[1]-(point0)[1]) +
399  ((point1)[2]-(point0)[2])*((point1)[2]-(point0)[2]) );
400 
401  return;
402 }
403 
404 //_____________________________________________________________________________________
405 /* http://www.mycplus.com/tutorial.asp?TID=80 */
406 double VP1LinAlgUtils::distPointLine2(const Amg::Vector3D& point, const Amg::Vector3D& point0, const Amg::Vector3D& point1, double& s)
407 {
408  VP1LinAlgUtils::distPointLineParam(point, point0, point1, s);
409 
410  // double dx = (*point)[0]-((*point0)[0] + s*((*point1)[0]-(*point0)[0]));
411  // double dy = (*point)[1]-((*point0)[1] + s*((*point1)[1]-(*point0)[1]));
412  // double dz = (*point)[2]-((*point0)[2] + s*((*point1)[2]-(*point0)[2]));
413 
414  Amg::Vector3D d = point-(point0 + s*(point1-point0));
415 
416  //return dx*dx + dy*dy + dz*dz;
417  return d.dot(d);
418 }
419 
420 //_____________________________________________________________________________________
421 /* http://www.mycplus.com/tutorial.asp?TID=81 */
422 void VP1LinAlgUtils::distLineLineParam(const Amg::Vector3D& point0, const Amg::Vector3D& point1, const Amg::Vector3D& point2, const Amg::Vector3D& point3, double& s, double& t)
423 {
424  //Actual distance not calculated, only parameters s and t.
425 
426  Amg::Vector3D p32 = point3 - point2;
427  double d3232 = p32.dot(p32);
428  Amg::Vector3D p10 = point1 - point0;
429  double d1010 = p10.dot(p10);
430  if (d3232==0.0||d1010==0.0||d3232!=d3232||d1010!=d1010) {
431  //One input line ill-defined:
432  s=0.5;
433  t=0.5;
434  VP1Msg::messageDebug("VP1LinAlgUtils distLineLineParam ERROR: One or both input lines ill defined");
435  return;
436  }
437 
438  double d3210 = p32.dot(p10);
439  double denom = d1010 * d3232 - d3210 * d3210;
440  Amg::Vector3D p02 = point0 - point2;
441  double d0232 = p02.dot(p32);
442 
443  if (denom==0) {
444  //Parallel input!
445  VP1Msg::messageDebug("VP1LinAlgUtils distLineLineParam Warning: Input lines are parallel. Choosing arbitrary point of closest approach");
446  s = 0.5;//We choose an arbitrary point on one of the two lines.
447  } else {
448  double d0210 = p02.dot(p10);
449  double numer = d0232 * d3210 - d0210 * d3232;
450  s = numer / denom;
451  }
452  t = (d0232 + d3210 * s) / d3232;
453 
454  return;
455 }
VP1LinAlgUtils::Imp
Definition: VP1LinAlgUtils.cxx:25
python.SystemOfUnits.s
int s
Definition: SystemOfUnits.py:131
get_generator_info.result
result
Definition: get_generator_info.py:21
phi
Scalar phi() const
phi method
Definition: AmgMatrixBasePlugin.h:67
TRTCalib_Extractor.det
det
Definition: TRTCalib_Extractor.py:36
hist_file_dump.d
d
Definition: hist_file_dump.py:137
VP1Msg.h
BAD
#define BAD(x)
Definition: VP1LinAlgUtils.cxx:181
M_PI
#define M_PI
Definition: ActiveFraction.h:11
VP1LinAlgUtils::decodeTransformation
static void decodeTransformation(const SbMatrix &, float &translation_x, float &translation_y, float &translation_z, float &rotaxis_x, float &rotaxis_y, float &rotaxis_z, float &rotangle_radians)
Definition: VP1LinAlgUtils.cxx:162
Amg::getAngleAxisFromRotation
void getAngleAxisFromRotation(Amg::RotationMatrix3D &rotation, double &rotationAngle, Amg::Vector3D &rotationAxis)
Definition: GeoPrimitivesHelpers.h:192
read_hist_ntuple.t
t
Definition: read_hist_ntuple.py:5
x
#define x
VP1String::str
static QString str(const QString &s)
Definition: VP1String.h:49
drawFromPickle.atan
atan
Definition: drawFromPickle.py:36
VP1LinAlgUtils::phiFromXY
static double phiFromXY(const double &x, const double &y)
Definition: VP1LinAlgUtils.cxx:374
VP1LinAlgUtils.h
lumiFormat.i
int i
Definition: lumiFormat.py:85
z
#define z
xAOD::rotation
rotation
Definition: TrackSurface_v1.cxx:15
Amg::Transform3D
Eigen::Affine3d Transform3D
Definition: GeoPrimitives.h:46
VP1LinAlgUtils::distLineLineParam
static void distLineLineParam(const Amg::Vector3D &point0, const Amg::Vector3D &point1, const Amg::Vector3D &point2, const Amg::Vector3D &point3, double &s, double &t)
Definition: VP1LinAlgUtils.cxx:422
hist_file_dump.f
f
Definition: hist_file_dump.py:135
compute_lumi.denom
denom
Definition: compute_lumi.py:76
python.selection.number
number
Definition: selection.py:20
VP1LinAlgUtils::distPointLineParam
static void distPointLineParam(const Amg::Vector3D &point, const Amg::Vector3D &point0, const Amg::Vector3D &point1, double &s)
Definition: VP1LinAlgUtils.cxx:390
VP1LinAlgUtils::Imp::atlasR
static double atlasR
Definition: VP1LinAlgUtils.cxx:27
VP1LinAlgUtils::transformToMatrix
static void transformToMatrix(SoTransform *xf, SbMatrix &result)
Definition: VP1LinAlgUtils.cxx:101
VP1Msg::messageVerbose
static void messageVerbose(const QString &)
Definition: VP1Msg.cxx:84
Amg::Vector3D
Eigen::Matrix< double, 3, 1 > Vector3D
Definition: GeoPrimitives.h:47
VP1LinAlgUtils::distPointLine2
static double distPointLine2(const Amg::Vector3D &point, const Amg::Vector3D &point0, const Amg::Vector3D &point1, double &s)
Definition: VP1LinAlgUtils.cxx:406
VP1Msg::messageDebug
static void messageDebug(const QString &)
Definition: VP1Msg.cxx:39
y
#define y
Amg::RotationMatrix3D
Eigen::Matrix< double, 3, 3 > RotationMatrix3D
Definition: GeoPrimitives.h:49
VP1LinAlgUtils::isSane
static bool isSane(const HepGeom::Transform3D &, const QString &contextStr="")
Definition: VP1LinAlgUtils.cxx:184
VP1Msg::verbose
static bool verbose()
Definition: VP1Msg.h:31
updateCoolNtuple.limit
int limit
Definition: updateCoolNtuple.py:45
VP1LinAlgUtils::toSoTransform
static SoTransform * toSoTransform(const HepGeom::Transform3D &, SoTransform *t=0)
Definition: VP1LinAlgUtils.cxx:40
python.SystemOfUnits.km
int km
Definition: SystemOfUnits.py:95
VP1LinAlgUtils::Imp::isBad
static bool isBad(const double &x, const QString &ctxStr, const double &limit=1.0e20)
Definition: VP1LinAlgUtils.cxx:28
Generated on Sun Dec 22 2024 21:22:34 for ATLAS Offline Software by   doxygen 1.8.18