16 #include "CLHEP/Vector/Sqr.h"
17 #include "trg/cdc/Lpar.h"
44 if (l.alpha() != 0 && l.beta() != 0)
45 m_fi = atan2(l.alpha(), -l.beta());
48 m_da = 2 * l.gamma() / (1 + sqrt(1 + 4 * l.kappa() * l.gamma()));
85 double delta = (x1 - x2) * (y1 - y3) - (y1 - y2) * (x1 - x3);
91 double r12sq = (x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2);
93 double r12 = sqrt(r12sq);
98 double r13sq = (x1 - x3) * (x1 - x3) + (y1 - y3) * (y1 - y3);
100 double r13 = sqrt(r13sq);
101 m_beta = -(x1 - x3) / r13;
105 double r23sq = (x2 - x3) * (x2 - x3) + (y2 - y3) * (y2 - y3);
107 double r23 = sqrt(r23sq);
108 m_beta = -(x2 - x3) / r23;
119 double r1sq = x1 * x1 + y1 * y1;
120 double r2sq = x2 * x2 + y2 * y2;
121 double r3sq = x3 * x3 + y3 * y3;
122 double a = 0.5 * ((y1 - y3) * (r1sq - r2sq) - (y1 - y2) * (r1sq - r3sq)) / delta;
123 double b = 0.5 * (- (x1 - x3) * (r1sq - r2sq) + (x1 - x2) * (r1sq - r3sq)) / delta;
124 double csq = (x1 - a) * (x1 - a) + (y1 - b) * (y1 - b);
125 double c = sqrt(csq);
136 #ifdef BELLE_OPTIMIZED_RETURN
141 CLHEP::HepMatrix vret(3, 4);
147 vret(1, 1) = 2 * cp.da() *
s;
148 vret(1, 2) = -2 * cp.da() * c;
149 vret(1, 3) = cp.da() * cp.da();
155 vret(3, 1) = 2 * cp.cu() *
s;
156 vret(3, 2) = -2 * cp.cu() * c;
164 double t_kr2g =
kr2g(r);
165 double t_xi2 =
xi2();
166 double ro = r * r * t_xi2 - t_kr2g * t_kr2g;
167 if (ro < 0)
return false;
168 double rs = sqrt(ro);
196 if (!
xy(r,
x,
y, dir))
return -1;
197 double p = atan2(
y,
x);
198 if (p < 0) p += (2 * M_PI);
204 double ddm =
dr(
x,
y);
210 double kdp1 = 1 + 2 *
kappa() * ddm;
217 double xh, yh, xx, yy;
219 double fk = fabs(
kappa());
220 if (fk == 0)
return 0;
221 yy = 2 * fk * (
alpha() * yh -
beta() * xh);
223 double sp = atan2(yy, xx);
224 if (sp < 0) sp += (2 * M_PI);
231 if (fabs(r) < fabs(
d0))
return -1;
232 double b = fabs(
kappa()) * sqrt((r * r -
d0 *
d0) / (1 + 2 *
kappa() *
d0));
233 if (fabs(b) > 1)
return -1;
234 if (dir == 0)
return asin(b) / fabs(
kappa());
235 return (M_PI - asin(b)) / fabs(
kappa());
239 #ifdef BELLE_OPTIMIZED_RETURN
244 CLHEP::HepVector v(3);
255 CLHEP::HepVector cen1(lp1.
center());
256 CLHEP::HepVector cen2(lp2.
center());
257 double dx = cen1(1) - cen2(1);
258 double dy = cen1(2) - cen2(2);
259 double dc = sqrt(dx * dx + dy * dy);
260 if (dc < fabs(0.5 / lp1.
kappa()) + fabs(0.5 / lp2.
kappa())) {
261 double a1 = sqr(lp1.
alpha()) + sqr(lp1.
beta());
262 double a2 = sqr(lp2.
alpha()) + sqr(lp2.
beta());
265 if (fabs(det) > 1e-12) {
266 double c1 = a2 * sqr(lp1.
kappa()) + a1 * sqr(lp2.
kappa()) -
269 double cinv = 1.0 / c1;
270 double c2 = sqr(a3) - 0.5 * (a1 + a2) - 2.0 * a3 *
272 double c3 = a2 * sqr(lp1.
gamma()) + a1 * sqr(lp2.
gamma()) -
274 double root = sqr(c2) - 4.0 * c1 * c3;
278 rad2[0] = 0.5 * cinv * (-c2 - root);
279 rad2[1] = 0.5 * cinv * (-c2 + root);
284 double dinv = 1.0 / det;
285 v1(1) = dinv * (ab1 + ac1 * rad2[0]);
286 v1(2) = dinv * (ab2 + ac2 * rad2[0]);
288 v2(1) = dinv * (ab1 + ac1 * rad2[1]);
289 v2(2) = dinv * (ab2 + ac2 * rad2[1]);
333 return o <<
" al=" <<
s.m_alpha <<
" be=" <<
s.m_beta
334 <<
" ka=" <<
s.m_kappa <<
" ga=" <<
s.m_gamma;