00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 #include <cmath>
00018 #include <iostream>
00019
00020
00021 #include "TrackUtil/Lpav.h"
00022
00023
00024
00025
00026
00027 extern "C" {
00028 float prob_ (float *, int*);
00029 }
00030
00031
00032 static double err_dis_inv(double x, double y, double w, double a, double b) {
00033 if (a==0 && b==0) {
00034 return w;
00035 } else {
00036 double f = x * b - y * a;
00037 double rsq = x*x+y*y;
00038 f *= f;
00039 return w*rsq/f;
00040 }
00041 }
00042
00043
00044
00045
00046
00047
00048
00049
00050 Lpav::Lpav()
00051 {
00052 clear();
00053 }
00054
00055
00056
00057
00058
00059 Lpav::~Lpav()
00060 {
00061 }
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071
00072
00073
00074
00075
00076
00077
00078
00079
00080
00081
00082
00083
00084 void Lpav::calculate_average(double xi, double yi, double wi) {
00085 if(m_wsum<=0) return;
00086 m_wsum_temp = m_wsum + wi;
00087 double rri(xi * xi + yi * yi);
00088 double wrri(wi * rri);
00089 double wsum_inv(1/m_wsum_temp);
00090 m_xav = (m_xsum + wi * xi) * wsum_inv;
00091 m_yav = (m_ysum + wi * yi) * wsum_inv;
00092
00093 double xxav((m_xxsum + wi * xi * xi) * wsum_inv);
00094 double yyav((m_yysum + wi * yi * yi) * wsum_inv);
00095 double xyav((m_xysum + wi * xi * yi) * wsum_inv);
00096 double xrrav((m_xrrsum + xi * wrri) * wsum_inv);
00097 double yrrav((m_yrrsum + yi * wrri) * wsum_inv);
00098 double rrrrav((m_rrrrsum + wrri * rri) * wsum_inv);
00099
00100 calculate_average_n(xxav, yyav, xyav, xrrav, yrrav, rrrrav);
00101
00102 }
00103
00104 void Lpav::calculate_average(void) {
00105 if(m_wsum<=0) return;
00106 m_wsum_temp = m_wsum;
00107 double wsum_inv(1/m_wsum_temp);
00108 m_xav = m_xsum * wsum_inv;
00109 m_yav = m_ysum * wsum_inv;
00110
00111 double xxav(m_xxsum * wsum_inv);
00112 double yyav(m_yysum * wsum_inv);
00113 double xyav(m_xysum * wsum_inv);
00114 double xrrav(m_xrrsum * wsum_inv);
00115 double yrrav(m_yrrsum * wsum_inv);
00116 double rrrrav(m_rrrrsum * wsum_inv);
00117
00118 calculate_average_n(xxav, yyav, xyav, xrrav, yrrav, rrrrav);
00119 }
00120
00121 void Lpav::calculate_average_n(double xxav, double yyav, double xyav,
00122 double xrrav, double yrrav, double rrrrav) {
00123 double xxav_p = xxav - m_xav * m_xav;
00124 double yyav_p = yyav - m_yav * m_yav;
00125 double xyav_p = xyav - m_xav * m_yav;
00126 double rrav_p = xxav_p + yyav_p;
00127
00128 double a = std::fabs(xxav_p - yyav_p);
00129 double b = 4 * xyav_p * xyav_p;
00130 double asqpb = a * a + b;
00131 double rasqpb = std::sqrt(asqpb);
00132 double splus = 1 + a / rasqpb;
00133 double sminus = b / (asqpb*splus);
00134 splus = std::sqrt(0.5*splus);
00135 sminus = std::sqrt(0.5*sminus);
00136
00137
00138
00139 if ( xxav_p <= yyav_p ) {
00140 m_cosrot = sminus;
00141 m_sinrot = splus;
00142 } else {
00143 m_cosrot = splus;
00144 m_sinrot = sminus;
00145 }
00146
00147
00148
00149 if (xyav_p < 0) m_sinrot = - m_sinrot;
00150
00151
00152
00153
00154
00155
00156
00157
00158
00159 if ( m_cosrot*m_xav + m_sinrot*m_yav <= 0 ) {
00160 m_cosrot = - m_cosrot;
00161 m_sinrot = - m_sinrot;
00162 }
00163 m_rscale = std::sqrt(rrav_p);
00164 double cos2 = m_cosrot * m_cosrot;
00165 double sin2 = m_sinrot * m_sinrot;
00166 double cs2 = 2 * m_sinrot * m_cosrot;
00167 double rrav_p_inv(1/rrav_p);
00168 m_xxavp = (cos2 * xxav_p + cs2 * xyav_p + sin2 * yyav_p) * rrav_p_inv;
00169 m_yyavp = (cos2 * yyav_p - cs2 * xyav_p + sin2 * xxav_p) * rrav_p_inv;
00170
00171 double xav2 = m_xav * m_xav;
00172 double yav2 = m_yav * m_yav;
00173 double xrrav_p = (xrrav - 2 * xxav * m_xav + xav2 * m_xav -
00174 2 * xyav * m_yav + m_xav * yav2) - m_xav * rrav_p;
00175 double yrrav_p = (yrrav - 2 * yyav * m_yav + yav2 * m_yav -
00176 2 * xyav * m_xav + m_yav * xav2) - m_yav * rrav_p;
00177 m_xrravp = ( m_cosrot * xrrav_p + m_sinrot * yrrav_p) * rrav_p_inv/m_rscale;
00178 m_yrravp = (- m_sinrot * xrrav_p + m_cosrot * yrrav_p) * rrav_p_inv/m_rscale;
00179
00180 double rrav = xxav + yyav;
00181 double rrrrav_p = rrrrav
00182 - 2 * m_yav * yrrav - 2 * m_xav * xrrav
00183 + rrav * (xav2 + yav2)
00184 - 2 * m_xav * xrrav_p - xav2 * rrav_p
00185 - 2 * m_yav * yrrav_p - yav2 * rrav_p;
00186 m_rrrravp = rrrrav_p * rrav_p_inv * rrav_p_inv;
00187 m_xyavp = 0;
00188 }
00189
00190 void Lpav::calculate_average3(double xi, double yi, double wi) {
00191 if(m_wsum<=0) return;
00192 m_wsum_temp = m_wsum + wi;
00193 double wsum_inv(1/m_wsum_temp);
00194 double rri(xi * xi + yi * yi);
00195 m_xav = (m_xsum + wi * xi) * wsum_inv;
00196 m_yav = (m_ysum + wi * yi) * wsum_inv;
00197
00198 m_rscale = 1;
00199 m_cosrot = 1;
00200 m_sinrot = 0;
00201 m_xxavp = (m_xxsum + wi * xi * xi) * wsum_inv;
00202 m_xyavp = (m_xysum + wi * xi * yi) * wsum_inv;
00203 m_yyavp = (m_yysum + wi * yi * yi) * wsum_inv;
00204 double wrri(wi * rri);
00205 m_xrravp = (m_xrrsum + xi * wrri) * wsum_inv;
00206 m_yrravp = (m_yrrsum + yi * wrri) * wsum_inv;
00207 m_rrrravp = (m_rrrrsum + rri * wrri) * wsum_inv;
00208 }
00209
00210 void Lpav::calculate_average3(void) {
00211 if(m_wsum<=0) return;
00212 m_wsum_temp = m_wsum;
00213 double wsum_inv(1/m_wsum_temp);
00214 m_xav = m_xsum * wsum_inv;
00215 m_yav = m_ysum * wsum_inv;
00216
00217 m_rscale = 1;
00218 m_cosrot = 1;
00219 m_sinrot = 0;
00220 m_xxavp = m_xxsum * wsum_inv;
00221 m_xyavp = m_xysum * wsum_inv;
00222 m_yyavp = m_yysum * wsum_inv;
00223 m_xrravp = m_xrrsum * wsum_inv;
00224 m_yrravp = m_yrrsum * wsum_inv;
00225 m_rrrravp = m_rrrrsum * wsum_inv;
00226 }
00227
00228
00229
00230
00231
00232
00233
00234
00235
00236
00237
00238 std::ostream &operator<<(std::ostream &o, const Lpav &a) {
00239
00240
00241
00242
00243
00244
00245
00246
00247
00248
00249
00250 o << " nc=" << a.m_nc << " chisq=" << a.m_chisq << " " << (Lpar&) a;
00251 return o;
00252 }
00253
00254 double Lpav::solve_lambda(void) {
00255 if (m_rscale<=0) return -1;
00256 double xrrxrr = m_xrravp * m_xrravp;
00257 double yrryrr = m_yrravp * m_yrravp;
00258 double rrrrm1 = m_rrrravp - 1;
00259 double xxyy = m_xxavp * m_yyavp;
00260
00261 double c0 = rrrrm1 * xxyy - xrrxrr * m_yyavp - yrryrr * m_xxavp;
00262 double c1 = - rrrrm1 + xrrxrr + yrryrr - 4 * xxyy;
00263 double c2 = 4 + rrrrm1 - 4 * xxyy;
00264 double c4 = - 4;
00265
00266
00267
00268 double c2d = 2 * c2;
00269 double c4d = 4 * c4;
00270
00271 double lambda = 0;
00272
00273 double chiscl = m_wsum_temp * m_rscale * m_rscale;
00274 double dlamax = 0.001 / chiscl;
00275 const int ntry = 5;
00276 int itry = 0;
00277 double dlambda = dlamax;
00278 while ( itry<ntry && std::fabs(dlambda) >= dlamax) {
00279 double cpoly = c0 + lambda * ( c1 + lambda *
00280 ( c2 + lambda * lambda * c4));
00281 double dcpoly = c1 + lambda * ( c2d + lambda * lambda * c4d);
00282 dlambda = - cpoly / dcpoly;
00283 lambda += dlambda;
00284 itry ++;
00285 }
00286 lambda = lambda<0 ? 0 : lambda;
00287 return lambda;
00288 }
00289
00290 double Lpav::solve_lambda3(void) {
00291 if (m_rscale<=0) return -1;
00292 double xrrxrr = m_xrravp * m_xrravp;
00293 double yrryrr = m_yrravp * m_yrravp;
00294 double rrrrm1 = m_rrrravp - 1;
00295 double xxyy = m_xxavp * m_yyavp;
00296
00297 double a = m_rrrravp;
00298 double b = xrrxrr + yrryrr - m_rrrravp * (m_xxavp + m_yyavp);
00299 double c = m_rrrravp * m_xxavp * m_yyavp
00300 - m_yyavp * xrrxrr - m_xxavp * yrryrr
00301 + 2 * m_xyavp * m_xrravp * m_yrravp - m_rrrravp * m_xyavp * m_xyavp;
00302 if (c>=0 && b<=0) {
00303 return (-b-std::sqrt(b*b-4*a*c))/2/a;
00304 } else if (c>=0 && b>0) {
00305 std::cerr << " returning " <<-1<<std::endl;
00306 return -1;
00307 } else if (c<0) {
00308 return (-b+std::sqrt(b*b-4*a*c))/2/a;
00309 }
00310 return -1;
00311 }
00312
00313 double Lpav::calculate_lpar(void) {
00314 double lambda = solve_lambda();
00315
00316
00317 if (lambda<0) return -1;
00318 double h11 = m_xxavp - lambda;
00319 double h22 = m_yyavp - lambda;
00320 if (h11==0.0) return -1;
00321 double h14 = m_xrravp;
00322 double h24 = m_yrravp;
00323 double h34 = 1 + 2 * lambda;
00324 double rootsq = (h14*h14/h11/h11) + 4 * h34;
00325 if ( std::fabs(h22) > std::fabs(h24) ) {
00326 if(h22==0.0) return -1;
00327 double ratio = h24/h22;
00328 rootsq += ratio * ratio ;
00329 m_kappa = 1/std::sqrt(rootsq);
00330 m_beta = - ratio * m_kappa;
00331 } else {
00332 if(h24==0.0) return -1;
00333 double ratio = h22 / h24;
00334 rootsq = 1 + ratio * ratio * rootsq;
00335 m_beta = 1 / std::sqrt(rootsq);
00336 m_beta = h24>0 ? -m_beta : m_beta;
00337 m_kappa = -ratio * m_beta;
00338 }
00339 m_alpha = - (h14/h11)*m_kappa;
00340 m_gamma = - h34 * m_kappa;
00341
00342
00343
00344
00345
00346
00347
00348
00349
00350
00351
00352 scale(m_rscale);
00353
00354
00355
00356 rotate(m_cosrot, -m_sinrot);
00357
00358
00359
00360 move(-m_xav, -m_yav);
00361 if (m_yrravp < 0) neg();
00362 if (lambda>=0) m_chisq = lambda * m_wsum_temp * m_rscale * m_rscale;
00363 return lambda;
00364 }
00365
00366 double Lpav::calculate_lpar3(void) {
00367 double lambda = solve_lambda3();
00368
00369
00370 if (lambda<0) return -1;
00371 double h11 = m_xxavp - lambda;
00372 double h22 = m_yyavp - lambda;
00373 double h14 = m_xrravp;
00374 double h24 = m_yrravp;
00375 m_gamma = 0;
00376 double h12 = m_xyavp;
00377 double det = h11*h22-h12*h12;
00378 if (det!=0) {
00379 double r1 = (h14*h22-h24*h12)/(det);
00380 double r2 = (h24*h11-h14*h12)/(det);
00381 double kinvsq = r1*r1 + r2*r2;
00382 m_kappa = std::sqrt(1/kinvsq);
00383 if(h11!=0) m_alpha = -m_kappa * r1;
00384 else m_alpha = 1;
00385 if(h22!=0) m_beta = -m_kappa * r2;
00386 else m_beta = 1;
00387 } else {
00388 m_kappa = 0;
00389 if (h11!=0 && h22!=0) {
00390 m_beta = 1/std::sqrt(1+h12*h12/h11/h11);
00391 m_alpha = std::sqrt(1-m_beta*m_beta);
00392 } else if (h11!=0) {
00393 m_beta = 1;
00394 m_alpha = 0;
00395 } else {
00396 m_beta = 0;
00397 m_alpha = 1;
00398 }
00399 }
00400 if((m_alpha*m_xav + m_beta*m_yav) *
00401 (m_beta*m_xav - m_alpha*m_yav)<0) neg();
00402
00403
00404
00405 if (lambda>=0) m_chisq = lambda * m_wsum_temp * m_rscale * m_rscale;
00406 return lambda;
00407 }
00408
00409 double Lpav::fit(double x, double y, double w) {
00410 if (m_nc<=3) return -1;
00411 m_chisq = -1;
00412 double q;
00413 if (m_nc<4) {
00414 calculate_average3(x,y,w);
00415 double q = calculate_lpar3();
00416 if (q>0) m_chisq = q * m_wsum_temp * m_rscale * m_rscale;
00417 } else {
00418 calculate_average(x,y,w);
00419 q = calculate_lpar();
00420 if (q>0) m_chisq = q * m_wsum_temp * m_rscale * m_rscale;
00421 }
00422 return m_chisq;
00423 }
00424
00425 double Lpav::fit(void) {
00426 if (m_nc<=3) return -1;
00427 m_chisq = -1;
00428 double q;
00429 if (m_nc<4) {
00430 calculate_average3();
00431 q = calculate_lpar3();
00432 if (q>0) m_chisq = q * m_wsum_temp * m_rscale * m_rscale;
00433 } else {
00434 calculate_average();
00435 q = calculate_lpar();
00436 if (q>0) m_chisq = q * m_wsum_temp * m_rscale * m_rscale;
00437 }
00438 return m_chisq;
00439 }
00440
00441 HepSymMatrix Lpav::cov(int inv) const
00442 #ifdef BELLE_OPTIMIZED_RETURN
00443 return vret(4);
00444 {
00445 #else
00446 {
00447 HepSymMatrix vret(4);
00448 #endif
00449 vret(1,1) = m_xxsum;
00450 vret(2,1) = m_xysum;
00451 vret(2,2) = m_yysum;
00452 vret(3,1) = m_xsum;
00453 vret(3,2) = m_ysum;
00454 vret(3,3) = m_wsum;
00455 vret(4,1) = m_xrrsum;
00456 vret(4,2) = m_yrrsum;
00457 vret(4,3) = m_xxsum + m_yysum;
00458 vret(4,4) = m_rrrrsum;
00459 if(inv==0) {
00460
00461 int i;
00462 vret.invert(i);
00463 if (i!=0) {
00464 std::cerr << "Lpav::cov:could not invert nc=" << m_nc << vret;
00465 #ifdef HAVE_EXCEPTION
00466 THROW(Lpav::cov,Singular);
00467 #endif
00468 }
00469 }
00470 return vret;
00471 }
00472
00473 HepSymMatrix Lpav::cov_c(int inv) const
00474 #ifdef BELLE_OPTIMIZED_RETURN
00475 return vret(3);
00476 {
00477 #else
00478 {
00479 HepSymMatrix vret(3);
00480 #endif
00481 #ifdef HAVE_EXCEPTION
00482 try {
00483 #endif
00484 vret = cov(1).similarity(dldc());
00485 #ifdef HAVE_EXCEPTION
00486 }
00487 catch (Lpav::Singular) {
00488 THROW(Lpav::cov_c1,Singular_c);
00489 }
00490 #endif
00491 if(inv==0) {
00492
00493 int i;
00494 vret.invert(i);
00495 if (i!=0) {
00496 std::cerr << "Lpav::cov_c:could not invert " << vret;
00497 #ifdef HAVE_EXCEPTION
00498 THROW(Lpav::cov_c2,Singular_c);
00499 #endif
00500 }
00501 }
00502 return vret;
00503 }
00504
00505 int Lpav::extrapolate(double r, double&phi, double &dphi) const {
00506 double x, y;
00507 if (m_chisq<0) return -1;
00508 if (xy(r, x, y)!=0) return -1;
00509 phi = std::atan2(y,x);
00510 if (phi<0) phi += (2*M_PI);
00511 HepVector v(4);
00512 v(1) = x;
00513 v(2) = y;
00514 v(3) = 1;
00515 v(4) = r * r;
00516
00517 #ifdef HAVE_EXCEPTION
00518 try {
00519 #endif
00520
00521
00522
00523 double l = cov().similarity(v);
00524 if(l>0) {
00525 double ls = std::sqrt(l);
00526 dphi = ls / r;
00527
00528 }
00529 #ifdef HAVE_EXCEPTION
00530 }
00531 catch (Lpav::Singular) {
00532 return -1;
00533 }
00534 #endif
00535
00536 return 0;
00537 }
00538
00539 double Lpav::similarity(double x, double y) const {
00540 if (m_nc<=3) return -1;
00541 HepVector v(4);
00542 v(1) = x;
00543 v(2) = y;
00544 v(3) = 1;
00545 v(4) = x * x + y * y;
00546 double l;
00547 #ifdef HAVE_EXCEPTION
00548 try {
00549 #endif
00550 l = cov().similarity(v);
00551 #ifdef HAVE_EXCEPTION
00552 }
00553 catch (Lpav::Singular) {
00554 return -1;
00555 }
00556 #endif
00557 return l;
00558 }
00559
00560 void Lpav::add(double xi, double yi, double w, double a, double b) {
00561 register double wi = err_dis_inv(xi, yi, w, a, b);
00562 add(xi, yi, wi);
00563 }
00564
00565 void Lpav::add_point(register double xi, register double yi,
00566 register double wi) {
00567 m_wsum += wi;
00568 m_xsum += wi * xi;
00569 m_ysum += wi * yi;
00570 m_xxsum += wi * xi * xi;
00571 m_yysum += wi * yi * yi;
00572 m_xysum += wi * xi * yi;
00573 register double rri = ( xi * xi + yi * yi );
00574 register double wrri = wi * rri;
00575 m_xrrsum += wrri * xi;
00576 m_yrrsum += wrri * yi;
00577 m_rrrrsum += wrri * rri;
00578 m_nc += 1;
00579 }
00580
00581 void Lpav::add_point_frac(double xi, double yi, double w, double a) {
00582 register double wi = w * a;
00583 m_wsum += wi;
00584 m_xsum += wi * xi;
00585 m_ysum += wi * yi;
00586 m_xxsum += wi * xi * xi;
00587 m_yysum += wi * yi * yi;
00588 m_xysum += wi * xi * yi;
00589 register double rri = ( xi * xi + yi * yi );
00590 register double wrri = wi * rri;
00591 m_xrrsum += wrri * xi;
00592 m_yrrsum += wrri * yi;
00593 m_rrrrsum += wrri * rri;
00594 m_nc += a;
00595 }
00596
00597 void Lpav::sub(double xi, double yi, double w, double a, double b) {
00598 register double wi = err_dis_inv(xi, yi, w, a, b);
00599 m_wsum -= wi;
00600 m_xsum -= wi * xi;
00601 m_ysum -= wi * yi;
00602 m_xxsum -= wi * xi * xi;
00603 m_yysum -= wi * yi * yi;
00604 m_xysum -= wi * xi * yi;
00605 register double rri = ( xi * xi + yi * yi );
00606 register double wrri = wi * rri;
00607 m_xrrsum -= wrri * xi;
00608 m_yrrsum -= wrri * yi;
00609 m_rrrrsum -= wrri * rri;
00610 m_nc -= 1;
00611 }
00612
00613 const Lpav & Lpav::operator+=(const Lpav &la1) {
00614 m_wsum += la1.m_wsum;
00615 m_xsum += la1.m_xsum;
00616 m_ysum += la1.m_ysum;
00617 m_xxsum += la1.m_xxsum;
00618 m_yysum += la1.m_yysum;
00619 m_xysum += la1.m_xysum;
00620 m_xrrsum += la1.m_xrrsum;
00621 m_yrrsum += la1.m_yrrsum;
00622 m_rrrrsum += la1.m_rrrrsum;
00623 m_nc += la1.m_nc;
00624 return *this;
00625 }
00626
00627 Lpav operator+(const Lpav &la1, const Lpav &la2)
00628 #ifdef BELLE_OPTIMIZED_RETURN
00629 return la;
00630 {
00631 #else
00632 {
00633 Lpav la;
00634 #endif
00635 la.m_wsum = la1.m_wsum + la2.m_wsum;
00636 la.m_xsum = la1.m_xsum + la2.m_xsum;
00637 la.m_ysum = la1.m_ysum + la2.m_ysum;
00638 la.m_xxsum = la1.m_xxsum + la2.m_xxsum;
00639 la.m_yysum = la1.m_yysum + la2.m_yysum;
00640 la.m_xysum = la1.m_xysum + la2.m_xysum;
00641 la.m_xrrsum = la1.m_xrrsum + la2.m_xrrsum;
00642 la.m_yrrsum = la1.m_yrrsum + la2.m_yrrsum;
00643 la.m_rrrrsum = la1.m_rrrrsum + la2.m_rrrrsum;
00644 la.m_nc = la1.m_nc + la2.m_nc;
00645 return la;
00646 }
00647
00648 double Lpav::prob() const {
00649 if (m_nc<=3) return 0;
00650 if (m_chisq<0) return 0;
00651 float c = m_chisq;
00652 int nci = (int)m_nc - 3;
00653 double p = (double) prob_(&c, &nci);
00654 return p;
00655 }
00656
00657 double Lpav::chi_deg() const {
00658 if (m_nc<=3) return -1;
00659 else return m_chisq/(m_nc-3);
00660 }
00661
00662 double Lpav::delta_chisq(double x, double y, double w) const {
00663 double sim = similarity(x,y);
00664 if(sim<0) return -1;
00665 double d = d0(x,y);
00666 double delta = std::sqrt(d) * w / (1 + sim * w);
00667 return delta;
00668 }
00669