1 package org.lcsim.recon.cat;
2
3 import hep.physics.vec.BasicHep3Vector;
4 import hep.physics.vec.BasicHepLorentzVector;
5 import org.lcsim.recon.cat.GarfieldTrack;
6 import java.util.*;
7 import java.lang.Math;
8 import org.lcsim.recon.cat.util.Const;
9
10
11
12
13
14
15
16
17
18
19
20
21
22 final public class SimpleConstrainedFit
23 {
24 private int nIter;
25 private double chi2;
26 private double bField=5.;
27
28
29 private int debugLevel;
30 public GarfieldTrack track1;
31 public GarfieldTrack track2;
32 private double[] vtx2D;
33 private double rXY;
34 double[] bas1;
35 double[] bas2;
36 double[] dir1;
37 double[] dir2;
38
39 private BasicHepLorentzVector pp1;
40 private BasicHepLorentzVector pp2;
41 private BasicHepLorentzVector ppsum;
42
43 private boolean usePrimaryVertexConstraint=true;
44
45 public SimpleConstrainedFit(){
46 nIter = 10;
47 debugLevel =0;
48 chi2=0.;
49 debugLevel=0;
50 vtx2D = new double[]{0.,0.};
51 bas1 = new double[]{0.,0.,0.};
52 bas2 = new double[]{0.,0.,0.};
53 dir1 = new double[]{0.,0.,0.};
54 dir2 = new double[]{0.,0.,0.};
55 track1 = new GarfieldTrack();
56 track2 = new GarfieldTrack();
57 pp1=new BasicHepLorentzVector();
58 pp2=new BasicHepLorentzVector();
59 ppsum=new BasicHepLorentzVector();
60 System.out.println("SimpleConstrainedFit constructor");
61 }
62 public void setDebugLevel(int i){ debugLevel = i;}
63 public void setPrimaryVertexConstraint(boolean val){ usePrimaryVertexConstraint = val;}
64 public void setNIter(int i){ nIter = i;}
65
66 private void calculateChi2(){
67 track1.hel.setPointOnHelixWithXY(vtx2D[0],vtx2D[1]);
68 track2.hel.setPointOnHelixWithXY(vtx2D[0],vtx2D[1]);
69 double z1= track1.hel.getPointOnHelix(2);
70 double z2= track2.hel.getPointOnHelix(2);
71 track1.calculateChi2();
72 track2.calculateChi2();
73 chi2 = track1.getChi2()+track2.getChi2()+(z1-z2)*(z1-z2)*1.E4;
74 if (usePrimaryVertexConstraint &&
75 Math.abs(track1.getPara("kappa")) > 1.E-10 &&
76 Math.abs(track2.getPara("kappa")) > 1.E-10 ){
77 double dZ= 0.5*(z1+z2);
78 double tanVtx = dZ / rXY;
79 getTrack4MomentumAtRxy(track1, vtx2D[0],vtx2D[1], 0.139, pp1);
80 getTrack4MomentumAtRxy(track2, vtx2D[0],vtx2D[1], 0.139, pp2);
81 addLorentzVector(pp1,pp2,ppsum);
82 double tanK0s= ppsum.v3().z()/Math.sqrt(ppsum.v3().x()*ppsum.v3().x()+ppsum.v3().y()*ppsum.v3().y());
83 chi2=chi2+(tanVtx-tanK0s)*(tanVtx-tanK0s)*1.E10;
84
85 }
86
87 }
88
89 private void getTrack4MomentumAtRxy(GarfieldTrack g, double xpos, double ypos, double mass, BasicHepLorentzVector pp) {
90 double pt= g.getPt(bField);
91 (g.hel).setPointOnHelixWithXY(xpos,ypos);
92 double px = pt*(g.hel).dirAtPoint(0);
93 double py = pt*(g.hel).dirAtPoint(1);
94 double pz = pt*g.getPara("lambda");
95 double en = Math.sqrt(mass*mass+px*px+py*py+pz*pz);
96 pp.setV3(en, px, py, pz);
97 }
98
99 private void addLorentzVector(BasicHepLorentzVector a, BasicHepLorentzVector b , BasicHepLorentzVector sum){
100 sum.setV3(component(a,0)+component(b,0),
101 component(a,1)+component(b,1),
102 component(a,2)+component(b,2),
103 component(a,3)+component(b,3));
104 }
105
106 private double component(BasicHepLorentzVector a, int i)
107 {
108 if (i==0) return a.t();
109 else return (a.v3().v())[i-1];
110 }
111
112
113
114
115
116
117
118
119
120
121 public boolean fitTwoTracksToVertexXY(double[] v, GarfieldTrack t1, GarfieldTrack t2){
122 vtx2D[0]=v[0];
123 vtx2D[1]=v[1];
124 track1= new GarfieldTrack(t1);
125
126 track1.calculateChi2();
127 track1.setDebugLevel(debugLevel);
128 track1.hel.setPointOnHelixWithXY(track1.getMipStubBase(0),track1.getMipStubBase(1));
129 track1.hel.setDir(track1.hel.dirAtPoint(0),track1.hel.dirAtPoint(1),track1.hel.dirAtPoint(2));
130 track1.hel.setBase(track1.hel.getPointOnHelix(0),track1.hel.getPointOnHelix(1),
131 track1.hel.getPointOnHelix(2));
132 track2= new GarfieldTrack(t2);
133
134 track2.setDebugLevel(debugLevel);
135 track2.hel.setPointOnHelixWithXY(track2.getMipStubBase(0),track2.getMipStubBase(1));
136 track2.hel.setDir(track2.hel.dirAtPoint(0),track2.hel.dirAtPoint(1),track2.hel.dirAtPoint(2));
137 track2.hel.setBase(track2.hel.getPointOnHelix(0),track2.hel.getPointOnHelix(1),
138 track2.hel.getPointOnHelix(2));
139
140 rXY=Math.sqrt(v[0]*v[0]+v[1]*v[1]);
141
142 calculateChi2();
143 double chi2Diff = track1.getChi2()+track2.getChi2()-t1.getChi2()+t2.getChi2();
144 if (debugLevel>=2){
145 System.out.println("SimpleConstrainedFit FULLChi2Fit before"+chi2+" chi2Diff="+
146 chi2Diff);
147 }
148 if (chi2Diff>1.E5){
149 if (debugLevel>=1) System.out.println("SimpleConstrainedFit refitting track");
150 track1.fullChi2Fit(0.1,10);
151 track2.fullChi2Fit(0.1,10);
152 }
153
154 track1.hel.setPointOnHelixWithXY(v[0],v[1]);
155 track2.hel.setPointOnHelixWithXY(v[0],v[1]);
156 double z1= track1.hel.getPointOnHelix(2);
157 double z2= track2.hel.getPointOnHelix(2);
158 double zMin = Math.min(z1,z2);
159 double zMax = Math.max(z1,z2);
160 double z=zMin;
161 double zStep=(Math.abs(zMax-zMin)/20.+1.*Const.mm);
162 boolean done =false;
163 double oldChi2=chi2;
164 int oldLevel =debugLevel;
165 this.debugLevel=0;
166 int n=0;
167 int nRounds = 0;
168 while (nRounds < nIter && chi2 > 3.){
169 done=false;
170 while (!done){
171 chi2FitIteration(zStep/rXY);
172 n++;
173 if (oldChi2<=chi2) done =true;
174 oldChi2=chi2;
175 if (n>4000){
176 this.debugLevel=oldLevel;
177 if (debugLevel>=2) System.out.println("GarfieldTrack FullChi2Fit runaway chi2="
178 +chi2+" zstep="+zStep);
179 this.track1.setTrackParameters();
180 this.track2.setTrackParameters();
181 if (chi2>1.E5) return false;
182 else return true;
183 }
184 }
185 zStep = zStep/4.;
186
187 nRounds++;
188 }
189 this.debugLevel=oldLevel;
190 if (debugLevel>=2) System.out.println("++FULLChi2Fit after"+chi2+" n="+n+" zstep="+zStep+" nRounds"+nRounds);
191 this.track1.setTrackParameters();
192 this.track2.setTrackParameters();
193 if (debugLevel>=4) {
194 System.out.println("SimpleConstrainedFit first track");
195 t1.debug();
196 System.out.println("first track constrained");
197 this.track1.debug();
198 System.out.println("second track");
199 t2.debug();
200 System.out.println("second track constrained");
201 this.track2.debug();
202 }
203 if (chi2>1.E8) return false;
204 return true;
205 }
206
207 private void chi2FitIteration(double dirStep){
208
209 double deltaD = dirStep;
210 calculateChi2();
211 int nItera = 2;
212 double oldChi2=chi2;
213 dir1[0]=track1.hel.dir(0);
214 dir1[1]=track1.hel.dir(1);
215 dir1[2]=track1.hel.dir(2);
216 dir2[0]=track2.hel.dir(0);
217 dir2[1]=track2.hel.dir(1);
218 dir2[2]=track2.hel.dir(2);
219
220 bas1[0]=track1.hel.base(0);
221 bas1[1]=track1.hel.base(1);
222 bas1[2]=track1.hel.base(2);
223 bas2[0]=track2.hel.base(0);
224 bas2[1]=track2.hel.base(1);
225 bas2[2]=track2.hel.base(2);
226
227 double bestDirZtrack1=track1.hel.dir(2);
228 double bestDirZtrack2=track2.hel.dir(2);
229 double minChi2=chi2;
230
231 for (int i1=-nItera; i1<=nItera;i1++){
232 track1.hel.setDir(dir1[0],dir1[1],dir1[2]+deltaD*i1);
233 for (int i2=-nItera; i2<=nItera;i2++){
234 track2.hel.setDir(dir2[0],dir2[1],dir2[2]+deltaD*i2);
235 calculateChi2();
236 if (chi2<minChi2){
237 bestDirZtrack1=track1.hel.dir(2);
238 bestDirZtrack2=track2.hel.dir(2);
239 minChi2=chi2;
240 }
241 }
242 }
243 track1.hel.setDir(dir1[0],dir1[1],bestDirZtrack1);
244 track2.hel.setDir(dir2[0],dir2[1],bestDirZtrack2);
245 calculateChi2();
246 }
247 }