CGEM BOSS 6.6.5.g
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
169bool TrkFitFun::getDoca(double trkpar[], double wpos[], double &doca,
170 double whitPos[], double zini){
171 int i = 0;
172 double zp; // z of the point above in the plane of the wire
173 double xyz[3]; // coordinate of the point on wire according to zc
174 double dxyz[3]; // orientation of the tangent line at the point above
175
176 double ten = wpos[6];
177 double a = 9.47e-05 / (2 * ten); // a = density(g/mm)/2T(g)
178 double dx = wpos[0] - wpos[3]; // the differential of xyz between the end planes
179 double dy = wpos[1] - wpos[4];
180 double dz = wpos[2] - wpos[5]; //
181 double length = sqrt(dx*dx + dz*dz);
182 //cout<<"length = "<<length<<endl;
183
184 double ztan = 0.0; // z of the doca point in the tangent line
185 if(whitPos[2] < 0.5*length) ztan = whitPos[2];
186
187 double zc=0.0; // z of the calculated point of the wire
188 //if( Alignment::gFlagMag ) zc = zini;
189
190 // alf is the angle between z and the projection of the wire on xz
191 double sinalf = dx / sqrt(dx*dx + dz*dz);
192 double cosalf = dz / sqrt(dx*dx + dz*dz);
193 double tanalf = dx / dz;
194 //cout<<"cosalf="<<cosalf<<endl;
195
196 /*
197 double posIni[3];
198 double rLayer = sqrt((wpos[3] * wpos[3]) + (wpos[4] * wpos[4]));
199 double phiIni = getPhiIni(trkpar, rLayer, posIni);
200 //cout<<"TrkFitFun::getDoca(): phiIni = "<<phiIni<<endl;
201 */
202
203 HepPoint3D pivot(0,0,0);
204 HepVector par(5);
205 for(int i=0; i<5; i++) par(i+1)=trkpar[i];
206 KalmanFit::Helix aHelix(pivot, par);
207 double tmp = (ztan-wpos[5])/dz;
208 pivot.setX(tmp*dx+wpos[3]);
209 pivot.setY(tmp*dy+wpos[4]);
210 pivot.setZ(tmp*dz+wpos[5]);
211 aHelix.pivot(pivot);
212 double phiIni=aHelix.phi0()+HFPI;
213 //cout<<"TrkFitFun::getDoca(): phiIni2 = "<<phiIni<<endl;
214
215 if(dz < 20){
216 std::cout << "ERROR: wire position error in getdocaLine() !!!"
217 << std::endl;
218 std::cout<<"dz = "<<dz<<std::endl;
219 return false;
220 }
221
222 while( 1 ){
223 i++;
224 if(i > 5){
225 return false;
226 }
227 zp = zc / cosalf;
228
229 xyz[0] = (zc - wpos[5]) * tanalf + wpos[3];
230 xyz[1] = a*zp*zp + (wpos[1] - wpos[4])*zp/length
231 + 0.5*(wpos[1] + wpos[4]) - a*length*length/4.0;
232 xyz[2] = zc;
233
234 dxyz[0] = sinalf;
235 dxyz[1] = 2.0 * a * zp + (wpos[1] - wpos[4]) / length;
236 dxyz[2] = cosalf;
237
238 //if( Alignment::gFlagMag ) doca = docaHelixWire(trkpar, xyz, dxyz, ztan, phiIni);
239 //else doca = docaLineWire(trkpar, xyz, dxyz, ztan);
240 doca = docaHelixWire(trkpar, xyz, dxyz, ztan, phiIni);
241 //cout<<"TrkFitFun::getDoca(): i, doca, ztan, zc = "<<i<<", "<<doca<<", "<<ztan<<", "<<zc<<endl;
242
243 if( fabs(zc-ztan) < 0.5 ) break;
244 /*else if( fabs(ztan) > (0.5*length) ){
245 doca = 99999.0;
246 break;
247 }
248 */ // --- comment out by wangll, 2020-04-27
249 zc = ztan;
250 }
251 whitPos[2] = ztan;
252 zp = ztan / cosalf;
253 whitPos[1] = a*zp*zp + (wpos[1] - wpos[4])*zp/length
254 + 0.5*(wpos[1] + wpos[4]) - a*length*length/4.0;
255 whitPos[0] = (ztan - wpos[5]) * tanalf + wpos[3];
256
257 return true;
258}
259
260double TrkFitFun::getPhiIni(double trkpar[], double rLayer, double pos[]){
261 double dr = trkpar[0];
262 double fi0 = trkpar[1];
263 double kap = trkpar[2];
264 double rw = rLayer;
265
266 double phi0 = fi0 + HFPI;
267 if(phi0 > PI2) phi0 -= PI2;
268 double g = 1.0e13 / (CC * BFIELD * kap); // alpha/kappa in cm/(GeV/c)
269
270 double aa = rw*rw - (dr-g)*(dr-g) - g*g;
271 double bb = 2*g*(dr - g);
272 //cout<<"TrkFitFun::getPhiIni() aa,bb="<<setw(10)<<aa<<setw(10)<<bb<<endl;
273 double cc = aa/bb;
274 double dd = M_PI;
275 if(fabs(cc)<=1)
276 dd=acos(cc); // dd (0, PI)
277
278 double phi;
279 if(kap > 0) phi = phi0 + dd;
280 else phi = phi0 - dd;
281 //cout<<"TrkFitFun::getPhiIni() phi0,dd="<<setw(10)<<phi0<<setw(10)<<dd<<endl;
282
283 if(phi > PI2) phi -= PI2;
284 if(phi < 0) phi += PI2;
285
286 double x0 = dr * cos(fi0);
287 double y0 = dr * sin(fi0);
288 pos[0] = x0 + g * (sin(phi) - sin(phi0));
289 pos[1] = y0 + g * (-cos(phi) + cos(phi0));
290 // pos[2] = trkpar[3] + g * trkpar[4] * (phi - phi0);
291 if(kap > 0) pos[2] = trkpar[3] + g * trkpar[4] * dd;
292 else pos[2] = trkpar[3] - g * trkpar[4] * dd;
293
294 return phi;
295}
296
297
298bool TrkFitFun::getDeriLoc(int ipar, double helix[], double &deri, double wpos[], double zini)
299{
300 int i;
301 double doca;
302 double sampar[NTRKPAR];
303 double xxLC[gNsamLC];
304 double yyLC[gNsamLC];
305 double aPointOnWire[3]={0,0,0};
306
307 for(i=0; i<NTRKPAR; i++) sampar[i] = helix[i];
308 double startpar = helix[ipar] - 0.5*gStepLC[ipar]*(double)gNsamLC;
309
310 for(i=0; i<gNsamLC; i++){
311 sampar[ipar] = startpar + (double)i * gStepLC[ipar];
312 xxLC[i] = sampar[ipar];
313 //if(0==ipar || 3==ipar) xxLC[i] *= 10.; // cm -> mm
314
315 getDoca(sampar, wpos, doca, aPointOnWire, zini);
316
317 if(NULL == doca){
318 // cout << "in getDeriLoc, doca = " << doca << endl;
319 return false;
320 }
321 yyLC[i] = doca;
322 }
323
324 double par = helix[ipar];
325 //if (0==ipar || 3==ipar) par*= 10.; // cm -> mm
326 TSpline3* pSpline3 = new TSpline3("deri", xxLC, yyLC, gNsamLC);
327 deri = pSpline3->Derivative(par);
328 delete pSpline3;
329 return true;
330}
331
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:106
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-14/KalFitAlg/helix/Helix.h:211
TrkFitFun::getPhiIni
double getPhiIni(double trkpar[], double rLayer, double pos[])
Definition: TrkFitFun.cxx:260
TrkFitFun::getDeriLoc
bool getDeriLoc(int ipar, double helix[], double &deri, double wpos[], double zini)
Definition: TrkFitFun.cxx:298
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)
Definition: TrkFitFun.cxx:169
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::gNiter
int gNiter
Definition: TrkFitFun.cxx:10
TrkFitFun::docaHelixWire
double docaHelixWire(double trkpar[], double wirest[], double wirev[], double &zwire, double zini)
Definition: TrkFitFun.cxx:88