CGEM BOSS 6.6.5.i
BESIII Offline Software System
Loading...
Searching...
No Matches
Alignment.cxx
Go to the documentation of this file.
1#include <iostream>
2#include <iomanip>
3#include <string>
4#include <vector>
5#include <math.h>
6#include "MdcAlignAlg/Alignment.h"
7#include "MdcAlignAlg/MdcAlignPar.h"
8
9#include "CLHEP/Matrix/Vector.h"
10#include "CLHEP/Matrix/SymMatrix.h"
11
12using namespace std;
13using namespace CLHEP;
14
15bool Alignment::gFlagMag;
16int Alignment::gNiter;
17
18int Alignment::getEpId(int lay, int iEnd){
19 int i;
20 if(lay < 8) i = 0;
21 else if(lay < 10) i = 1;
22 else if(lay < 12) i = 2;
23 else if(lay < 14) i = 3;
24 else if(lay < 16) i = 4;
25 else if(lay < 18) i = 5;
26 else if(lay < 20) i = 6;
27 else i = 7;
28
29 int iEP;
30 if(1 == iEnd) iEP = i; // east
31 else return iEP = i + 8; // west
32 return iEP;
33}
34
35void Alignment::expd(double veca[3], double vecb[3], double val[3]){
36 val[0] = veca[1]*vecb[2] - veca[2]*vecb[1];
37 val[1] = veca[2]*vecb[0] - veca[0]*vecb[2];
38 val[2] = veca[0]*vecb[1] - veca[1]*vecb[0];
39}
40
41int Alignment::dist2Line(double sta[3], double stb[3],
42 double veca[3], double vecb[3],
43 double &d, double &za, double &zb, int fgZcal){
44 int i;
45 double vst[3]; // P0 - W0
46 double vp[3]; // (P * W) / |P * W|
47 double modvp;
48 double m2;
49 double seca[3], secb[3];
50 double tpa = 0.0;
51 double twb = 0.0;
52
53 for(i=0; i<3; i++) vst[i] = sta[i] - stb[i]; // vector P0-W0
54
55 Alignment::expd(veca, vecb, vp); // exterior product
56
57 m2 = vp[0]*vp[0] + vp[1]*vp[1] + vp[2]*vp[2];
58 modvp = sqrt(m2);
59 for(i=0; i<3; i++) vp[i] /= modvp; // (P * W) / |P * W|
60
61 d = 0.0;
62 for(i=0; i<3; i++) d += vst[i] * vp[i];
63
64 if( 0 == fgZcal ) return 1;
65
66 double veca00 = veca[0]*veca[0];
67 double veca11 = veca[1]*veca[1];
68 double veca22 = veca[2]*veca[2];
69
70 double veca01 = veca[0]*veca[1];
71 double veca02 = veca[0]*veca[2];
72 double veca12 = veca[1]*veca[2];
73
74 double vecb00 = vecb[0]*vecb[0];
75 double vecb11 = vecb[1]*vecb[1];
76 double vecb22 = vecb[2]*vecb[2];
77 double vecb01 = vecb[0]*vecb[1];
78 double vecb02 = vecb[0]*vecb[2];
79 double vecb12 = vecb[1]*vecb[2];
80
81 seca[0] = veca[1]*vecb01 + veca[2]*vecb02 - veca[0]*(vecb11 + vecb22);
82 seca[1] = veca[0]*vecb01 + veca[2]*vecb12 - veca[1]*(vecb00 + vecb22);
83 seca[2] = veca[0]*vecb02 + veca[1]*vecb12 - veca[2]*(vecb00 + vecb11);
84
85 secb[0] = vecb[1]*veca01 + vecb[2]*veca02 - vecb[0]*(veca11 + veca22);
86 secb[1] = vecb[0]*veca01 + vecb[2]*veca12 - vecb[1]*(veca00 + veca22);
87 secb[2] = vecb[0]*veca02 + vecb[1]*veca12 - vecb[2]*(veca00 + veca11);
88
89 for(i=0; i<3; i++){
90 tpa += seca[i] * (sta[i] - stb[i]);
91 twb += secb[i] * (stb[i] - sta[i]);
92 }
93 tpa /= m2;
94 twb /= m2;
95 za = veca[2] * tpa + sta[2];
96 zb = vecb[2] * twb + stb[2];
97
98 return 1;
99}
100
101double Alignment::docaLineWire(double trkpar[], double wirest[],
102 double wirev[], double &zwire, int fgZcal){
103
104 double d;
105 double ztrk;
106 double ps[3];
107 double pv[3];
108
109 ps[0] = trkpar[0] * cos(trkpar[1]);
110 ps[1] = trkpar[0] * sin(trkpar[1]);
111 ps[2] = trkpar[3];
112
113 pv[0] = cos(trkpar[1] + HFPI);
114 pv[1] = sin(trkpar[1] + HFPI);
115 pv[2] = trkpar[4];
116
117 Alignment::dist2Line(ps, wirest, pv, wirev, d, ztrk, zwire, fgZcal);
118
119 return d;
120}
121
122double Alignment::docaHelixWireNewton(double trkpar[], double wirest[],
123 double wirev[], double &zwire, double zini){
124 int ifail;
125 double x0 = trkpar[0] * cos(trkpar[1]);
126 double y0 = trkpar[0] * sin(trkpar[1]);
127 double z0 = trkpar[3];
128 double phi0 = trkpar[1] + HFPI;
129 double g = 1000 / (0.3 * BFIELD * trkpar[2]); // alpha/kappa
130 double tanl = trkpar[4];
131
132 double wx = wirev[0];
133 double wy = wirev[1];
134 double wz = wirev[2];
135
136 double phi;
137 double t;
138
139 CLHEP::HepVector c(2, 0);
140 c[0] = phi0 + (zini - z0) / (g * tanl);
141 c[1] = (zini - wirest[2]) / wz;
142
143 phi = c[0];
144 t = c[1];
145 double xh = x0 + g * (sin(phi) - sin(phi0));
146 double yh = y0 + g * (-cos(phi) + cos(phi0));
147 double zh = z0 + g * tanl * (phi - phi0);
148 double xw = wirest[0] + wx*t;
149 double yw = wirest[1] + wy*t;
150 double zw = wirest[2] + wz*t;
151 double doca = sqrt( (xh-xw)*(xh-xw) + (yh-yw)*(yh-yw) + (zh-zw)*(zh-zw) );
152
153 int iter = 0;
154 // cout << "iter " << setw(5) << "ini" << setw(15) << c[0] << setw(15) << c[1]
155 // << setw(15) << doca << endl; // for debug
156 for(iter=0; iter<1000; iter++){
157 CLHEP::HepVector a(2, 0);
158 CLHEP::HepSymMatrix omega(2, 0);
159 phi = c[0];
160 t = c[1];
161
162 a[0] = (x0 - g*sin(phi0) - wirest[0] - wx*t) * cos(phi)
163 + (y0 + g*cos(phi0) - wirest[1] - wy*t) * sin(phi)
164 + (z0 + g*tanl*phi - g*tanl*phi0 - wirest[2] - wz*t) * tanl;
165 a[1] = (x0 + g*sin(phi) - g*sin(phi0) - wirest[0] - wx*t) * wx
166 + (y0 - g*cos(phi) + g*cos(phi0) - wirest[1] - wy*t) * wy
167 + (z0 + g*tanl*phi - g*tanl*phi0 - wirest[2] - wz*t) * wz;
168 omega[0][0] = 0 - (x0 - g*sin(phi0) - wirest[0] - wx*t) * sin(phi)
169 + (y0 + g*cos(phi0) - wirest[1] - wy*t) * cos(phi) + g*tanl*tanl;
170 omega[0][1] = -wx*cos(phi) - wy*sin(phi) - wz*tanl;
171 omega[1][0] = g * (wx*cos(phi) + wy*sin(phi) + wz*tanl);
172 omega[1][1] = -wx*wx - wy*wy - wz*wz;
173
174 HepVector cc(2, 0);
175 cc = c - omega.inverse(ifail) * a;
176
177 phi = c[0];
178 t = c[1];
179 xh = x0 + g * (sin(phi) - sin(phi0));
180 yh = y0 + g * (-cos(phi) + cos(phi0));
181 zh = z0 + g * tanl * (phi - phi0);
182 xw = wirest[0] + wx*t;
183 yw = wirest[1] + wy*t;
184 zw = wirest[2] + wz*t;
185 doca = sqrt( (xh-xw)*(xh-xw) + (yh-yw)*(yh-yw) + (zh-zw)*(zh-zw) );
186 // cout << "iter " << setw(5) << iter << setw(15) << cc[0] << setw(15) << cc[1]
187 // << setw(15) << a[0] << setw(15) << a[1]
188 // << setw(15) << doca << endl; // for debug
189
190
191 c = cc;
192 phi = c[0];
193 t = c[1];
194 a[0] = (x0 - g*sin(phi0) - wirest[0] - wx*t) * cos(phi)
195 + (y0 + g*cos(phi0) - wirest[1] - wy*t) * sin(phi)
196 + (z0 + g*tanl*phi - g*tanl*phi0 - wirest[2] - wz*t) * tanl;
197 a[1] = (x0 + g*sin(phi) - g*sin(phi0) - wirest[0] - wx*t) * wx
198 + (y0 - g*cos(phi) + g*cos(phi0) - wirest[1] - wy*t) * wy
199 + (z0 + g*tanl*phi - g*tanl*phi0 - wirest[2] - wz*t) * wz;
200 if((fabs(a[0]) < 0.001) && (fabs(a[1]) < 0.001)) break;
201
202 // if((fabs(c[0]-cc[0]) < 0.0001) && (fabs(c[1]-cc[1]) < 0.01)){
203 // c = cc;
204 // break;
205 // }
206
207 }
208 Alignment::gNiter = iter;
209
210 phi = c[0];
211 t = c[1];
212 xh = x0 + g * (sin(phi) - sin(phi0));
213 yh = y0 + g * (-cos(phi) + cos(phi0));
214 zh = z0 + g * tanl * (phi - phi0);
215 xw = wirest[0] + wx*t;
216 yw = wirest[1] + wy*t;
217 zw = wirest[2] + wz*t;
218 doca = sqrt( (xh-xw)*(xh-xw) + (yh-yw)*(yh-yw) + (zh-zw)*(zh-zw) );
219 zwire = zw;
220 cout << endl;
221
222 // phi = c[0];
223 // t = c[1];
224 // double xh = x0 + g * (sin(phi) - sin(phi0));
225 // double yh = y0 + g * (-cos(phi) + cos(phi0));
226 // double zh = z0 + g * tanl * (phi - phi0);
227 // double xw = wirest[0] + wx*t;
228 // double yw = wirest[1] + wy*t;
229 // double zw = wirest[2] + wz*t;
230 // double doca = sqrt( (xh-xw)*(xh-xw) + (yh-yw)*(yh-yw) + (zh-zw)*(zh-zw) );
231 // zwire = zw;
232
233 // cout << setw(15) << xh << setw(15) << yh << setw(15) << zh
234 // << setw(15) << xw << setw(15) << yw << setw(15) << zw << setw(15) << doca << endl;
235
236 // double xc = (trkpar[0] - g) * cos(trkpar[1]);
237 // double yc = (trkpar[0] - g) * sin(trkpar[1]);
238 // double dcw = sqrt((xc-xw)*(xc-xw) + (yc-yw)*(yc-yw));
239 // double dch = sqrt((xc-xh)*(xc-xh) + (yc-yh)*(yc-yh));
240 // cout << setw(15) << xc << setw(15) << yc << setw(20) << dch
241 // << setw(15) << dcw << setw(15) << g
242 // << setw(17) << (dch - fabs(g)) << setw(15) << (dcw - fabs(g)) << endl << endl;
243
244 return doca;
245}
246
247double Alignment::docaHelixWire(double trkpar[], double wirest[],
248 double wirev[], double &zwire, double phiIni){
249 double x0 = trkpar[0] * cos(trkpar[1]);
250 double y0 = trkpar[0] * sin(trkpar[1]);
251 double z0 = trkpar[3];
252 double phi0 = trkpar[1] + HFPI;
253 if(phi0 > PI2) phi0 -= PI2;
254 double g = 1000 / (0.3 * BFIELD * trkpar[2]); // alpha/kappa
255 double tanl = trkpar[4];
256
257 double trkst[3];
258 double trkv[3];
259 //double phi = phi0 + (zini - z0) / (g * tanl);
260 double phi = phiIni;
261 double dphi;
262
263 double doca;
264 double ztrk;
265 double phiNew;
266 int iter = 0;
267 //for(iter=0; iter<10; iter++){
268 // trkst[0] = x0 + g * (sin(phi) - sin(phi0));
269 // trkst[1] = y0 + g * (-cos(phi) + cos(phi0));
270 // trkst[2] = z0 + g * tanl * (phi - phi0);
271
272 // trkv[0] = cos(phi);
273 // trkv[1] = sin(phi);
274 // trkv[2] = tanl;
275
276 // Alignment::dist2Line(trkst, wirest, trkv, wirev, doca, ztrk, zwire);
277
278 // phiNew = phi0 + (ztrk - z0) / (g*tanl);
279 // if(fabs(phiNew - phi) < 1.0E-8) break;
280 // phi = phiNew;
281 //}
282 for(iter=0; iter<10; iter++){
283 dphi = phi - phi0;
284 if(dphi > PI) dphi -= PI2;
285 if(dphi < -PI) dphi += PI2;
286
287 trkst[0] = x0 + g * (sin(phi) - sin(phi0));
288 trkst[1] = y0 + g * (-cos(phi) + cos(phi0));
289 // trkst[2] = z0 + g * tanl * (phi - phi0);
290 trkst[2] = z0 + g * tanl * dphi;
291
292 trkv[0] = cos(phi);
293 trkv[1] = sin(phi);
294 trkv[2] = tanl;
295
296 dist2Line(trkst, wirest, trkv, wirev, doca, ztrk, zwire);
297
298 phiNew = phi0 + (ztrk - z0) / (g*tanl);
299 if(fabs(phiNew - phi) < 1.0E-8) break;
300 phi = phiNew;
301 }
302
303 gNiter = iter;
304
305 return doca;
306}
307
308// double Alignment::docaHelixWire(double trkpar[], double wirest[],
309// double wirev[], double &zwire, double zini){
310// double x0 = trkpar[0] * cos(trkpar[1]);
311// double y0 = trkpar[0] * sin(trkpar[1]);
312// double z0 = trkpar[3];
313// double phi0 = trkpar[1] + HFPI;
314// double g = 1000 / (0.3 * BFIELD * trkpar[2]); // alpha/kappa
315// double tanl = trkpar[4];
316
317// double wx = wirev[0];
318// double wy = wirev[1];
319// double wz = wirev[2];
320
321// // double phi;
322// // double t;
323
324// // CLHEP::HepVector c(2, 0);
325// // c[0] = phi0 + (zini - z0) / (g * tanl);
326// // c[1] = (zini - wirest[2]) / wz;
327
328// // phi = c[0];
329// // t = c[1];
330// // double xh = x0 + g * (sin(phi) - sin(phi0));
331// // double yh = y0 + g * (-cos(phi) + cos(phi0));
332// // double zh = z0 + g * tanl * (phi - phi0);
333// // double xw = wirest[0] + wx*t;
334// // double yw = wirest[1] + wy*t;
335// // double zw = wirest[2] + wz*t;
336// // double doca = sqrt( (xh-xw)*(xh-xw) + (yh-yw)*(yh-yw) + (zh-zw)*(zh-zw) );
337
338// double docaIter[10000];
339// double phiIni = phi0 + (zini - z0) / (g * tanl) - 0.0000001*5000.0;
340// double phi;
341// double t = (zini - wirest[2]) / wz;
342
343// int iter;
344// for(iter=0; iter<10000; iter++){
345// phi = 0.0000001 * (double)iter + phiIni;
346// double xh = x0 + g * (sin(phi) - sin(phi0));
347// double yh = y0 + g * (-cos(phi) + cos(phi0));
348// double zh = z0 + g * tanl * (phi - phi0);
349// double xw = wirest[0] + wx*t;
350// double yw = wirest[1] + wy*t;
351// double zw = wirest[2] + wz*t;
352// docaIter[iter] = sqrt( (xh-xw)*(xh-xw) + (yh-yw)*(yh-yw) + (zh-zw)*(zh-zw) );
353
354// // cout << "iter " << setw(5) << iter << setw(15) << phi << setw(15) << doca << endl;
355// }
356// gNiter = iter;
357
358// double doca = docaIter[0];
359// for(iter=0; iter<10000; iter++){
360// if(fabs(docaIter[iter]) < fabs(doca)) doca = docaIter[iter];
361// }
362
363// return doca;
364// }
365
366bool Alignment::getDoca(double trkpar[], double wpos[], double &doca,
367 double whitPos[], double zini){
368 int i = 0;
369 double zp; // z of the point above in the plane of the wire
370 double xyz[3]; // coordinate of the point on wire according to zc
371 double dxyz[3]; // orientation of the tangent line at the point above
372
373 double ten = wpos[6];
374 double a = 9.47e-06 / (2 * ten); // a = density(g/mm)/2T(g)
375 double dx = wpos[0] - wpos[3]; // the differential of xyz between the end planes
376 double dz = wpos[2] - wpos[5]; //
377 double length = sqrt(dx*dx + dz*dz);
378
379 double ztan = 0.0; // z of the doca point in the tangent line
380 if(whitPos[2] < 0.5*length) ztan = whitPos[2];
381
382 double zc=0.0; // z of the calculated point of the wire
383 if( Alignment::gFlagMag ) zc = zini;
384
385 // alf is the angle between z and the projection of the wire on xz
386 double sinalf = dx / sqrt(dx*dx + dz*dz);
387 double cosalf = dz / sqrt(dx*dx + dz*dz);
388 double tanalf = dx / dz;
389
390 double posIni[3];
391 double rLayer = sqrt((wpos[3] * wpos[3]) + (wpos[4] * wpos[4]));
392 double phiIni = getPhiIni(trkpar, rLayer, posIni);
393
394 if(dz < 50){
395 std::cout << "ERROR: wire position error in getdocaLine() !!!"
396 << std::endl;
397 return false;
398 }
399
400 while( 1 ){
401 i++;
402 if(i > 5){
403 return false;
404 }
405 zp = zc / cosalf;
406
407 xyz[0] = (zc - wpos[5]) * tanalf + wpos[3];
408 xyz[1] = a*zp*zp + (wpos[1] - wpos[4])*zp/length
409 + 0.5*(wpos[1] + wpos[4]) - a*length*length/4.0;
410 xyz[2] = zc;
411
412 dxyz[0] = sinalf;
413 dxyz[1] = 2.0 * a * zp + (wpos[1] - wpos[4]) / length;
414 dxyz[2] = cosalf;
415
416 if( Alignment::gFlagMag ) doca = docaHelixWire(trkpar, xyz, dxyz, ztan, phiIni);
417 else doca = docaLineWire(trkpar, xyz, dxyz, ztan);
418
419 if( fabs(zc-ztan) < 0.5 ) break;
420 else if( fabs(ztan) > (0.5*length) ){
421 doca = 99999.0;
422 break;
423 }
424 zc = ztan;
425 }
426 whitPos[2] = ztan;
427 zp = ztan / cosalf;
428 whitPos[1] = a*zp*zp + (wpos[1] - wpos[4])*zp/length
429 + 0.5*(wpos[1] + wpos[4]) - a*length*length/4.0;
430 whitPos[0] = (ztan - wpos[5]) * tanalf + wpos[3];
431
432 return true;
433}
434double Alignment::getPhiIni(double trkpar[], double rLayer, double pos[]){
435 double dr = trkpar[0];
436 double fi0 = trkpar[1];
437 double kap = trkpar[2];
438 double rw = rLayer;
439
440 double phi0 = fi0 + HFPI;
441 if(phi0 > PI2) phi0 -= PI2;
442 double g = 1000 / (0.3 * 1.0 * kap); // alpha/kappa
443
444 double aa = rw*rw - (dr-g)*(dr-g) - g*g;
445 double bb = 2*g*(dr - g);
446 double cc = aa/bb;
447 double dd = acos(cc); // dd (0, PI)
448
449 double phi;
450 if(kap > 0) phi = phi0 + dd;
451 else phi = phi0 - dd;
452
453 if(phi > PI2) phi -= PI2;
454 if(phi < 0) phi += PI2;
455
456 double x0 = dr * cos(fi0);
457 double y0 = dr * sin(fi0);
458 pos[0] = x0 + g * (sin(phi) - sin(phi0));
459 pos[1] = y0 + g * (-cos(phi) + cos(phi0));
460 // pos[2] = trkpar[3] + g * trkpar[4] * (phi - phi0);
461 if(kap > 0) pos[2] = trkpar[3] + g * trkpar[4] * dd;
462 else pos[2] = trkpar[3] - g * trkpar[4] * dd;
463
464 return phi;
465}
466
467
sin
double sin(const BesAngle a)
Definition BesAngle.h:210
cos
double cos(const BesAngle a)
Definition BesAngle.h:213
length
double length
Definition CgemLineFit.cxx:121
iter
EvtStreamInputIterator< typename Generator::result_type > iter(Generator gen, int N=0)
Definition EvtStreamInputIterator.hh:101
Alignment::getDoca
bool getDoca(double trkpar[], double wpos[], double &doca, double whitPos[], double zini)
Definition Alignment.cxx:366
Alignment::PI2
const double PI2
Definition Alignment.h:41
Alignment::BFIELD
const double BFIELD
Definition Alignment.h:52
Alignment::dist2Line
int dist2Line(double sta[3], double stb[3], double veca[3], double vecb[3], double &d, double &za, double &zb, int fgZcal=1)
Definition Alignment.cxx:41
Alignment::docaHelixWire
double docaHelixWire(double trkpar[], double wirest[], double wirev[], double &zwire, double zini)
Definition Alignment.cxx:247
Alignment::HFPI
const double HFPI
Definition Alignment.h:42
Alignment::getEpId
int getEpId(int lay, int iEnd)
Definition Alignment.cxx:18
Alignment::docaHelixWireNewton
double docaHelixWireNewton(double trkpar[], double wirest[], double wirev[], double &zwire, double zini)
Definition Alignment.cxx:122
Alignment::docaLineWire
double docaLineWire(double trkpar[], double wirest[], double wirev[], double &zwire, int fgZcal=1)
Definition Alignment.cxx:101
Alignment::getPhiIni
double getPhiIni(double trkpar[], double rLayer, double pos[])
Definition Alignment.cxx:434
Alignment::gFlagMag
bool gFlagMag
Definition Alignment.cxx:15
Alignment::expd
void expd(double veca[3], double vecb[3], double val[3])
Definition Alignment.cxx:35
Alignment::PI
const double PI
Definition Alignment.h:40
t
int t()
Definition t.c:1