1
2
3
4
5
6
7
8 package org.lcsim.fit.zsegment;
9
10 import hep.physics.matrix.BasicMatrix;
11 import hep.physics.matrix.Matrix;
12 import hep.physics.matrix.MatrixOp;
13 import hep.physics.matrix.MutableMatrix;
14 import hep.physics.matrix.SymmetricMatrix;
15
16 import java.util.ArrayList;
17 import java.util.List;
18
19
20
21
22
23
24
25
26
27
28 public class ZSegmentFitter {
29
30 private double[] _s;
31 private double[] _zmin;
32 private double[] _zmax;
33 private List<double[]> _polygon;
34 private double _area;
35 private double[] _centroid = new double[2];
36 private SymmetricMatrix _covariance;
37 private double _eps = 1e-6;
38
39
40
41
42 public ZSegmentFitter() {
43
44 }
45
46
47
48
49
50
51
52
53
54 public boolean fit(double[] s, double[] zmin, double[] zmax) {
55
56 _s = s;
57 _zmin = zmin;
58 _zmax = zmax;
59 _polygon = new ArrayList<double[]>();
60 _centroid[0] = 0.;
61 _centroid[1] = 0.;
62 _area = 0.;
63 _covariance = null;
64
65 if (_s.length!=_zmin.length) return false;
66 if (_s.length!=_zmax.length) return false;
67 if (_s.length<2) return false;
68
69
70
71
72
73
74
75
76
77
78
79
80
81 for (int i=0; i<_s.length-1; i++) {
82 for (int j=i+1; j<_s.length; j++) {
83 IntersectLines(_s[i],_zmin[i],_s[j],_zmin[j]);
84 IntersectLines(_s[i],_zmin[i],_s[j],_zmax[j]);
85 IntersectLines(_s[i],_zmax[i],_s[j],_zmin[j]);
86 IntersectLines(_s[i],_zmax[i],_s[j],_zmax[j]);
87 }
88 }
89
90
91
92 int nv = _polygon.size();
93 if (nv<3) return false;
94
95
96
97 OrderVertices();
98
99
100 for (int i=0; i<nv; i++) {
101 int j = (i+1) % nv;
102 double[] p0 = _polygon.get(i);
103 double[] p1 = _polygon.get(j);
104 double darea = 0.5*(p0[0]*p1[1] - p1[0]*p0[1]);
105 _area += darea;
106 _centroid[0] += (p0[0]+p1[0]) * darea;
107 _centroid[1] += (p0[1]+p1[1]) * darea;
108 }
109 _centroid[0] /= 3*_area;
110 _centroid[1] /= 3*_area;
111 _area = Math.abs(_area);
112
113
114 _covariance = PolygonCovariance();
115
116 return true;
117 }
118
119 private void IntersectLines(double s1, double o1, double s2, double o2) {
120
121 double[] cross = new double[2];
122 if (s1==s2) return;
123 cross[0] = (o1*s2 - o2*s1) / (s2 - s1);
124 cross[1] = (o2 - o1) / (s2 - s1);
125
126
127 for (int i=0; i<_s.length; i++) {
128 double zpred = cross[0] + _s[i]*cross[1];
129 if (zpred<_zmin[i]-_eps || zpred>_zmax[i]+_eps ) return;
130 }
131
132
133 for (double[] old_cross : _polygon) {
134 if (Math.pow(cross[0]-old_cross[0],2)+Math.pow(cross[1]-old_cross[1],2)<Math.pow(_eps,2)) return;
135 }
136
137
138 _polygon.add(cross);
139 }
140
141 private void OrderVertices() {
142
143 double[] pcent = PseudoCentroid();
144 int nv = _polygon.size();
145 for (int i=0; i<nv-1; i++) {
146 for (int j=i+1; j<nv; j++) {
147
148 double phi1 = Math.atan2(_polygon.get(i)[1]-pcent[1],_polygon.get(i)[0]-pcent[0]);
149 double phi2 = Math.atan2(_polygon.get(j)[1]-pcent[1],_polygon.get(j)[0]-pcent[0]);
150 if (phi1>phi2) {
151 double[] temp = _polygon.get(j);
152 _polygon.set(j,_polygon.get(i));
153 _polygon.set(i,temp);
154 }
155 }
156 }
157 }
158
159 private double[] PseudoCentroid() {
160
161 double[] pcent = {0.,0.};
162 int nv = _polygon.size();
163 for (double[] point : _polygon) {
164 pcent[0] += point[0]/ nv;
165 pcent[1] += point[1]/ nv;
166 }
167 return pcent;
168 }
169
170 private SymmetricMatrix PolygonCovariance() {
171
172
173
174
175 int nv = _polygon.size();
176 double cxx = 0.;
177 double cxy = 0.;
178 double cyy = 0.;
179 for (int i=0; i<nv; i++) {
180 int j = (i+1) % nv;
181
182
183 BasicMatrix vertices = new BasicMatrix(2,2);
184 vertices.setElement(0,0,_polygon.get(i)[0]-_centroid[0]);
185 vertices.setElement(1,0,_polygon.get(i)[1]-_centroid[1]);
186 vertices.setElement(0,1,_polygon.get(j)[0]-_centroid[0]);
187 vertices.setElement(1,1,_polygon.get(j)[1]-_centroid[1]);
188
189
190 double phi = Math.atan2(vertices.e(1,1),vertices.e(0,1));
191 BasicMatrix rotmat = new BasicMatrix(2,2);
192 rotmat.setElement(0,0,Math.cos(phi));
193 rotmat.setElement(0,1,Math.sin(phi));
194 rotmat.setElement(1,0,-rotmat.e(0,1));
195 rotmat.setElement(1,1,rotmat.e(0,0));
196 Matrix rotvert = MatrixOp.mult(rotmat,vertices);
197
198
199
200
201
202
203
204 double darea = Math.abs(0.5*rotvert.e(1,0)*rotvert.e(0,1));
205 SymmetricMatrix cov_loc = new SymmetricMatrix(2);
206 cov_loc.setElement(0,0,darea*(Math.pow(rotvert.e(0,0),2)+rotvert.e(0,0)*rotvert.e(0,1)+Math.pow(rotvert.e(0,1),2))/6.);
207 cov_loc.setElement(1,0,darea*rotvert.e(1,0)*(2.*rotvert.e(0,0)+rotvert.e(0,1))/12);
208 cov_loc.setElement(1,1,darea*Math.pow(rotvert.e(1,0),2)/6);
209
210
211
212 MutableMatrix rotbar = new BasicMatrix(2,2);
213 MatrixOp.transposed(rotmat,rotbar);
214 Matrix cov_glb = MatrixOp.mult(rotbar,MatrixOp.mult(cov_loc,rotmat));
215
216
217 cxx += cov_glb.e(0,0);
218 cxy += cov_glb.e(1,0);
219 cyy += cov_glb.e(1,1);
220 }
221
222
223 SymmetricMatrix cov = new SymmetricMatrix(2);
224 cov.setElement(0,0,cxx/_area);
225 cov.setElement(1,0,cxy/_area);
226 cov.setElement(1,1,cyy/_area);
227
228 return cov;
229 }
230
231
232
233
234
235 public ZSegmentFit getFit() {
236 return new ZSegmentFit(_polygon, _centroid, _covariance);
237 }
238 }