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