CGEM BOSS 6.6.5.f
BESIII Offline Software System
Loading...
Searching...
No Matches
HoughStereo.cxx
Go to the documentation of this file.
1#include "MdcHoughFinder/HoughRecHit.h"
2#include "MdcHoughFinder/HoughStereo.h"
3
4int HoughStereo::m_debug=0;
5//HoughStereo::HoughStereo(){
6//}
7//HoughStereo::HoughStereo(double bunchTime):_bunchTime(bunchTime){
8//}
9HoughStereo::HoughStereo( double bunchTime , Hough2D *circle , HoughRecHit* rechit):_bunchTime(bunchTime),_circle(circle),_rechit(rechit){
10 _lleft=-99;
11 _zleft=-99;
12 _lright=-99;
13 _zright=-99;
14 _ambig=0;
15 _charge=circle->getCharge();
16}
17void HoughStereo::setAmb(int i){
18 //initial
19 _ambig=i;
20}
21
22
23int HoughStereo::cald(){
24 if(_rechit->getDetectorType()==CGEM){
25 double xc = _circle->getCirX();
26 double yc = _circle->getCirY();
27 double rc = _circle->getCirR();
28 double x1 = _rechit->getMidPoint().x();
29 double y1 = _rechit->getMidPoint().y();
30 double z1 = _rechit->getMidPoint().z();
31 //cout<<"CGEM z "<<z1<<endl;
32 double l(0),theta(0);
33 if(xc==0||x1-xc==0){
34 theta=0;
35 }
36 else{
37 theta=M_PI-atan2(y1-yc,x1-xc)+atan2(yc,xc);
38 if(theta>2*M_PI){
39 theta=theta-2*M_PI;
40 }
41 if(theta<0){
42 theta=theta+2*M_PI;
43 }
44 }
45 if(_charge == 1 ) theta = 2*M_PI-theta;
46 l = rc*theta;
47 _lleft=l;
48 _zleft=z1;
49 _lright=l;
50 _zright=z1;
51 HepPoint3D leftPoint(x1,y1,_zleft);
52 HepPoint3D rightPoint(x1,y1,_zright);
53 _rechit->setLeftPoint(leftPoint);
54 _rechit->setRightPoint(rightPoint);
55 return -2;
56 }else{
57 bool ok_ambig[2];
58 bool ok[2][2];
59 ok_ambig[0]=true;
60 ok_ambig[1]=true;
61 ok[0][0]=false;
62 ok[0][1]=false;
63 ok[1][0]=false;
64 ok[1][1]=false;
65 double xeast = _rechit->getEastPoint().x();
66 double xwest = _rechit->getWestPoint().x();
67 double yeast = _rechit->getEastPoint().y();
68 double ywest = _rechit->getWestPoint().y();
69 double zeast = _rechit->getEastPoint().z();
70 double zwest = _rechit->getWestPoint().z();
71 double x = _rechit->getPointTruth().x();
72 double y = _rechit->getPointTruth().y();
73 double z = _rechit->getPointTruth().z();
74 //cout<<"xeast xwest "<<xeast<<" "<<xwest<<endl;
75 double k = (ywest-yeast)/(xwest-xeast);
76 double b = -k*xeast+yeast;
77 // cout<<" k b "<<k<<" "<<b<<endl;
78 double xc = _circle->getCirX();
79 double yc = _circle->getCirY();
80 double rc = _circle->getCirR();
81 //cout<<"d0 phi0 omega "<<_circle->getD0()<<" "<<_circle->getPhi0()<<" "<<_circle->getOmega()<<endl;
82 //cout<<"xc yc rc "<<xc<<" "<<yc<<" "<<rc<<endl;
83 double drift = _rechit->getDriftDist();
84 //double drift = _rechit->getDriftDistTruth();
85 // cout<<"drift "<<_rechit->getDriftDist()<<endl;
86 // cout<<"driftTruth "<<_rechit->getDriftDistTruth()<<endl;
87 double x1(999),y1(999);
88 double x2(999),y2(999);
89 double a = k*k+1;
90 double b1 = -2*(xc+k*yc-k*b);
91 double c1 = xc*xc+(yc-b)*(yc-b)-(rc+drift)*(rc+drift);
92 double c2 = xc*xc+(yc-b)*(yc-b)-(rc-drift)*(rc-drift);
93 double delta1 = (b1*b1-4*a*c1);
94 double delta2 = (b1*b1-4*a*c2);
95 if( delta1 <0 ) ok_ambig[0] = false;
96 if( delta2 <0 ) ok_ambig[1] = false;
97 // cout<<"(b1*b1-4*a*c1) "<<(b1*b1-4*a*c1)<<endl;
98 // cout<<"(b1*b1-4*a*c2) "<<(b1*b1-4*a*c2)<<endl;
99 if( delta1>=0 ) {
100 double x1_0 = ( -b1+sqrt(delta1) ) /(2*a);
101 double x1_1 = ( -b1-sqrt(delta1) ) /(2*a);
102 //cout<<"x east west"<<xeast<<" "<<xwest<<endl;
103 //cout<<"x1 0 1 "<<x1_0<<" "<<x1_1<<endl;
104 if( (xeast>=x1_0 && xwest<=x1_0) or (xeast<=x1_0 && xwest>=x1_0) ) ok[0][0] = true;
105 if ( (xeast>=x1_1 && xwest<=x1_1) or (xeast<=x1_1 && xwest>=x1_1) ) ok[0][1]=true;
106 if( ok[0][0] == true && ok[0][1]==false ) x1 = x1_0;
107 if( ok[0][0] == false && ok[0][1]==true ) x1 = x1_1;
108 if( ok[0][0] == true && ok[0][1]==true ) {
109 x1 = x1_0; //??good
110 // cout<<" error both ok "<<endl;
111 }
112 if( ok[0][0] == false && ok[0][1]==false ) ok_ambig[0] = false;
113 y1 = k*x1+b;
114 double theta1 = 0;
115 double l1= 0;
116 double z1= 0;
117 if(ok_ambig[0]==true) {
118 calcu(x1,y1,xc,yc,rc,xeast,yeast,zeast,xwest,ywest,zwest,theta1,l1,z1);
119 if(m_debug>0) cout<<" theta1 l1 z1 "<<theta1<<" "<<l1<<" "<<z1<<endl;
120 _zleft = z1;
121 _lleft = l1;
122 }
123 //cout<<"left x y z "<<x1<<" "<<y1<<" "<<z1<<endl;
124 }
125
126 if( delta2>=0){
127 double x2_0 = ( -b1+sqrt(delta2) ) /(2*a);
128 double x2_1 = ( -b1-sqrt(delta2) ) /(2*a);
129 // cout<<"x2 0 1 "<<x2_0<<" "<<x2_1<<endl;
130 if( (xeast>=x2_0 && xwest<=x2_0) or (xeast<=x2_0 && xwest>=x2_0) ) ok[1][0] = true;
131 if ( (xeast>=x2_1 && xwest<=x2_1) or (xeast<=x2_1 && xwest>=x2_1) ) ok[1][1]=true;
132 if( ok[1][0] == true && ok[1][1]==false ) x2 = x2_0;
133 if( ok[1][0] == false && ok[1][1]==true ) x2 = x2_1;
134 if( ok[1][0] == true && ok[1][1]==true ) {
135 x2 = x2_0;
136 // cout<<" error both ok "<<endl;
137 }
138 if( ok[1][0] == false && ok[1][1]==false ) ok_ambig[1] = false;
139 y2 = k*x2+b;
140 double theta2 = 0;
141 double l2= 0;
142 double z2= 0;
143 if(ok_ambig[1]==true) {
144 calcu(x2,y2,xc,yc,rc,xeast,yeast,zeast,xwest,ywest,zwest,theta2,l2,z2);
145 if(m_debug>0) cout<<" theta2 l2 z2 "<<theta2<<" "<<l2<<" "<<z2<<endl;
146 _zright = z2;
147 _lright = l2;
148 }
149 //cout<<"right x y z "<<x2<<" "<<y2<<" "<<z2<<endl;
150 }
151 HepPoint3D leftPoint(x1,y1,_zleft);
152 HepPoint3D rightPoint(x2,y2,_zright);
153 _rechit->setLeftPoint(leftPoint);
154 _rechit->setRightPoint(rightPoint);
155 if( ok_ambig[0] == true && ok_ambig[1] == false ) return -1;
156 if( ok_ambig[0] == false && ok_ambig[1] == true ) return 1;
157 if( ok_ambig[0] == true && ok_ambig[1] == true ) return 2;
158 if( ok_ambig[0] == false && ok_ambig[1] == false ) return 0;
159 }
160 //cout<<" ztruth : "<<_rechit->getZTruth()<<endl;
161}
162
163
164void HoughStereo::calcu(double x1 ,double y1 ,double xc,double yc,double rc,double x_east,double y_east,double z_east,double x_west,double y_west,double z_west,double& theta,double& l,double& z){
165 //rphi -> sz
166 if(xc==0||x1-xc==0){
167 theta=0;
168 }
169 else{
170 theta=M_PI-atan2(y1-yc,x1-xc)+atan2(yc,xc);
171 if(theta>2*M_PI){
172 theta=theta-2*M_PI;
173 }
174 if(theta<0){
175 theta=theta+2*M_PI;
176 }
177 }
178 if(_charge == 1 ) theta = 2*M_PI-theta;
179
180 double d1=sqrt((x1-x_west)*(x1-x_west)+(y1-y_west)*(y1-y_west));
181 double d2=sqrt((x_east-x_west)*(x_east-x_west)+(y_east-y_west)*(y_east-y_west));
182 //cout<<"d1/d2 "<<d1/d2<<" "<<d1<<" "<<d2<<endl;
183 //cout<<"amplify "<<(z_west-z_east)/d2<<endl;
184// cout<<"zw ze "<<z_west<<" "<<z_east<<endl;
185 z = z_west-(z_west-z_east)*d1/d2;
186 l = rc*theta;
187}
188/*
189
190
191int HoughStereo::cald(){
192 HepPoint3D fp =_rechit->getWestPoint();
193 HepPoint3D rp =_rechit->getEastPoint();
194 HepPoint3D X=_rechit->getMidPoint();
195 if(m_debug>0) std::cout<<"fp rp "<<fp<<" "<<rp<<std::endl;//yzhang debug
196 if(m_debug>0) std::cout<<"Xmid "<<X<<std::endl;//yzhang debug
197 HepVector3D xx = HepVector3D(X.x(), X.y(), 0.);
198 HepPoint3D center( _circle->getCirX(),_circle->getCirY(),0 );
199 HepVector3D yy = center - xx;
200 HepVector3D ww = HepVector3D(yy.x(), yy.y(), 0.);
201 double wwmag2 = ww.mag2();
202 double wwmag = sqrt(wwmag2);
203 double driftdist = fabs( _rechit->calDriftDist(_bunchTime,_ambig) );
204 if(m_debug>0) cout<<"Bunch time: "<<_bunchTime<<endl;
205 HepVector3D lr(driftdist/wwmag * ww.x(), driftdist/wwmag * ww.y(), 0.);
206
207 if(m_debug>0){
208 std::cout<<"xc "<< _circle->getCirX()<< " yc "<<_circle->getCirY()<< " drfitdist "<<driftdist<<std::endl;
209 std::cout<<"lr "<<lr<<" "<<" ambig "<<_ambig
210 <<"left "<<_rechit->calDriftDist(0,1)
211 <<"right "<<_rechit->calDriftDist(0,-1)<<std::endl;
212 }
213
214 HepPoint3D ORIGIN = HepPoint3D(0., 0., 0.);
215 if (_ambig == 0) lr = ORIGIN;
216 if (_charge> 0){
217 if (_ambig == -1){
218 lr = - lr;
219 }
220 }else{
221 if (_ambig == 1){
222 lr = - lr;
223 }
224 }
225 X += lr;
226
227 HepPoint3D tmp(-9999., -9999., 0.);
228 HepVector3D x = HepVector3D(X.x(), X.y(), 0.);
229 HepVector3D w = x - center;
230 // //modified the next sentence because the direction are different from belle.
231 HepVector3D V = _rechit->wire()->zAxis();
232 HepVector3D v = HepVector3D(V.x(), V.y(), 0.);
233 double vmag2 = v.mag2();
234 //double vmag = sqrt(vmag2);
235 //double r = _helix->curv();
236 double wv = w.dot(v);
237 // wv = abs(wv);
238 if(m_debug>0){
239 std::cout<<"X_fix "<<X<<" center "<<center<<std::endl;
240 std::cout<<"V "<<V<<std::endl;//yzhang debug
241 std::cout<<"w "<<w<<" v "<<v<<std::endl;
242 std::cout<<"wv "<<wv<<endl;
243 }
244 double d2 = wv * wv - vmag2 * (w.mag2() - _circle->getCirR()* _circle->getCirR() );
245 if ( d2<0. ) return -1;
246 double d=sqrt(d2);
247
248 double l[2];
249 l[0] =-1*( (- wv + d) / vmag2 ); //multiply -1 ......?
250 l[1] =-1*( (- wv - d) / vmag2 );
251
252 double z[2];
253 //z[0] = X.z() + l[0] * V.z();
254 z[0] = X.z() - l[0] * V.z(); //l *-1
255 z[1] = X.z() - l[1] * V.z();
256 if(m_debug>0) cout<<"z0 z1: "<<z[0]<<" "<<z[1]<<endl;
257
258
259 bool ok[2];
260 ok[0] = true;
261 ok[1] = true;
262 //...Check z position...
263 if (_ambig == 0) {
264 if (z[0] > rp.z()+20. || z[0] < fp.z()-20.) {
265 ok[0] = false;
266 }
267 if (z[1] > rp.z()+20. || z[1] < fp.z()-20.) {
268 ok[1] = false;
269 }
270 } else {
271 if (fabs(z[0]/rp.z())>1.05 ) { ok[0] = false; }
272 if (fabs(z[1]/rp.z())>1.05 ) { ok[1] = false; }
273 }
274 if ((! ok[0]) && (! ok[1])) {
275 return -1;
276 }
277 if (( ok[0]) && ( ok[1])) {
278 return 2;
279 }
280
281 unsigned best = 0;
282 if (ok[1]) best = 1; //need improve
283 HepVector3D p[2];
284 p[0] = x + l[0] * v;
285 p[1] = x + l[1] * v;
286 double cosdPhi = - center.dot((p[best] - center).unit()) / center.mag();
287 double dPhi;
288 if(fabs(cosdPhi)<=1.0) {
289 dPhi = acos(cosdPhi);
290 } else if (cosdPhi>1.0) {
291 dPhi = 0.0;
292 } else {
293 dPhi = pi;
294 }
295
296 double stemp=_circle->getCirR()*dPhi;
297 if(_ambig==-1){
298 _zright=z[best];
299 _lright=stemp;
300 }
301 if(_ambig==1){
302 _zleft=z[best];
303 _lleft=stemp;
304 }
305 if(m_debug>0){
306 cout<<"in hough stereo "<<endl;
307 cout<<"left: "<<_zleft<<" "<<_lleft<<endl;
308 cout<<"right: "<<_zright<<" "<<_lright<<endl;
309 }
310 return 0;
311}
312*/
313
314
315void HoughStereo::setRecHit( ){
316 _rechit->setzAmb( 0 , _zleft);
317 _rechit->setsAmb( 0 , _lleft);
318 _rechit->setzAmb( 1 , _zright);
319 _rechit->setsAmb( 1 , _lright);
320}
321void HoughStereo::print(){
322 cout<<"Hit"<<"("<<_rechit->getLayerId()<<","<<_rechit->getWireId()<<") "<<_rechit->getStyle()<<endl;
323 cout<<" left: "<<_lleft<<" , "<<_zleft<<endl;
324 cout<<" right: "<<_lright<<" , "<<_zright<<endl;
325 double xc = _circle->getCirX();
326 double yc = _circle->getCirY();
327 double rc = _circle->getCirR();
328 double x1 = _rechit->getPointTruth().x();
329 double y1 = _rechit->getPointTruth().y();
330 double z1 = _rechit->getPointTruth().z();
331 double l(0),theta(0);
332 if(xc==0||x1-xc==0){
333 theta=0;
334 }
335 else{
336 theta=M_PI-atan2(y1-yc,x1-xc)+atan2(yc,xc);
337 if(theta>2*M_PI){
338 theta=theta-2*M_PI;
339 }
340 if(theta<0){
341 theta=theta+2*M_PI;
342 }
343 }
344 if(_charge == 1 ) theta = 2*M_PI-theta;
345 l = rc*theta;
346 cout<<" truth: "<<l<<" , "<<z1<<endl;
347 cout<<"truth x y z "<<x1<<" "<<y1<<" "<<z1<<endl;
348}
349
x
Double_t x[10]
Definition: DataBase/tau_mode.c:57
M_PI
#define M_PI
Definition: TConstant.h:4
HoughStereo::setRecHit
void setRecHit()
Definition: HoughStereo.cxx:315
HoughStereo::HoughStereo
HoughStereo(double bunchTime, Hough2D *circle, HoughRecHit *rechit)
Definition: HoughStereo.cxx:9
HoughStereo::setAmb
void setAmb(int i)
Definition: HoughStereo.cxx:17
HoughStereo::calcu
void calcu(double x1, double y1, double xc, double yc, double rc, double x_east, double y_east, double z_east, double x_west, double y_west, double z_west, double &theta, double &l, double &z)
Definition: HoughStereo.cxx:164
c1
TCanvas * c1
Definition: tau_mode.c:75