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