00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015 #include <iostream>
00016
00017
00018 #include <cmath>
00019
00020 #include "KalFitAlg/lpav/Lpar.h"
00021 using CLHEP::HepVector;
00022 using CLHEP::Hep3Vector;
00023 using CLHEP::HepMatrix;
00024 using CLHEP::HepSymMatrix;
00025
00026
00027
00028
00029
00030 const double Lpar::BELLE_ALPHA(333.564095);
00031
00032
00033
00034
00035
00036
00037 Lpar::Cpar::Cpar(const Lpar&l) {
00038 m_cu = l.kappa();
00039 if (l.alpha() !=0 && l.beta() !=0)
00040 m_fi = atan2(l.alpha(), -l.beta());
00041 else m_fi = 0;
00042 if(m_fi<0) m_fi+=2*M_PI;
00043 m_da = 2 * l.gamma()/ (1 + sqrt (1 + 4 * l.kappa() * l.gamma()));
00044 m_cfi = cos(m_fi);
00045 m_sfi = sin(m_fi);
00046 }
00047
00048
00049
00050
00051
00052 Lpar::~Lpar()
00053 {
00054 }
00055
00056
00057
00058
00059
00060
00061
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071
00072
00073
00074
00075
00076
00077 void Lpar::circle(double x1, double y1, double x2, double y2,
00078 double x3, double y3) {
00079 double a;
00080 double b;
00081 double c;
00082 double delta = (x1-x2)*(y1-y3) - (y1-y2)*(x1-x3);
00083 if(delta==0) {
00084
00085
00086
00087 m_kappa = 0;
00088 double r12sq = (x1-x2)*(x1-x2) + (y1-y2)*(y1-y2);
00089 if (r12sq>0) {
00090 double r12 = sqrt(r12sq);
00091 m_beta = -(x1-x2)/r12;
00092 m_alpha = (y1-y2)/r12;
00093 m_gamma = - (m_alpha*x1+m_beta*y1);
00094 } else {
00095 double r13sq = (x1-x3)*(x1-x3) + (y1-y3)*(y1-y3);
00096 if (r13sq>0) {
00097 double r13 = sqrt(r13sq);
00098 m_beta = -(x1-x3)/r13;
00099 m_alpha = (y1-y3)/r13;
00100 m_gamma = - (m_alpha*x3+m_beta*y3);
00101 } else {
00102 double r23sq = (x2-x3)*(x2-x3) + (y2-y3)*(y2-y3);
00103 if (r23sq>0) {
00104 double r23 = sqrt(r23sq);
00105 m_beta = -(x2-x3)/r23;
00106 m_alpha = (y2-y3)/r23;
00107 m_gamma = - (m_alpha*x3+m_beta*y3);
00108 } else {
00109 m_alpha = 1;
00110 m_beta = 0;
00111 m_gamma = 0;
00112 }
00113 }
00114 }
00115 } else {
00116 double r1sq = x1 * x1 + y1 * y1;
00117 double r2sq = x2 * x2 + y2 * y2;
00118 double r3sq = x3 * x3 + y3 * y3;
00119 a = 0.5 * ( (y1-y3)*(r1sq-r2sq) - (y1-y2)*(r1sq-r3sq)) / delta;
00120 b = 0.5 * (- (x1-x3)*(r1sq-r2sq) + (x1-x2)*(r1sq-r3sq)) / delta;
00121 double csq = (x1-a)*(x1-a) + (y1-b)*(y1-b);
00122 c = sqrt(csq);
00123 double csq2 = (x2-a)*(x2-a) + (y2-b)*(y2-b);
00124 double csq3 = (x3-a)*(x3-a) + (y3-b)*(y3-b);
00125 m_kappa = 1 / (2 * c);
00126 m_alpha = - 2 * a * m_kappa;
00127 m_beta = - 2 * b * m_kappa;
00128 m_gamma = (a*a + b*b - c*c) * m_kappa;
00129 }
00130 }
00131
00132 HepMatrix Lpar::dldc() const
00133 #ifdef BELLE_OPTIMIZED_RETURN
00134 return vret(3,4);
00135 {
00136 #else
00137 {
00138 HepMatrix vret(3,4);
00139 #endif
00140 Cpar cp(*this);
00141 double xi = cp.xi();
00142 double s = cp.sfi();
00143 double c = cp.cfi();
00144 vret(1,1) = 2*cp.da()*s;
00145 vret(1,2) = -2*cp.da()*c;
00146 vret(1,3) = cp.da()*cp.da();
00147 vret(1,4) = 1;
00148 vret(2,1) = xi*c;
00149 vret(2,2) = xi*s;
00150 vret(2,3) = 0;
00151 vret(2,4) = 0;
00152 vret(3,1) = 2*cp.cu()*s;
00153 vret(3,2) = -2*cp.cu()*c;
00154 vret(3,3) = xi;
00155 vret(3,4) = 0;
00156 return vret;
00157 }
00158
00159 bool Lpar::xy(double r, double &x, double &y, int dir) const {
00160 double t_kr2g = kr2g(r);
00161 double t_xi2 = xi2();
00162 double ro = r * r * t_xi2 - t_kr2g * t_kr2g;
00163 if ( ro < 0 ) return false;
00164 double rs = sqrt(ro);
00165 if(dir==0) {
00166 x = (- m_alpha * t_kr2g - m_beta * rs) / t_xi2;
00167 y = (- m_beta * t_kr2g + m_alpha * rs) / t_xi2;
00168 } else {
00169 x = (- m_alpha * t_kr2g + m_beta * rs) / t_xi2;
00170 y = (- m_beta * t_kr2g - m_alpha * rs) / t_xi2;
00171 }
00172 return true;
00173 }
00174
00175 double Lpar::x(double r) const {
00176 double t_x, t_y;
00177 xy(r, t_x, t_y);
00178 return t_x;
00179 }
00180
00181 double Lpar::y(double r) const {
00182 double t_x, t_y;
00183 xy(r, t_x, t_y);
00184 return t_y;
00185 }
00186
00187 double Lpar::phi(double r,int dir) const {
00188 double x, y;
00189 if (!xy(r,x,y, dir)) return -1;
00190 double p = atan2(y,x);
00191 if (p<0) p += (2*M_PI);
00192 return p;
00193 }
00194
00195 void Lpar::xhyh(double x, double y, double &xh, double &yh) const {
00196 double ddm = dr(x, y);
00197 if (ddm==0) {
00198 xh = x;
00199 yh = y;
00200 return;
00201 }
00202 double kdp1 = 1 + 2 * kappa() * ddm;
00203 xh = x - ddm * ( 2 * kappa() * x + alpha())/kdp1;
00204 yh = y - ddm * ( 2 * kappa() * y + beta())/kdp1;
00205 }
00206
00207 double Lpar::s(double x, double y) const {
00208 double xh, yh, xx, yy;
00209 xhyh(x, y, xh, yh);
00210 double fk = fabs(kappa());
00211 if (fk==0) return 0;
00212 yy = 2 * fk * ( alpha() * yh - beta() * xh);
00213 xx = 2 * kappa() * ( alpha() * xh + beta() * yh ) + xi2();
00214 double sp = atan2(yy, xx);
00215 if (sp<0) sp += (2*M_PI);
00216 return sp / 2 / fk;
00217 }
00218
00219 double Lpar::s(double r, int dir) const {
00220 double d0 = da();
00221 if (fabs(r)<fabs(d0)) return -1;
00222 double b = fabs(kappa()) * sqrt((r*r-d0*d0)/(1 + 2 * kappa() * d0));
00223 if (fabs(b)>1) return -1;
00224 if(dir==0)return asin(b)/fabs(kappa());
00225 return (M_PI-asin(b))/fabs(kappa());
00226 }
00227
00228 HepVector Lpar::center() const
00229 #ifdef BELLE_OPTIMIZED_RETURN
00230 return v(3);
00231 {
00232 #else
00233 {
00234 HepVector v(3);
00235 #endif
00236 v(1) = xc();
00237 v(2) = yc();
00238 v(3) = 0;
00239 return(v);
00240 }
00241
00242 int intersect(const Lpar&lp1, const Lpar&lp2, HepVector&v1, HepVector&v2) {
00243 HepVector cen1(lp1.center());
00244 HepVector cen2(lp2.center());
00245 double dx = cen1(1)-cen2(1);
00246 double dy = cen1(2)-cen2(2);
00247 double dc = sqrt(dx*dx+dy*dy);
00248 if(dc<fabs(0.5/lp1.kappa())+fabs(0.5/lp2.kappa())) {
00249 double a1 = std::sqrt(lp1.alpha()) + std::sqrt(lp1.beta());
00250 double a2 = std::sqrt(lp2.alpha()) + std::sqrt(lp2.beta());
00251 double a3 = lp1.alpha()*lp2.alpha() + lp1.beta()*lp2.beta();
00252 double det = lp1.alpha()*lp2.beta() - lp1.beta()*lp2.alpha();
00253 if(fabs(det)>1e-12) {
00254 double c1 = a2 * std::sqrt(lp1.kappa()) + a1 * std::sqrt(lp2.kappa()) -
00255 2.0 * a3 * lp1.kappa() * lp2.kappa();
00256 if(c1!=0) {
00257 double cinv = 1.0 / c1;
00258 double c2 = std::sqrt(a3) - 0.5 * (a1 + a2) - 2.0 * a3 *
00259 (lp1.gamma() * lp2.kappa() + lp2.gamma() * lp1.kappa());
00260 double c3 = a2 * std::sqrt(lp1.gamma()) + a1 * std::sqrt(lp2.gamma()) -
00261 2.0 * a3 * lp1.gamma() * lp2.gamma();
00262 double root = std::sqrt(c2) - 4.0 * c1 * c3;
00263 if (root>=0) {
00264 root = sqrt(root);
00265 double rad2[2];
00266 rad2[0] = 0.5 * cinv * (-c2 - root);
00267 rad2[1] = 0.5 * cinv * (-c2 + root);
00268 double ab1 = -(lp2.beta() * lp1.gamma() - lp1.beta() * lp2.gamma());
00269 double ab2 = (lp2.alpha() * lp1.gamma() - lp1.alpha() * lp2.gamma());
00270 double ac1 = -(lp2.beta() * lp1.kappa() - lp1.beta() * lp2.kappa());
00271 double ac2 = (lp2.alpha() * lp1.kappa() - lp1.alpha() * lp2.kappa());
00272 double dinv = 1.0 / det;
00273 v1(1) = dinv * (ab1 + ac1 * rad2[0]);
00274 v1(2) = dinv * (ab2 + ac2 * rad2[0]);
00275 v1(3) = 0;
00276 v2(1) = dinv * (ab1 + ac1 * rad2[1]);
00277 v2(2) = dinv * (ab2 + ac2 * rad2[1]);
00278 v2(3) = 0;
00279 double d1 = lp1.d(v1(1),v1(2));
00280 double d2 = lp2.d(v1(1),v1(2));
00281 double d3 = lp1.d(v2(1),v2(2));
00282 double d4 = lp2.d(v2(1),v2(2));
00283 double r = sqrt(rad2[0]);
00284 Lpar::Cpar cp1(lp1);
00285 Lpar::Cpar cp2(lp2);
00286 for(int j=0;j<2;j++) {
00287 double s1,s2;
00288 if(j==0) {
00289 s1 = lp1.s(v1(1),v1(2));
00290 s2 = lp2.s(v1(1),v1(2));
00291 } else {
00292 s1 = lp1.s(v2(1),v2(2));
00293 s2 = lp2.s(v2(1),v2(2));
00294 }
00295 double phi1 = cp1.fi() + 2 * cp1.cu() * s1;
00296 double phi2 = cp2.fi() + 2 * cp2.cu() * s2;
00297 double f = (1 + 2 * cp1.cu() * cp1.da()) *
00298 (1 + 2 * cp2.cu() * cp2.da()) * cos(cp1.fi()-cp2.fi());
00299 f -= 2 * (lp1.gamma() * lp2.kappa() + lp2.gamma() * lp1.kappa());
00300 double cosphi12 = f;
00301 }
00302 return 2;
00303 }
00304 }
00305 }
00306 }
00307 return 0;
00308 }
00309
00310
00311
00312
00313
00314
00315
00316
00317
00318 std::ostream& operator<<(std::ostream &o, Lpar &s) {
00319 return o << " al=" << s.m_alpha << " be=" << s.m_beta
00320 << " ka=" << s.m_kappa << " ga=" << s.m_gamma;
00321 }
00322