CocoaModel/src/ALILine.cc

0001 //   COCOA class implementation file
0002 //Id:  ALILine.C
0003 //CAT: Fit
0004 //
0005 //   History: v1.0
0006 //   Pedro Arce
0007
0008 #include "Alignment/CocoaModel/interface/ALILine.h"
0009 #include "Alignment/CocoaUtilities/interface/CocoaGlobals.h"
0010 #include "Alignment/CocoaModel/interface/ALIPlane.h"
0011 #include "Alignment/CocoaUtilities/interface/ALIUtils.h"
0012 #include <cstdlib>
0013 #include <cmath>  // include floating-point std::abs functions
0014
0015 //@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
0016 //@@ Constructor
0017 //@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
0018 ALILine::ALILine(const CLHEP::Hep3Vector& point, const CLHEP::Hep3Vector& direction) {
0019   _point = point;
0020   _direction = direction * (1 / direction.mag());
0021   if (ALIUtils::debug >= 9)
0022     ALIUtils::dump3v(_point, " ALILine point _point = ");
0023   if (ALIUtils::debug >= 9)
0024     ALIUtils::dump3v(_direction, " ALILine direction _direction = ");
0025 }
0026
0027 CLHEP::Hep3Vector ALILine::intersect(const ALILine& l2, bool notParallel) {
0028   if (ALIUtils::debug >= 3) {
0029     std::cout << "***** ALILine::intersect (constructor) two lines: " << std::endl
0030               << " line1: " << *this << std::endl
0031               << " line2: " << l2 << std::endl;
0032   }
0033   CLHEP::Hep3Vector inters;
0034   //--- Check that they are not parallel
0035   double acosvec = vec().dot(l2.vec()) / vec().mag() / l2.vec().mag();
0036
0037   if (ALIUtils::debug >= 5) {
0038     std::cout << "\t Determination of acosvec = vec().dot(l2.vec()) / vec().mag() / l2.vec().mag() " << std::endl
0039               << std::endl;
0040     //      std::cout << "\t vec().dot = " << vec().dot << std::endl;
0041     std::cout << "\t l2.vec() = " << l2.vec() << std::endl;
0042
0043     std::cout << "\t vec().mag() = " << vec().mag() << std::endl;
0044     std::cout << "\t l2.vec().mag() = " << l2.vec().mag() << std::endl << std::endl;
0045   }
0046
0047   if (ALIUtils::debug >= 5)
0048     std::cout << " acosvec = " << acosvec << std::endl;
0049   if (1 - std::abs(acosvec) < 1E-8) {
0050     if (notParallel) {
0051       std::cerr << " !!!EXITING ALILine::intersect: two lines are parallel" << std::endl;
0052       exit(1);
0053     } else {
0054       if (ALIUtils::debug >= 5)
0055         std::cout << " !!! ALILine::intersect: two lines are parallel (no errors)" << std::endl;
0056       //gcc2952      inters = CLHEP::Hep3Vector( DBL_MAX, DBL_MAX, DBL_MAX );
0057       inters = CLHEP::Hep3Vector(ALI_DBL_MAX, ALI_DBL_MAX, ALI_DBL_MAX);
0058     }
0059   } else {
0060     //  ****************************************************************        //
0061     //  ****************************************************************        //
0062     //  ****************************************************************        //
0063     //                   Determination of Fact                  //
0064     //                                          //
0065     //  Problem :  3D quantity was determined by doing calculation with         //
0066     //         the 2D projections of the std::vectors.  It is possible      //
0067     //         for projection in a particular plane to be 0                 //
0068     //                                                                             //
0069     //  Solution : Test for problem and redo calculation if necessary by        //
0070     //         projecting into a different plane                            //
0071     //  ****************************************************************        //
0072     //  ****************************************************************        //
0073     //  ****************************************************************        //
0074
0075     ALIdouble fact = (vec().y() * l2.pt().x() - vec().x() * l2.pt().y() - vec().y() * pt().x() + vec().x() * pt().y()) /
0076                      (vec().x() * l2.vec().y() - vec().y() * l2.vec().x());
0077
0078     if (ALIUtils::debug >= 2) {
0079       std::cout << std::endl << std::endl << "*** START CALC OF FACT ***" << std::endl;
0080       std::cout << "    ==================" << std::endl << std::endl;
0081       std::cout << "*** Determination of fact ->";
0082       std::cout << "\t fact = ("
0083                 << vec().y() * l2.pt().x() - vec().x() * l2.pt().y() - vec().y() * pt().x() + vec().x() * pt().y()
0084                 << "/";
0085
0086       std::cout << vec().x() * l2.vec().y() - vec().y() * l2.vec().x() << ") = " << fact << std::endl;
0087     }
0088
0089     //    ALIdouble old_fact = fact;
0090     ALIdouble old_fact_denominator = vec().x() * l2.vec().y() - vec().y() * l2.vec().x();
0091     //-    ALIdouble old_fact2 = 0.;
0092     ALIdouble old_fact_denominator2 = 999;
0093
0094     if (std::abs(fact) > 1e8 || std::abs(old_fact_denominator) < 1.e-10) {
0095       // do calculation by rotating 90 degrees into xy plane
0096       // Z-> X
0097       // X-> -Z
0098
0099       if (ALIUtils::debug >= 2 && std::abs(old_fact_denominator) < 1.e-10)
0100         std::cout << " ** Problem:  old_fact_denominator -> " << old_fact_denominator << std::endl;
0101
0102       if (ALIUtils::debug >= 2 && std::abs(fact) > 1e8)
0103         std::cout << " ** Problem: fact -> " << fact << std::endl;
0104
0105       if (ALIUtils::debug >= 2 && (vec().x() * l2.vec().y() - vec().y() * l2.vec().x()) == 0)
0106         std::cout << " ** Division by 0 !!! " << std::endl;
0107       if (ALIUtils::debug >= 2)
0108         std::cout << " ** Must rotate to yz plane for calculation (X-> Z) ";
0109
0110       fact = (-1 * vec().y() * l2.pt().z() + vec().z() * l2.pt().y() + vec().y() * pt().z() - vec().z() * pt().y()) /
0111              (-1 * vec().z() * l2.vec().y() + vec().y() * l2.vec().z());
0112
0113       if (ALIUtils::debug >= 2)
0114         std::cout << "\t -- 1st Recalculation of fact in yz plane = " << fact << std::endl;
0115
0116       old_fact_denominator2 = -1 * vec().z() * l2.vec().y() + vec().y() * l2.vec().z();
0117
0118       if (std::abs(-1 * vec().z() * l2.vec().y() + vec().y() * l2.vec().z()) < 1.e-10) {
0119         if (ALIUtils::debug >= 2)
0120           std::cout << " ** Must rotate to xz plane for calculation (Y-> -Z) ";
0121         if (ALIUtils::debug >= 2 && std::abs(old_fact_denominator2) < 1.e-10)
0122           std::cout << " ** Problem: old_fact_denominator2 -> " << old_fact_denominator2 << std::endl;
0123         //-            if(ALIUtils::debug >= 2 && std::abs(old_fact2) > 1.e8) std::cout << " ** Problem: old_fact2 -> " << old_fact2 << std::endl;
0124
0125         fact = (-1 * vec().z() * l2.pt().x() + vec().x() * l2.pt().z() + vec().z() * pt().x() - vec().x() * pt().z()) /
0126                (-1 * vec().x() * l2.vec().z() + vec().z() * l2.vec().x());
0127
0128         if (ALIUtils::debug >= 2)
0129           std::cout << "\t -- 2nd Recalculation of fact in xz plane = " << fact << std::endl;
0130
0131       } else {
0132         if (ALIUtils::debug >= 2)
0133           std::cout << "*!* 2nd calculation sufficient" << std::endl;
0134       }
0135
0136     } else {
0137       if (ALIUtils::debug >= 2)
0138         std::cout << "*!* Standard calculation - things are fine" << std::endl;
0139       if (ALIUtils::debug >= 2)
0140         std::cout << "\t ----> fact = ( "
0141                   << vec().y() * l2.pt().x() - vec().x() * l2.pt().y() - vec().y() * pt().x() + vec().x() * pt().y()
0142                   << " / " << vec().x() * l2.vec().y() - vec().y() * l2.vec().x() << " ) = " << fact << std::endl;
0143     }
0144
0145     //  ****************************************************************    //
0146     //  ****************************************************************    //
0147     //  ****************************************************************    //
0148     //                      Finished With Fact                  //
0149     //  ****************************************************************    //
0150     //  ****************************************************************    //
0151     //  ****************************************************************    //
0152
0153     inters = l2.pt() + fact * l2.vec();
0154
0155     if (ALIUtils::debug >= 2) {
0156       std::cout << "Determination of intersection = l2.pt() + fact * l2.vec()" << std::endl << std::endl;
0157       ALIUtils::dump3v(l2.pt(), "\t --> l2.pt() = ");
0158       std::cout << "\t --> fact = " << fact << std::endl;
0159       std::cout << "\t --> l2.vec() = " << l2.vec() << std::endl;
0160       ALIUtils::dump3v(inters, "***\t --> ALILine::intersection at: ");
0161     }
0162   }
0163
0164   return inters;
0165 }
0166
0167 std::ostream& operator<<(std::ostream& out, const ALILine& li) {
0168   out << " ALILine point " << li._point << std::endl;
0169   out << " ALILine direc " << li._direction;
0170
0171   return out;
0172 }
0173
0174 //@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
0175 //@@ intersect: Intersect a LightRay with a plane and change thePoint to the intersection point
0176 //@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
0177
0178 CLHEP::Hep3Vector ALILine::intersect(const ALIPlane& plane, bool notParallel) {
0179   if (ALIUtils::debug >= 5)
0180     std::cout << "***** ALILine::intersect WITH PLANE" << std::endl;
0181   if (ALIUtils::debug >= 4) {
0182     ALIUtils::dump3v(plane.point(), "plane point");
0183     ALIUtils::dump3v(plane.normal(), "plane normal");
0184   }
0185
0186   //---------- Check that they intersect
0187   if (std::abs(plane.normal() * _direction) < 1.E-10) {
0188     std::cerr << " !!!! INTERSECTION NOT POSSIBLE: LightRay is perpendicular to plane " << std::endl;
0189     std::cerr << " plane.normal()*direction() = " << plane.normal() * _direction << std::endl;
0190     ALIUtils::dump3v(_direction, "LightRay direction ");
0191     ALIUtils::dump3v(plane.normal(), "plane normal ");
0192     exit(1);
0193   }
0194
0195   //---------- Get intersection point between LightRay and plane
0196   //-   if(ALIUtils::debug >= 4) std::cout << " ALILine::intersect WITH LightRay" << std::endl;
0197
0198   CLHEP::Hep3Vector vtemp = plane.point() - _point;
0199   if (ALIUtils::debug >= 4)
0200     ALIUtils::dump3v(vtemp, "n_r = point  - point_plane");
0201
0202   ALIdouble dtemp = _direction * plane.normal();
0203   if (ALIUtils::debug >= 4)
0204     std::cout << " lightray* plate normal" << dtemp << std::endl;
0205
0206   if (dtemp != 0.) {
0207     dtemp = (vtemp * plane.normal()) / dtemp;
0208     if (ALIUtils::debug >= 4)
0209       std::cout << " n_r*plate normal (dtemp) : " << dtemp << std::endl;
0210     if (ALIUtils::debug >= 4)
0211       std::cout << " Old vtemp : " << vtemp << std::endl;
0212   } else
0213     std::cerr << "!!! LightRay: Intersect With Plane: plane and light ray parallel: no intersection" << std::endl;
0214
0215   vtemp = _direction * dtemp;
0216   if (ALIUtils::debug >= 4)
0217     ALIUtils::dump3v(vtemp, "n_r scaled (vtemp) : ");
0218   if (ALIUtils::debug >= 4)
0219     ALIUtils::dump3v(CLHEP::Hep3Vector(dtemp), "dtemp analog to vtemp : ");
0220
0221   CLHEP::Hep3Vector inters = vtemp + _point;
0222   if (ALIUtils::debug >= 4)
0223     ALIUtils::dump3v(inters, "intersection point = vtemp + _point");
0224   if (ALIUtils::debug >= 4)
0225     ALIUtils::dump3v(vtemp, "vtemp =  ");
0226   if (ALIUtils::debug >= 4)
0227     ALIUtils::dump3v(_point, "_point =  ");
0228
0229   return inters;
0230 }