1 package org.lcsim.recon.cat;
2
3
4
5
6
7
8 import java.util.*;
9 import java.lang.Math;
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31 final public class GarfieldHelix
32 {
33
34 private static double k_cm = 10.;
35 private double[] base;
36 private double[] dir;
37 private double kappa;
38 private double[] pointOnHelix;
39 private double sSave;
40 private int debugLevel;
41
42
43
44
45
46
47 public GarfieldHelix()
48 {
49 this.base = new double[]{0.,0.,0.};
50 this.dir = new double[]{1.,0.,0.};
51 this.pointOnHelix = new double[]{0.,0.,0.};
52 this.kappa = 0.;
53 this.sSave = 0.;
54 normalizeDir();
55 debugLevel=0;
56 }
57
58
59
60
61
62 public GarfieldHelix(double[] b, double[] d, double ka)
63 {
64 this.base = new double[]{b[0],b[1],b[2]};
65 this.dir = new double[]{d[0],d[1],d[2]};;
66 this.pointOnHelix = new double[]{0.,0.,0.};
67 normalizeDir();
68 kappa = ka;
69 this.sSave = 0.;
70 debugLevel = 0;
71 }
72
73
74 public int q() {
75 if (kappa<0.) return -1;
76 return 1;
77 }
78 public double sSave(){ return sSave;}
79 public double dir(int i){ return dir[i];}
80 public double base(int i){ return base[i];}
81 public double[] dir(){ return dir;}
82 public double[] base(){ return base;}
83 public double kappa(){ return kappa;}
84 final public double getPointOnHelix(int i){ return pointOnHelix[i];}
85
86
87
88 public void setDebugLevel(int i){ debugLevel = i;}
89
90 public void setKappa(double ka){ kappa=ka;}
91
92 public void setBase(double x,double y,double z)
93 {
94 base[0] = x;
95 base[1] = y;
96 base[2] = z;
97 }
98
99 public void setDir(double x,double y, double z)
100 {
101 dir[0] = x;
102 dir[1] = y;
103 dir[2] = z;
104 normalizeDir();
105 }
106
107
108 public double sqr(double x){ return x*x;}
109
110
111
112
113
114 public void normalizeDir(){
115 double r=Math.sqrt(dir[0]*dir[0]+dir[1]*dir[1]);
116 if (r>1.E-25){
117 dir[0]=dir[0]/r;
118 dir[1]=dir[1]/r;
119 dir[2]=dir[2]/r;
120 }
121 }
122
123 final public double distanceToHit3D(double[] pos)
124 {
125
126 if (Math.abs(pos[2])>100.*k_cm && Math.abs(dir[2])>0.1) setPointOnHelixWithZ(pos[2]);
127 else setPointOnHelixWithXY(pos[0],pos[1]);
128
129 return Math.sqrt((pos[0]-pointOnHelix[0])*(pos[0]-pointOnHelix[0])+(pos[1]-pointOnHelix[1])*(pos[1]-pointOnHelix[1])+(pos[2]-pointOnHelix[2])*(pos[2]-pointOnHelix[2]));
130 }
131
132 final public double distanceBaseToPoint(double[] p){
133 return Math.sqrt(sqr(p[0]-base[0])+sqr(p[1]-base[1]));
134 }
135
136
137 final public double distanceBaseToPoint(GarfieldHit h){
138 return Math.sqrt(sqr(base[0]-h.getPoint(0))+sqr(base[1]-h.getPoint(1)));
139 }
140
141
142
143
144 private final static double myAbs(double ka){return ka>0.? ka : -ka;}
145
146
147
148
149 final private double sinfrac(double x, double ka){
150
151
152
153 double kabs=ka>0.? ka : -ka;
154 double xx=x*ka;
155 double y= 0.;
156 if (kabs > 1.E-3){
157 y=Math.sin(xx)/ka;
158 }
159 else if (kabs > 1.E-5){
160 y= x - (x*xx*xx)/6. + (x*xx*xx*xx*xx)/120.;
161 }
162 else if (kabs>1.E-7){
163 y= x - (x*xx*xx)/6.;
164 }
165 else{
166 y= x ;
167 }
168 return y;
169 }
170
171
172
173
174 final private double cosfrac(double x, double ka){
175
176
177
178 double kabs=ka>0.? ka : -ka;
179 double xx=x*ka;
180 double y= 0.;
181 if (kabs > 1.E-3){
182 y=(Math.cos(xx)-1.)/ka;
183 }
184 else if (kabs > 1.E-5){
185 y= -(x*xx)/2. + (x*xx*xx*xx)/24.;
186 }
187 else{
188 y= -(x*xx)/2.;
189 }
190 return y;
191 }
192
193
194
195
196
197 final private double distanceToInterval(double a, double b, double c)
198 {
199 if (a<b){
200 if (c<a) return Math.abs(a-c);
201 else if (c>b) return Math.abs(c-b);
202 else return 0.;
203 }
204 else{
205 if (c<b) return Math.abs(b-c);
206 else if (c>a) return Math.abs(c-a);
207 else return 0.;
208 }
209 }
210
211 final public double getCenter(int i){
212 if (i==0) return base[0]-dir[1]/kappa;
213 if (i==1) return base[1]+dir[0]/kappa;
214 return 0.;
215 }
216
217
218
219
220
221 public void setPointOnHelix(double s) {
222 sSave = s;
223 double cfrac=cosfrac(s,kappa);
224 double sfrac=sinfrac(s,kappa);
225 pointOnHelix[0]=base[0]+dir[1]*cfrac + dir[0]*sfrac;
226 pointOnHelix[1]=base[1]-dir[0]*cfrac + dir[1]*sfrac;
227 pointOnHelix[2]=base[2]+dir[2]*s;
228 }
229
230 public void setPointOnHelixWithXY(double x, double y) {
231
232
233 double kabs = kappa>0.? kappa : -kappa;
234 if (kabs<1.E-7){
235 double s=dir[0]*(x-base[0])+dir[1]*(y-base[1]);
236 setPointOnHelix(s);
237 }
238 else{
239 double d = Math.sqrt((x-base[0])*(x-base[0])+(y-base[1])*(y-base[1]));
240 if (d<1.E-12){
241 setPointOnHelix(0.);
242 return;
243 }
244 double cx=getCenter(0);
245 double cy=getCenter(1);
246 double l = Math.sqrt((x-cx)*(x-cx)+(y-cy)*(y-cy));
247 double R = 1./kabs;
248 double cosAngle = 0.5*(l/R+R/l-d*d/(l*R));
249 double s=0.;
250 if (cosAngle <=1. && cosAngle >= -1.) s=Math.acos(cosAngle)*R;
251 else if (debugLevel>=4){
252 System.out.println("GarfieldHelix problem in setXY"+cosAngle+" "+l+" "+R+" "+d);
253 System.out.println(" center x="+cx+" "+cy);
254 }
255 if (dir[0]*(x-base[0])+dir[1]*(y-base[1])>0.) setPointOnHelix(s);
256 else setPointOnHelix(-s);
257 }
258 }
259
260 public void setPointOnHelixWithZ(double z) {
261
262
263 if (dir[2]>1.E-12 || dir[2]<-1.E-12){
264 sSave = (z-base[2])/dir[2];
265 }
266 else{
267 if (debugLevel>=4) {System.out.println("problem in GarfieldHelix setPointOnHelixWithZ, dir2="+dir[2]);}
268 sSave= 10.;
269 }
270 double cfrac=cosfrac(sSave,kappa);
271 double sfrac=sinfrac(sSave,kappa);
272 pointOnHelix[0]=base[0]+dir[1]*cfrac + dir[0]*sfrac;
273 pointOnHelix[1]=base[1]-dir[0]*cfrac + dir[1]*sfrac;
274 pointOnHelix[2]=z;
275 }
276
277 public double dirAtPoint(int i)
278 {
279 double phi= sSave*kappa;
280 if (i==2) return dir[2];
281 else if (i==0) return dir[0]*Math.cos(phi)-dir[1]*Math.sin(phi);
282 else if (i==1) return dir[1]*Math.cos(phi)+dir[0]*Math.sin(phi);
283 else {
284 System.out.println("GarfieldHelix dirAtPoint bad index i="+i);
285 return -999.;
286 }
287 }
288
289
290 public double distanceToLine2D(GarfieldHit h, double px, double py)
291 {
292 double x = h.getLength2D();
293 double xx = h.x1(0)-h.x0(0);
294 double xy = h.x1(1)-h.x0(1);
295 double lx = px - h.x0(0);
296 double ly = py - h.x0(1);
297 double ll = lx*lx+ly*ly;
298 double lDotX=lx*xx+ly*xy;
299 if (lDotX<0.) return Math.sqrt(ll);
300 if (lDotX > x*x) return Math.sqrt((px - h.x1(0))*(px - h.x1(0))+(py - h.x1(1))*(py - h.x1(1)));
301 return Math.sqrt(ll-(lDotX/x)*(lDotX/x));
302 }
303
304
305 public double distanceToHit2D(GarfieldHit h)
306 {
307 double kabs = Math.abs(kappa);
308 if (h.hasZ() && Math.abs(dir[2])>0.1){
309 setPointOnHelixWithZ(h.getPoint(2));
310 return distanceToLine2D(h,pointOnHelix[0],pointOnHelix[1]);
311 }
312 else{
313 double x=h.getPoint(0);
314 double y=h.getPoint(1);
315 setPointOnHelixWithXY(x,y);
316 double eps = distanceToInterval(h.x0(2),h.x1(2),pointOnHelix[2]);
317
318
319
320
321
322 return Math.sqrt(sqr(x-pointOnHelix[0])+sqr(y-pointOnHelix[1]));
323 }
324 }
325
326 }
327
328
329
330
331