StRoot  1
FtfUtilities.cxx
1 //:>------------------------------------------------------------------
2 //: FILE: FTF_Utilities.cxx
3 //: HISTORY:
4 //: 28oct1996 version 1.00
5 //:<------------------------------------------------------------------
6 //:>------------------------------------------------------------------
7 //: FUNCTIONS: Standalone functions
8 //: DESCRIPTION: Generic functions used in tracking
9 //: AUTHOR: ppy - Pablo Yepes, yepes@physics.rice.edu
10 //:>------------------------------------------------------------------
11 #include <stdio.h>
12 #include <math.h>
13
14 //void invert_matrix ( int n, double *h ) ;
15 //void matrix_diagonal ( double *h, double *h11, double *h22, double *h33 ) ;
16 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
17 // Invert matrix h of dimension n
18 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
19 //
20 //
21 // Originally written in FORTRAN by Jawluen Tang, Physics department , UT-Austin
22 // modified and translated to C by Pablo Yepes, Rice U.
23 //
24 // The following routine is to invert a square symmetric matrix
25 // (in our case,it is a 3x3 matrix,so NOD=3)and calculate its
26 // determinant,substituting the inverse matrix into the same array
27 // of the original matrix H.
28 // See Philip R. Bevington,"Data reduction and error analysis for
29 // the physical science",p302
30 //
31 void ftfInvertMatrix ( int n, double *h ) {
32  static double detm, dmax_, temp;
33  static int i, j, k, l;
34  static int ik[3], jk[3];
35
36  detm = 1.F ;
37
38  for (k = 0 ; k < n ; ++k) {
39  dmax_ = (double)0.;
40  j = -1 ;
41  while(j < k) {
42  i = -1 ;
43  while(i < k) {
44
45  for (i = k; i < n; ++i) {
46
47  for (j = k; j < n; ++j) {
48  if (fabs(dmax_) <= fabs(h[i+j*3]))
49  {
50  dmax_ = h[i + j * 3];
51  ik[k] = i;
52  jk[k] = j;
53  }
54  }
55  }
56  if (dmax_ == (double)0.) {
57  //printf ( "Determinant is ZERO!" );
58  return ;
59  }
60  i = ik[k];
61  }
62  if (i > k) {
63
64  for (j = 0 ; j < n; ++j) {
65  temp = h[k + j * 3];
66  h[k + j * 3] = h[i + j * 3];
67  h[i + j * 3] = -(double)temp;
68  }
69  }
70  j = jk[k];
71  }
72  if (j != k) {
73
74  for (i = 0 ; i < n; ++i) {
75  temp = h[i + k * 3];
76  h[i + k * 3] = h[i + j * 3];
77  h[i + j * 3] = -(double)temp;
78  }
79  }
80
81  for (i = 0 ; i < n; ++i) {
82  if (i != k) {
83  h[i + k * 3] = -(double)h[i + k * 3] / dmax_;
84  }
85  }
86
87  for (i = 0; i < n; ++i) {
88
89  for (j = 0; j < n; ++j) {
90  if (i != k && j != k) {
91  h[i + j * 3] += h[i + k * 3] * h[k + j * 3];
92  }
93  }
94  }
95  for (j = 0; j < n; ++j) {
96  if (j != k) {
97  h[k + j * 3] /= dmax_;
98  }
99  }
100  h[k + k * 3] = 1.F / dmax_;
101  detm *= dmax_;
102  }
103  for (l = 0; l < n; ++l) {
104  k = n - l -1 ;
105  j = ik[k];
106  if (j > k) {
107  for (i = 0; i < n; ++i) {
108  temp = h[i + k * 3];
109  h[i + k * 3] = -(double)h[i + j * 3];
110  h[i + j * 3] = temp;
111  }
112  }
113  i = jk[k];
114  if (i > k) {
115  for (j = 0; j < n; ++j) {
116  temp = h[k + j * 3];
117  h[k + j * 3] = -(double)h[i + j * 3];
118  h[i + j * 3] = temp;
119  }
120  }
121  }
122  return ;
123 }
124 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
125 // Function to give the diagonal elements (h11,h22,h33)
126 // of the inverse symmetric 3x3 matrix of h
127 // Calculation by Geary Eppley (Rice University)
128 // coded by Pablo Yepes (Rice University)
129 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
130 void ftfMatrixDiagonal ( double *h, double &h11, double &h22, double &h33 ){
131  double f1, f2, f3 ;
132
133  f1 = h[5]*h[6]-h[8]*h[1] ;
134  f2 = h[4]*h[8]-h[5]*h[5] ;
135  f3 = h[8]*h[0]-h[2]*h[2] ;
136  h11 = double(h[8] / ( f3 - f1 * f1 / f2 )) ;
137
138  f1 = h[2]*h[1]-h[0]*h[5] ;
139  f2 = h[8]*h[0]-h[2]*h[2] ;
140  f3 = h[0]*h[4]-h[1]*h[1] ;
141  h22 = double(h[0] / ( f3 - f1 * f1 / f2 )) ;
142
143  f1 = h[1]*h[5]-h[4]*h[2] ;
144  f2 = h[0]*h[4]-h[1]*h[1] ;
145  f3 = h[4]*h[8]-h[7]*h[7] ;
146  h33 = double(h[4] / ( f3 - f1 * f1 / f2 )) ;
147 }