250 {
251 CLHEP::HepVector cen1(lp1.center());
252 CLHEP::HepVector cen2(lp2.center());
253 double dx = cen1(1) - cen2(1);
254 double dy = cen1(2) - cen2(2);
255 double dc =
sqrt(dx * dx + dy * dy);
256 if (dc < fabs(0.5 / lp1.kappa()) + fabs(0.5 / lp2.kappa())) {
257 double a1 = sqr(lp1.alpha()) + sqr(lp1.beta());
258 double a2 = sqr(lp2.alpha()) + sqr(lp2.beta());
259 double a3 = lp1.alpha() * lp2.alpha() + lp1.beta() * lp2.beta();
260 double det = lp1.alpha() * lp2.beta() - lp1.beta() * lp2.alpha();
261 if (fabs(det) > 1e-12) {
262 double c1 = a2 * sqr(lp1.kappa()) + a1 * sqr(lp2.kappa()) -
263 2.0 * a3 * lp1.kappa() * lp2.kappa();
264 if (c1 != 0) {
265 double cinv = 1.0 / c1;
266 double c2 = sqr(a3) - 0.5 * (a1 + a2) - 2.0 * a3 *
267 (lp1.gamma() * lp2.kappa() + lp2.gamma() * lp1.kappa());
268 double c3 = a2 * sqr(lp1.gamma()) + a1 * sqr(lp2.gamma()) -
269 2.0 * a3 * lp1.gamma() * lp2.gamma();
270 double root = sqr(c2) - 4.0 * c1 * c3;
271 if (root >= 0) {
273 double rad2[2];
274 rad2[0] = 0.5 * cinv * (-c2 - root);
275 rad2[1] = 0.5 * cinv * (-c2 + root);
276 double ab1 = -(lp2.beta() * lp1.gamma() - lp1.beta() * lp2.gamma());
277 double ab2 = (lp2.alpha() * lp1.gamma() - lp1.alpha() * lp2.gamma());
278 double ac1 = -(lp2.beta() * lp1.kappa() - lp1.beta() * lp2.kappa());
279 double ac2 = (lp2.alpha() * lp1.kappa() - lp1.alpha() * lp2.kappa());
280 double dinv = 1.0 / det;
281 v1(1) = dinv * (ab1 + ac1 * rad2[0]);
282 v1(2) = dinv * (ab2 + ac2 * rad2[0]);
284 v2(1) = dinv * (ab1 + ac1 * rad2[1]);
285 v2(2) = dinv * (ab2 + ac2 * rad2[1]);
287
288
289
290
291
292 TRGCDCLpar::Cpar cp1(lp1);
293 TRGCDCLpar::Cpar cp2(lp2);
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310 return 2;
311 }
312 }
313 }
314 }
315 return 0;
316 }
double sqrt(double a)
sqrt for double
const std::vector< double > v2
MATLAB generated random vector.
const std::vector< double > v1
MATLAB generated random vector.