1 package org.lcsim.recon.tracking.ftf;
2 public class FtfUtil
3 {
4 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
5 // Invert matrix h of dimension n
6 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
7 //
8 //
9 // Originally written in FORTRAN by Jawluen Tang, Physics department , UT-Austin
10 // modified and translated to C by Pablo Yepes, Rice U.
11 //
12 // The following routine is to invert a square symmetric matrix
13 // (in our case,it is a 3x3 matrix,so NOD=3)and calculate its
14 // determinant,substituting the inverse matrix into the same array
15 // of the original matrix H.
16 // See Philip R. Bevington,"Data reduction and error analysis for
17 // the physical science",p302
18 //
19 public static void ftfInvertMatrix(int n, double[] h)
20 {
21 double detm, dmax_, temp;
22
23 int i, j, k, l;
24
25 int[] ik = new int[3];
26
27 int[] jk = new int[3];
28
29 detm = 1.;
30
31 for (k = 0 ; k < n ; ++k)
32 {
33 dmax_ = 0.;
34 j = -1 ;
35 while(j < k)
36 {
37 i = -1 ;
38 while(i < k)
39 {
40
41 for (i = k; i < n; ++i)
42 {
43
44 for (j = k; j < n; ++j)
45 {
46 if (Math.abs(dmax_) <= Math.abs(h[i+j*3]))
47 {
48 dmax_ = h[i + j * 3];
49 ik[k] = i;
50 jk[k] = j;
51 }
52 }
53 }
54 if (dmax_ == 0.)
55 {
56 System.out.println( "Determinant is ZERO!" );
57 return ;
58 }
59 i = ik[k];
60 }
61 if (i > k)
62 {
63
64 for (j = 0 ; j < n; ++j)
65 {
66 temp = h[k + j * 3];
67 h[k + j * 3] = h[i + j * 3];
68 h[i + j * 3] = -temp;
69 }
70 }
71 j = jk[k];
72 }
73 if (j != k)
74 {
75
76 for (i = 0 ; i < n; ++i)
77 {
78 temp = h[i + k * 3];
79 h[i + k * 3] = h[i + j * 3];
80 h[i + j * 3] = -temp;
81 }
82 }
83
84 for (i = 0 ; i < n; ++i)
85 {
86 if (i != k)
87 {
88 h[i + k * 3] = -(double)h[i + k * 3] / dmax_;
89 }
90 }
91
92 for (i = 0; i < n; ++i)
93 {
94
95 for (j = 0; j < n; ++j)
96 {
97 if (i != k && j != k)
98 {
99 h[i + j * 3] += h[i + k * 3] * h[k + j * 3];
100 }
101 }
102 }
103 for (j = 0; j < n; ++j)
104 {
105 if (j != k)
106 {
107 h[k + j * 3] /= dmax_;
108 }
109 }
110 h[k + k * 3] = 1.F / dmax_;
111 detm *= dmax_;
112 }
113 for (l = 0; l < n; ++l)
114 {
115 k = n - l -1 ;
116 j = ik[k];
117 if (j > k)
118 {
119 for (i = 0; i < n; ++i)
120 {
121 temp = h[i + k * 3];
122 h[i + k * 3] = -h[i + j * 3];
123 h[i + j * 3] = temp;
124 }
125 }
126 i = jk[k];
127 if (i > k)
128 {
129 for (j = 0; j < n; ++j)
130 {
131 temp = h[k + j * 3];
132 h[k + j * 3] = -h[i + j * 3];
133 h[i + j * 3] = temp;
134 }
135 }
136 }
137
138 }
139
140
141 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
142 // Function to give the diagonal elements (h11,h22,h33)
143 // of the inverse symmetric 3x3 matrix of h
144 // Calculation by Geary Eppley (Rice University)
145 // coded by Pablo Yepes (Rice University)
146 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
147 public static void ftfMatrixDiagonal( double[] h, double[] hxx )
148 {
149 double f1, f2, f3 ;
150
151 f1 = h[5]*h[6]-h[8]*h[1] ;
152 f2 = h[4]*h[8]-h[5]*h[5] ;
153 f3 = h[8]*h[0]-h[2]*h[2] ;
154 hxx[0] = (h[8] / ( f3 - f1 * f1 / f2 )) ;
155
156 f1 = h[2]*h[1]-h[0]*h[5] ;
157 f2 = h[8]*h[0]-h[2]*h[2] ;
158 f3 = h[0]*h[4]-h[1]*h[1] ;
159 hxx[1] = (h[0] / ( f3 - f1 * f1 / f2 )) ;
160
161 f1 = h[1]*h[5]-h[4]*h[2] ;
162 f2 = h[0]*h[4]-h[1]*h[1] ;
163 f3 = h[4]*h[8]-h[7]*h[7] ;
164 hxx[2] = (h[4] / ( f3 - f1 * f1 / f2 )) ;
165 }
166
167 //
168 // simple utility to print out 3x3 matrix
169 //
170
171 public static void ftfPrintMatrix( double[] h)
172 {
173 for(int i=0; i<3; ++i) System.out.print(h[i]+" ");
174 System.out.println("\n");
175 for(int i=3; i<6; ++i) System.out.print(h[i]+" ");
176 System.out.println("\n");
177 for(int i=6; i<9; ++i) System.out.print(h[i]+" ");
178 System.out.println("\n");
179
180 }
181
182 }