00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014 #include "TrkBase/TrkDifPoca.h"
00015 #include "TrkBase/TrkDifTraj.h"
00016 #include "CLHEP/Vector/ThreeVector.h"
00017 #include "CLHEP/Matrix/Vector.h"
00018 #include "CLHEP/Geometry/Point3D.h"
00019 #include "MdcRecoUtil/DifVector.h"
00020 #include "MdcRecoUtil/DifNumber.h"
00021 #include "MdcRecoUtil/DifPoint.h"
00022 #include "TrkBase/TrkPoca.h"
00023 #include "TrkBase/TrkErrCode.h"
00024
00025 TrkDifPoca::TrkDifPoca(const TrkDifTraj& traj1, double f1,
00026 const Trajectory& traj2, double f2, double prec)
00027 : TrkPocaBase(f1,f2,prec), _doca(-9999.,0)
00028 {
00029 minimize(traj1,f1,traj2,f2);
00030 if (status().failure()) return;
00031 calcDist(traj1,traj2);
00032
00033 }
00034
00035
00036 TrkDifPoca::TrkDifPoca(const TrkDifTraj& traj, double f1,
00037 const HepPoint3D& pt, double prec)
00038 : TrkPocaBase(f1,prec), _doca(-9999.,0)
00039 {
00040 minimize(traj,f1,pt);
00041 if (status().failure()) return;
00042 calcDist(traj,pt);
00043 }
00044
00045
00046 void
00047 TrkDifPoca::calcDist(const TrkDifTraj& traj1, const Trajectory& traj2)
00048 {
00049
00050
00051
00052
00053
00054
00055
00056 static DifVector dir1;
00057 static DifPoint pos1;
00058 static Hep3Vector dirTem2;
00059 static HepPoint3D posTem2;
00060
00061
00062
00063 traj2.getInfo(flt2(), posTem2, dirTem2);
00064 traj1.getDFInfo2(flt1(), pos1, dir1);
00065 DifVector dir2(dirTem2);
00066 DifPoint pos2(posTem2);
00067
00068 DifVector delta = pos2 - pos1;
00069 if (status().success() != 3) {
00070 DifVector between = cross( dir1, dir2 );
00071 between.normalize();
00072 _doca = delta * between;
00073 } else {
00074 _doca = (delta - delta.dot(dir1) * dir1).length();
00075 if (dir1.dot(dir2) > 0.) _doca.flipsign();
00076 if (fabs(_doca.number()) < 0.0001 * precision()) {
00077
00078 _doca = 0;
00079 _status.setFailure(3);
00080 }
00081 }
00082 }
00083
00084 void
00085 TrkDifPoca::calcDist(const TrkDifTraj& traj, const HepPoint3D& pt)
00086 {
00087
00088
00089
00090
00091 static DifVector dir;
00092 static DifPoint posTraj;
00093
00094 DifPoint posPoint(pt);
00095 traj.getDFInfo2(flt1(), posTraj, dir);
00096 DifVector delta = posTraj - posPoint;
00097
00098
00099 DifNumber dist = delta.length();
00100 if (dist.number() > 0.01 * precision()) {
00101 _doca = dist;
00102 } else {
00103
00104
00105 Hep3Vector fudgeVec(0., 0.1 * precision(), 0.0);
00106 if (dir.x.number() == 0.0 && dir.z.number() == 0.0) {
00107 fudgeVec = Hep3Vector(0.1 * precision(), 0., 0.);
00108 }
00109 posPoint += fudgeVec;
00110 delta = posTraj - posPoint;
00111 _doca = dist;
00112 _status.setSuccess(20, "TrkDifPoca: distance zero, calculation fudged.");
00113 }
00114 }