#include "hkalgeomtools.h" // from ROOT #include "TGeoTorus.h" #include "TMath.h" #include "TVector3.h" #include using namespace std; ClassImp(HKalGeomTools) //_HADES_CLASS_DESCRIPTION /////////////////////////////////////////////////////////////////////////////// // // Provides static functions to solve some geometric problems. // /////////////////////////////////////////////////////////////////////////////// // ----------------------------------- // Implementation of public methods // ----------------------------------- Double_t HKalGeomTools::distance2Points(const TVector3 &vec1, const TVector3 &vec2) { // Calculates the distance between two points. TVector3 distV = vec1 - vec2; return distV.Mag(); } Double_t HKalGeomTools::distancePointToPlane(const TVector3 &point, const TVector3 &planeCenter, const TVector3 &planeNormal) { // Calculates the distance of vector pos to the plane. // Plane equation in Hesse Normal Form: center * normal - d = 0 // Distance of point to plane = point * normal - d return TMath::Abs(signedDistancePointToPlane(point, planeCenter, planeNormal)); } Double_t HKalGeomTools::distancePointToLine(TVector3 &pca, Int_t &pcaFlag, const TVector3 &point, const TVector3 &segPoint1, const TVector3 &segPoint2) { // Calculates the distance between a test point and a line defined by two points. // This function also find the point on the segment that is closest to the test point. // For method used see: // http://www.geometrictools.com/Documentation/DistancePointLine.pdf // // return value: Distance of the test point to the segment. // output parameters: // pca: The point on the line that is closest to the test point. // iFlag: =0 if the PCA is on the segment, // =1 the PCA is one of the segment end points. // input parameters: // point: Find the distance of this test point to the line segment. // segPoint1, segPoint2: The two points defining the line segment. if(segPoint1 == segPoint2) { ::Warning("distancePointToLine()", "Both line points are equal."); return (point - segPoint1).Mag(); } TVector3 segDir = segPoint2 - segPoint1; TVector3 diff = point - segPoint1; Double_t segPar = segDir * diff / (segDir * segDir); Double_t dist = 0.; if(segPar > 1. || segPar <= 0.) { pcaFlag = 1; } else { pcaFlag = 0; } pca = segPoint1 + segPar * segDir; dist = (diff - segPar * segDir).Mag(); return dist; } Double_t HKalGeomTools::distancePointToSegment(const TVector3 &point, const TVector3 &segPoint1, const TVector3 &segPoint2) { // Calculates the distance between a test point and a line segment. // For method used see: // http://www.geometrictools.com/Documentation/DistancePointLine.pdf // // input: // point: the test point // segPoint1, segPoint2: The two points defining the line segment. TVector3 pca; Int_t iFlag; return distancePointToSegment(pca, iFlag, point, segPoint1, segPoint2); } Double_t HKalGeomTools::distancePointToSegment(TVector3 &pca, Int_t &pcaFlag, const TVector3 &point, const TVector3 &segPoint1, const TVector3 &segPoint2) { // Calculates the distance between a test point and a line segment defined by two points. // This function also find the point on the segment that is closest to the test point. // For method used see: // http://www.geometrictools.com/Documentation/DistancePointLine.pdf // // return value: Distance of the test point to the segment. // output parameters: // pca: The point on the line segment that is closest to the test point. // iFlag: =0 if the PCA is on the segment, // =1 the PCA is one of the segment end points. // input parameters: // point: Find the distance of this test point to the line segment. // segPoint1, segPoint2: The two points defining the line segment. if(segPoint1 == segPoint2) { ::Warning("distancePointToSegment()", "Both segment end points are equal."); return (point - segPoint1).Mag(); } TVector3 segDir = segPoint2 - segPoint1; TVector3 diff = point - segPoint1; Double_t segPar = segDir * diff / (segDir * segDir); Double_t dist = 0.; pcaFlag = 0; if(segPar > 0.) { // 0 < segPar < 1 if(segPar < 1.) { dist = (diff - segPar * segDir).Mag(); pca = segPoint1 + segPar * segDir; } else { // segPar >= 1 dist = (diff - segDir).Mag(); pca = segPoint2; pcaFlag = 1; } } else { // segPar <= 0 dist = diff.Mag(); pca = segPoint1; pcaFlag = 1; } return dist; } Bool_t HKalGeomTools::findIntersectionLinePlane(TVector3 &pointIntersect, const TVector3 &pos, const TVector3 &dir, const TVector3 &planeCenter, const TVector3 &planeNormal) { // Finds the intersection point of a straight line with any plane. // pointIntersect: the intersection point (return value). // pos: a point on the straight line. // dir: direction of the straight line. // planeCenter: a point on the plane. // planeNormal: normal vector of the plane. Double_t denom = planeNormal.Dot(dir); if (denom != 0.0) { Double_t t = ((planeCenter.x() - pos.x()) * planeNormal.x() + (planeCenter.y() - pos.y()) * planeNormal.y() + (planeCenter.z() - pos.z()) * planeNormal.z()) / denom; pointIntersect = pos + (t * dir); return kTRUE; } else { ::Warning("findIntersection()", "No intersection point found : (plane || track)"); return kFALSE; } return kFALSE; } Bool_t HKalGeomTools::isPointOnPlane(const TVector3 &point, const TVector3 &planeCenter, const TVector3 &planeNormal){ // Checks if point is on the plane. Double_t dist = distancePointToPlane(point, planeCenter, planeNormal); return ((TMath::Abs(dist) < 1.e-10) ? kTRUE : kFALSE); } Double_t HKalGeomTools::signedDistancePointToPlane(const TVector3 &point, const TVector3 &planeCenter, const TVector3 &planeNormal) { // Calculates the distance of a test point to the plane. // The signed distance is positive if the origin of the coordinate system and the test point // are on opposite sides of the plane and negative if they are on the same side. // // input parameters: // point: test point // planeCenter: a point on the plane // planeNormal: normal vector of the plane // Plane equation in Hesse Normal Form: center * normal - d = 0 // Distance of point to plane = point * normal - d if(planeCenter * planeNormal < 0.) { // Normal vector of the plane is pointing in the wrong direction. return (point - planeCenter) * planeNormal.Unit() * -1.; } return (point - planeCenter) * planeNormal.Unit(); } void HKalGeomTools::Track2ToLine(TVector3 &Pfinal, TVector3 &Pwire, Int_t &Iflag, Double_t &Dist, Double_t &Length, const TVector3 &X1, const TVector3 &X2, const TVector3 &w1, const TVector3 &w2) { // Code from GEANE. // // Closest approach to a line from 2 GEANE points // // METHOD: the nearest points on the two lines // x1,x2 and w1,w2 is found. // The method is described in: // http://softsurfer.com/Archive/algorithm_0106/algorithm_0106.htm // http://www.geometrictools.com/Documentation/DistanceLine3Line3.pdf. // // INPUT: x1, x2 closest appoach GEANE points // w1, w2 points of the line (wire) to approach // // OUTPUT: Pfinal point of closest approach on the track // Pwire point of closest approach on the wire // Dist distance between Pfian and w1 // Length arc length to add to the GEANE length of x1 // Iflag =1 when Pwire is outside [w1,w2] // In this case, when w1 and w2 are the extremes // of the wire, the user could remake the procedure // by calling Track3ToPoint or Track2ToPoint, where the // Point is w1 or w2; // = 2 when the two lines are parallel and the solution // does not exists. // // Authors: Andrea Fontana and Alberto Rotondi 20 MAy 2007 // TVector3 x21, x32, w21; TVector3 xw1, xw2; Double_t a1, b1, c1, d1, e1, t1, s1; Double_t Delta1; Double_t Eps = 1.E-08; Iflag =0; // line-line distance x21 = X2-X1; w21 = w2-w1; xw1 = X1-w1; xw2 = X2-w1; a1 = x21.Mag2(); b1 = x21.Dot(w21); c1 = w21.Mag2(); d1 = xw1.Dot(x21); e1 = xw1.Dot(w21); Delta1 = a1*c1-b1*b1; if(Delta1 > Eps) { t1 = (a1*e1-b1*d1)/Delta1; s1 = (b1*e1-c1*d1)/Delta1; Pfinal = (X1 + x21*s1); Pwire = (w1 + w21*t1); Length = s1*x21.Mag(); Dist= (Pfinal-Pwire).Mag(); } else { // lines are parallel, no solution does exist Pfinal.SetXYZ(0.,0.,0.); Pwire.SetXYZ(0.,0.,0.); Dist=0.; Length=0.; Iflag = 2; return; } // flag when the point on the wire is outside (w1,w2) if((((Pwire[0] 1e-11 && fabs(Pwire[0]- w2[0]) > 1e-11)) || (((Pwire[1] 1e-11 && fabs(Pwire[1]- w2[1]) > 1e-11)) || (((Pwire[2] 1e-11 && fabs(Pwire[2]- w2[2]) > 1e-11))) { Iflag=1; } }