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