CGEM BOSS 6.6.5.i
BESIII Offline Software System
Loading...
Searching...
No Matches
TrkFitFun.cxx
Go to the documentation of this file.
1#include <math.h>
2#include <iostream>
3#include "DotsConnection/TrkFitFun.h"
4#include "TSpline.h"
5#include "KalFitAlg/helix/Helix.h"
6
7using namespace std;
8//using namespace CLHEP;
9
10int TrkFitFun::gNiter;
11
12void TrkFitFun::expd(double veca[3], double vecb[3], double val[3]){
13 val[0] = veca[1]*vecb[2] - veca[2]*vecb[1];
14 val[1] = veca[2]*vecb[0] - veca[0]*vecb[2];
15 val[2] = veca[0]*vecb[1] - veca[1]*vecb[0];
16}
17
18int TrkFitFun::dist2Line(double sta[3], double stb[3],
19 double veca[3], double vecb[3],
20 double &d, double xyz_a[3], double xyz_b[3], int fgZcal){
21 int i;
22 double vst[3]; // P0 - W0
23 double vp[3]; // (P * W) / |P * W|
24 double modvp;
25 double m2;
26 double seca[3], secb[3];
27 double tpa = 0.0;
28 double twb = 0.0;
29
30 for(i=0; i<3; i++) vst[i] = sta[i] - stb[i]; // vector P0-W0
31
32 //cout<<"TrkFitFun::dist2Line() veca=("<<veca[0]<<", "<<veca[1]<<", "<<veca[2]<<")"<<endl;
33 //cout<<"TrkFitFun::dist2Line() vecb=("<<vecb[0]<<", "<<vecb[1]<<", "<<vecb[2]<<")"<<endl;
34 TrkFitFun::expd(veca, vecb, vp); // exterior product
35
36 //cout<<"TrkFitFun::dist2Line() vp=("<<vp[0]<<", "<<vp[1]<<", "<<vp[2]<<")"<<endl;
37 m2 = vp[0]*vp[0] + vp[1]*vp[1] + vp[2]*vp[2];
38 //cout<<"TrkFitFun::dist2Line() m2="<<m2<<endl;
39 modvp = sqrt(m2);
40 for(i=0; i<3; i++) vp[i] /= modvp; // (P * W) / |P * W|
41
42 d = 0.0;
43 for(i=0; i<3; i++) d += vst[i] * vp[i];
44 //cout<<"TrkFitFun::dist2Line() d="<<d<<endl;
45
46 if( 0 == fgZcal ) return 1;
47
48 double veca00 = veca[0]*veca[0];
49 double veca11 = veca[1]*veca[1];
50 double veca22 = veca[2]*veca[2];
51
52 double veca01 = veca[0]*veca[1];
53 double veca02 = veca[0]*veca[2];
54 double veca12 = veca[1]*veca[2];
55
56 double vecb00 = vecb[0]*vecb[0];
57 double vecb11 = vecb[1]*vecb[1];
58 double vecb22 = vecb[2]*vecb[2];
59 double vecb01 = vecb[0]*vecb[1];
60 double vecb02 = vecb[0]*vecb[2];
61 double vecb12 = vecb[1]*vecb[2];
62
63 seca[0] = veca[1]*vecb01 + veca[2]*vecb02 - veca[0]*(vecb11 + vecb22);
64 seca[1] = veca[0]*vecb01 + veca[2]*vecb12 - veca[1]*(vecb00 + vecb22);
65 seca[2] = veca[0]*vecb02 + veca[1]*vecb12 - veca[2]*(vecb00 + vecb11);
66
67 secb[0] = vecb[1]*veca01 + vecb[2]*veca02 - vecb[0]*(veca11 + veca22);
68 secb[1] = vecb[0]*veca01 + vecb[2]*veca12 - vecb[1]*(veca00 + veca22);
69 secb[2] = vecb[0]*veca02 + vecb[1]*veca12 - vecb[2]*(veca00 + veca11);
70
71 for(i=0; i<3; i++){
72 tpa += seca[i] * (sta[i] - stb[i]);
73 twb += secb[i] * (stb[i] - sta[i]);
74 }
75 tpa /= m2;
76 twb /= m2;
77
78 for(i=0; i<3; i++)
79 {
80 xyz_a[i] = veca[i] * tpa + sta[i];
81 xyz_b[i] = vecb[i] * twb + stb[i];
82 }
83
84 return 1;
85}
86
87
88double TrkFitFun::docaHelixWire(double trkpar[], double wirest[], double wirev[], double &zwire, double phiIni)
89{
90 HepPoint3D pivot(0,0,0);
91 HepVector a(5);
92 for(int i=0; i<5; i++) a(i+1)=trkpar[i];
93 KalmanFit::Helix aHelix(pivot, a);
94 double x0 = trkpar[0] * cos(trkpar[1]);
95 double y0 = trkpar[0] * sin(trkpar[1]);
96 double z0 = trkpar[3];
97 double phi0 = trkpar[1] + HFPI;
98 if(phi0 > PI2) phi0 -= PI2;
99 double g = 1e13 / (CC * BFIELD * trkpar[2]); // alpha/kappa in cm/(GeV/c)
100 double tanl = trkpar[4];
101
102 double trkst[3];// point on track
103 double trkv[3];
104 //double phi = phi0 + (zini - z0) / (g * tanl);
105 double phi = phiIni;
106 double dphi;
107
108 double doca;
109 double ztrk;
110 double xyz_trk[3];
111 double xyz_wire[3];
112 double phiNew;
113 int iter = 0;
114 //for(iter=0; iter<10; iter++){
115 // trkst[0] = x0 + g * (sin(phi) - sin(phi0));
116 // trkst[1] = y0 + g * (-cos(phi) + cos(phi0));
117 // trkst[2] = z0 + g * tanl * (phi - phi0);
118
119 // trkv[0] = cos(phi);
120 // trkv[1] = sin(phi);
121 // trkv[2] = tanl;
122
123 // Alignment::dist2Line(trkst, wirest, trkv, wirev, doca, ztrk, zwire);
124
125 // phiNew = phi0 + (ztrk - z0) / (g*tanl);
126 // if(fabs(phiNew - phi) < 1.0E-8) break;
127 // phi = phiNew;
128 //}
129 for(iter=0; iter<10; iter++){
130 dphi = phi - phi0;
131 if(dphi > PI) dphi -= PI2;
132 if(dphi < -PI) dphi += PI2;
133
134 trkst[0] = x0 + g * (sin(phi) - sin(phi0));
135 trkst[1] = y0 + g * (-cos(phi) + cos(phi0));
136 // trkst[2] = z0 + g * tanl * (phi - phi0);
137 trkst[2] = z0 + g * tanl * dphi;
138
139 trkv[0] = cos(phi);
140 trkv[1] = sin(phi);
141 trkv[2] = tanl;
142 //cout<<"TrkFitFun::docaHelixWire: trkv = "<<setw(10)<<trkv[0]<<setw(10)<<trkv[1]<<setw(10)<<trkv[2]<<endl;
143
144 // wirest: coordinate of the point on wire according to zc
145 dist2Line(trkst, wirest, trkv, wirev, doca, xyz_trk, xyz_wire);
146
147 //ztrk=xyz_trk[2];
148 //phiNew = phi0 + (ztrk - z0) / (g*tanl);
149 //cout<<"phiNew = "<<phiNew<<endl;
150
151 HepPoint3D newPivot(xyz_trk[0],xyz_trk[1],xyz_trk[2]);
152 aHelix.pivot(newPivot);
153 phiNew=aHelix.phi0()+HFPI;
154 //cout<<"phiNew2 = "<<phiNew<<endl;
155
156 double delPhi = phiNew - phi;
157 while(delPhi>M_PI) delPhi-=M_PI*2.0;
158 while(delPhi<-M_PI) delPhi+=M_PI*2.0;
159 if(fabs(delPhi) < 1.0E-8) break;
160 phi = phiNew;
161 }
162 zwire=xyz_wire[2];
163
164 gNiter = iter;
165
166 return doca;
167}
168
169/**
170 * @brief doca: distance of closest approach.
171 *
172 * @param trkpar[5]
173 * @param wpos[7]
174 * @param doca
175 * @param whitPos[3]
176 * @param zini[3]
177 * @return true
178 * @return false
179 */
180bool TrkFitFun::getDoca(double trkpar[], double wpos[], double &doca,
181 double whitPos[], double zini){
182 int i = 0;
183 double zp; // z of the point above in the plane of the wire
184 double xyz[3]; // coordinate of the point on wire according to zc
185 double dxyz[3]; // orientation of the tangent line at the point above
186
187 double ten = wpos[6];
188 double a = 9.47e-05 / (2 * ten); // a = density(g/mm)/2T(g)
189 double dx = wpos[0] - wpos[3]; // the differential of xyz between the end planes
190 double dy = wpos[1] - wpos[4];
191 double dz = wpos[2] - wpos[5]; //
192 double length = sqrt(dx*dx + dz*dz);
193 //cout<<"length = "<<length<<endl;
194
195 double ztan = 0.0; // z of the doca point in the tangent line
196 if(whitPos[2] < 0.5*length) ztan = whitPos[2];
197
198 double zc=0.0; // z of the calculated point of the wire
199 //if( Alignment::gFlagMag ) zc = zini;
200
201 // alf is the angle between z and the projection of the wire on xz
202 double sinalf = dx / sqrt(dx*dx + dz*dz);
203 double cosalf = dz / sqrt(dx*dx + dz*dz);
204 double tanalf = dx / dz;
205 //cout<<"cosalf="<<cosalf<<endl;
206
207 /*
208 double posIni[3];
209 double rLayer = sqrt((wpos[3] * wpos[3]) + (wpos[4] * wpos[4]));
210 double phiIni = getPhiIni(trkpar, rLayer, posIni);
211 //cout<<"TrkFitFun::getDoca(): phiIni = "<<phiIni<<endl;
212 */
213
214 HepPoint3D pivot(0,0,0);
215 HepVector par(5);
216 for(int i=0; i<5; i++) par(i+1)=trkpar[i];
217 KalmanFit::Helix aHelix(pivot, par);
218 double tmp = (ztan-wpos[5])/dz;
219 pivot.setX(tmp*dx+wpos[3]);
220 pivot.setY(tmp*dy+wpos[4]);
221 pivot.setZ(tmp*dz+wpos[5]);
222 aHelix.pivot(pivot);
223 double phiIni=aHelix.phi0()+HFPI;
224 //cout<<"TrkFitFun::getDoca(): phiIni2 = "<<phiIni<<endl;
225
226 if(dz < 20){
227 std::cout << "ERROR: wire position error in getdocaLine() !!!"
228 << std::endl;
229 std::cout<<"dz = "<<dz<<std::endl;
230 return false;
231 }
232
233 while( 1 ){
234 i++;
235 if(i > 5){
236 return false;
237 }
238 zp = zc / cosalf;
239
240 xyz[0] = (zc - wpos[5]) * tanalf + wpos[3];
241 xyz[1] = a*zp*zp + (wpos[1] - wpos[4])*zp/length
242 + 0.5*(wpos[1] + wpos[4]) - a*length*length/4.0;
243 xyz[2] = zc;
244
245 dxyz[0] = sinalf;
246 dxyz[1] = 2.0 * a * zp + (wpos[1] - wpos[4]) / length;
247 dxyz[2] = cosalf;
248
249 //if( Alignment::gFlagMag ) doca = docaHelixWire(trkpar, xyz, dxyz, ztan, phiIni);
250 //else doca = docaLineWire(trkpar, xyz, dxyz, ztan);
251 doca = docaHelixWire(trkpar, xyz, dxyz, ztan, phiIni);
252 //cout<<"TrkFitFun::getDoca(): i, doca, ztan, zc = "<<i<<", "<<doca<<", "<<ztan<<", "<<zc<<endl;
253
254 if( fabs(zc-ztan) < 0.5 ) break;
255 /*else if( fabs(ztan) > (0.5*length) ){
256 doca = 99999.0;
257 break;
258 }
259 */ // --- comment out by wangll, 2020-04-27
260 zc = ztan;
261 }
262 whitPos[2] = ztan;
263 zp = ztan / cosalf;
264 whitPos[1] = a*zp*zp + (wpos[1] - wpos[4])*zp/length
265 + 0.5*(wpos[1] + wpos[4]) - a*length*length/4.0;
266 whitPos[0] = (ztan - wpos[5]) * tanalf + wpos[3];
267
268 return true;
269}
270
271double TrkFitFun::getPhiIni(double trkpar[], double rLayer, double pos[]){
272 double dr = trkpar[0];
273 double fi0 = trkpar[1];
274 double kap = trkpar[2];
275 double rw = rLayer;
276
277 double phi0 = fi0 + HFPI;
278 if(phi0 > PI2) phi0 -= PI2;
279 double g = 1.0e13 / (CC * BFIELD * kap); // alpha/kappa in cm/(GeV/c)
280
281 double aa = rw*rw - (dr-g)*(dr-g) - g*g;
282 double bb = 2*g*(dr - g);
283 //cout<<"TrkFitFun::getPhiIni() aa,bb="<<setw(10)<<aa<<setw(10)<<bb<<endl;
284 double cc = aa/bb;
285 double dd = M_PI;
286 if(fabs(cc)<=1)
287 dd=acos(cc); // dd (0, PI)
288
289 double phi;
290 if(kap > 0) phi = phi0 + dd;
291 else phi = phi0 - dd;
292 //cout<<"TrkFitFun::getPhiIni() phi0,dd="<<setw(10)<<phi0<<setw(10)<<dd<<endl;
293
294 if(phi > PI2) phi -= PI2;
295 if(phi < 0) phi += PI2;
296
297 double x0 = dr * cos(fi0);
298 double y0 = dr * sin(fi0);
299 pos[0] = x0 + g * (sin(phi) - sin(phi0));
300 pos[1] = y0 + g * (-cos(phi) + cos(phi0));
301 // pos[2] = trkpar[3] + g * trkpar[4] * (phi - phi0);
302 if(kap > 0) pos[2] = trkpar[3] + g * trkpar[4] * dd;
303 else pos[2] = trkpar[3] - g * trkpar[4] * dd;
304
305 return phi;
306}
307
308
309bool TrkFitFun::getDeriLoc(int ipar, double helix[], double &deri, double wpos[], double zini)
310{
311 int i;
312 double doca;
313 double sampar[NTRKPAR];
314 double xxLC[gNsamLC];
315 double yyLC[gNsamLC];
316 double aPointOnWire[3]={0,0,0};
317
318 for(i=0; i<NTRKPAR; i++) sampar[i] = helix[i];
319 double startpar = helix[ipar] - 0.5*gStepLC[ipar]*(double)gNsamLC;
320
321 for(i=0; i<gNsamLC; i++){
322 sampar[ipar] = startpar + (double)i * gStepLC[ipar];
323 xxLC[i] = sampar[ipar];
324 //if(0==ipar || 3==ipar) xxLC[i] *= 10.; // cm -> mm
325
326 getDoca(sampar, wpos, doca, aPointOnWire, zini);
327
328 if(NULL == doca){
329 // cout << "in getDeriLoc, doca = " << doca << endl;
330 return false;
331 }
332 yyLC[i] = doca;
333 }
334
335 double par = helix[ipar];
336 //if (0==ipar || 3==ipar) par*= 10.; // cm -> mm
337 TSpline3* pSpline3 = new TSpline3("deri", xxLC, yyLC, gNsamLC);
338 deri = pSpline3->Derivative(par);
339 /*
340 if(deri!=deri) {
341 cout<<"ipar = "<<ipar<<", par="<<helix[ipar]<<endl;
342 for(i=0; i<gNsamLC; i++){
343 cout<<"i, xxLC, yyLC = "<<i<<", "<<xxLC[i]<<", "<<yyLC[i]<<endl;
344 }
345 }
346 */
347 delete pSpline3;
348 return true;
349}
350
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
PI
const double PI
Definition PipiJpsi.cxx:55
M_PI
#define M_PI
Definition TConstant.h:4
KalmanFit::Helix::pivot
const HepPoint3D & pivot(void) const
returns pivot position.
Definition Reconstruction/KalFitAlg/KalFitAlg-00-15-20/KalFitAlg/helix/Helix.h:211
TrkFitFun::getPhiIni
double getPhiIni(double trkpar[], double rLayer, double pos[])
Definition TrkFitFun.cxx:271
TrkFitFun::getDeriLoc
bool getDeriLoc(int ipar, double helix[], double &deri, double wpos[], double zini)
Definition TrkFitFun.cxx:309
TrkFitFun::expd
void expd(double veca[3], double vecb[3], double val[3])
Definition TrkFitFun.cxx:12
TrkFitFun::getDoca
bool getDoca(double trkpar[], double wpos[], double &doca, double whitPos[], double zini)
doca: distance of closest approach.
Definition TrkFitFun.cxx:180
TrkFitFun::dist2Line
int dist2Line(double sta[3], double stb[3], double veca[3], double vecb[3], double &d, double xyz_a[3], double xyz_b[3], int fgZcal=1)
Definition TrkFitFun.cxx:18
TrkFitFun::docaHelixWire
double docaHelixWire(double trkpar[], double wirest[], double wirev[], double &zwire, double zini)
Definition TrkFitFun.cxx:88