@@ -78,10 +78,104 @@ Double_t DistanceToAxis(const TVector3& axisPoint, const TVector3& axisVector, c
7878// / and moving in direction `dir` and the plane defined by its normal vector `n` and the point `a`. This is
7979// / equivalent to move/translate the position `pos` to the plane.
8080// /
81- TVector3 GetPlaneVectorIntersection (TVector3 pos, TVector3 dir, TVector3 n, TVector3 a) {
81+ TVector3 GetPlaneVectorIntersection (const TVector3& pos, const TVector3& dir, const TVector3& n,
82+ const TVector3& a) {
8283 return MoveToPlane (pos, dir, n, a);
8384}
8485
86+ // /////////////////////////////////////////////
87+ // / \brief It returns the cone matrix M = d^T x d - cosTheta^2 x I, extracted from the document
88+ // / by "David Eberly, Geometric Tools, Redmond WA 98052, Intersection of a Line and a Cone".
89+ // /
90+ TMatrixD GetConeMatrix (const TVector3& d, const Double_t& cosTheta) {
91+ double cAxis[3 ];
92+ d.GetXYZ (cAxis);
93+
94+ TVectorD coneAxis (3 , cAxis);
95+
96+ TMatrixD M (3 , 3 );
97+ M.Rank1Update (coneAxis, coneAxis);
98+
99+ double cT2 = cosTheta * cosTheta;
100+ TMatrixD gamma (3 , 3 );
101+ gamma.UnitMatrix ();
102+ gamma *= cT2;
103+
104+ M -= gamma;
105+ return M;
106+ }
107+
108+ // /////////////////////////////////////////////
109+ // / \brief This method will find the intersection of the trajectory defined by the vector starting at
110+ // / `pos` and moving in direction `dir` and the cone defined by its axis vector `d` and the vertex`v`.
111+ // / The cosine of the angle defining the cone should be also given inside the `cosTheta` argument.
112+ // /
113+ // / This method will return `t`, which is the value the particle position, `pos`, needs to be displaced
114+ // / by the vector, `dir`, to get the particle at the surface of the cone. If the particle does not
115+ // / cross the cone, then the value returned will be zero (no particle displacement).
116+ //
117+ // / This method is based on the document by "David Eberly, Geometric Tools, Redmond WA 98052,
118+ // / Intersection of a Line and a Cone".
119+ // /
120+ Double_t GetConeVectorIntersection (const TVector3& pos, const TVector3& dir, const TVector3& d,
121+ const TVector3& v, const Double_t& cosTheta) {
122+ TMatrixD M = GetConeMatrix (d, cosTheta);
123+ return GetConeVectorIntersection (pos, dir, M, d, v);
124+ }
125+
126+ // /////////////////////////////////////////////
127+ // / \brief This method will find the intersection of the trajectory defined by the vector starting at `pos`
128+ // / and moving in direction `dir` and the cone defined by its characteristic matrix `M`, which is built
129+ // / using the cone axis vector `d` as `d^T x d`, and the vertex`v`. The resulting TVector3 will be the
130+ // / position of the particle placed at the cone surface.
131+ // /
132+ // / This method will return `t`, which is the value the particle position, `pos`, needs to be displaced
133+ // / by the vector, `dir`, to get the particle at the surface of the cone. If the particle does not
134+ // / cross the cone, then the value returned will be zero (no particle displacement).
135+ // /
136+ // / This method is based on the document by "David Eberly, Geometric Tools, Redmond WA 98052,
137+ // / Intersection of a Line and a Cone".
138+ // /
139+ Double_t GetConeVectorIntersection (const TVector3& pos, const TVector3& dir, const TMatrixD& M,
140+ const TVector3& axis, const TVector3& v) {
141+ double u[3 ];
142+ dir.GetXYZ (u);
143+ TMatrixD U (3 , 1 , u);
144+ TMatrixD Ut (1 , 3 , u);
145+
146+ double delta[3 ];
147+ TVector3 deltaV = pos - v;
148+ deltaV.GetXYZ (delta);
149+ TMatrixD D (3 , 1 , delta);
150+ TMatrixD Dt (1 , 3 , delta);
151+
152+ TMatrixD C2 = Ut * M * U;
153+ Double_t c2 = C2[0 ][0 ];
154+
155+ TMatrixD C1 = Ut * M * D;
156+ Double_t c1 = C1[0 ][0 ];
157+
158+ TMatrixD C0 = Dt * M * D;
159+ Double_t c0 = C0[0 ][0 ];
160+
161+ Double_t root = c1 * c1 - c0 * c2;
162+ if (root < 0 ) return 0 ;
163+
164+ Double_t t1 = (-c1 + TMath::Sqrt (root)) / c2;
165+ Double_t t2 = (-c1 - TMath::Sqrt (root)) / c2;
166+
167+ // The projections along the cone axis. If positive then the solution
168+ // gives the cone intersection with the side defined by `axis`
169+ Double_t h1 = t1 * dir.Dot (axis) + axis.Dot (deltaV);
170+ Double_t h2 = t2 * dir.Dot (axis) + axis.Dot (deltaV);
171+
172+ // We use it to select the root we are interested in
173+ if (h2 > 0 )
174+ return t2;
175+ else
176+ return t1;
177+ }
178+
85179// /////////////////////////////////////////////
86180// / \brief This method transports a position `pos` by a distance `d` in the direction defined by `dir`.
87181// /
0 commit comments