00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015 #include "MdcGeom/Trajectory.h"
00016 #include "TrkBase/TrkPoca.h"
00017 #include "CLHEP/Vector/ThreeVector.h"
00018 #include "CLHEP/Matrix/Vector.h"
00019 #include "CLHEP/Geometry/Point3D.h"
00020 #include "TrkBase/TrkErrCode.h"
00021
00022 TrkPoca::TrkPoca(const Trajectory& traj1, double f1,
00023 const Trajectory& traj2, double f2, double prec)
00024 : TrkPocaBase(f1,f2,prec), _doca(-9999.)
00025 {
00026 minimize(traj1,f1,traj2,f2);
00027 if (status().failure()) return;
00028 calcDist(traj1,traj2);
00029 }
00030
00031 TrkPoca::TrkPoca(const Trajectory& traj, double flt,
00032 const HepPoint3D& pt, double prec)
00033 : TrkPocaBase(flt,prec)
00034 {
00035 minimize(traj,flt,pt);
00036 if (status().failure()) return;
00037 _doca = (traj.position(flt1()) - pt).mag();
00038 }
00039
00040 void
00041 TrkPoca::calcDist(const Trajectory& traj1, const Trajectory& traj2)
00042 {
00043
00044
00045
00046
00047 static Hep3Vector dir1,dir2;
00048 static HepPoint3D pos1,pos2;
00049
00050 traj1.getInfo(flt1(), pos1, dir1);
00051 traj2.getInfo(flt2(), pos2, dir2);
00052 Hep3Vector delta = ((CLHEP::Hep3Vector)pos2) - ((CLHEP::Hep3Vector)pos1);
00053 Hep3Vector between = dir1.cross( dir2 );
00054
00055 if (status().success() != 3) {
00056 between = between.unit();
00057 _doca = delta.dot(between);
00058 } else {
00059
00060 double sign = (dir1.dot(dir2) > 0.) ? -1. : 1.;
00061 _doca = (delta - delta.dot(dir1) * dir1).mag();
00062 _doca *= sign;
00063 }
00064 }