LCFIVertex  0.7.2
matrix.cpp
1 #include "../inc/matrix.h"
2 
3 
4 //fg: boost headers should be included here in user code
5 #include <boost/numeric/ublas/operation.hpp>
6 #include <boost/numeric/ublas/vector_proxy.hpp>
7 #include <boost/numeric/ublas/matrix_proxy.hpp>
8 #include <boost/numeric/ublas/vector.hpp>
9 #include <boost/numeric/ublas/triangular.hpp>
10 
11 
12 namespace vertex_lcfi
13 {
14 namespace util
15 {
16 /* Matrix inversion routine.- only 3x3 for now - make sure input type is not symetric
17  Uses lu_factorize and lu_substitute in uBLAS to invert a matrix */
18 //template<class T>
19 SymMatrix5x5 InvertMatrix5x5(SymMatrix5x5 input)
20 {
21  using namespace boost::numeric::ublas;
22  // create a working copy of the input
23  matrix<double> A(input);
24  //std::cout << "A1: " << A << std::endl;
25  // perform LU-factorization
26  lu_factorize(A);
27  //std::cout << "A2: " << A << std::endl;
28 
29  // create identity matrix of "inverse"
30  matrix<double> B(5,5);
31  B.clear();
32  for (unsigned int i = 0; i < A.size1(); i++)
33  B(i,i) = 1;
34  //std::cout << "B1: " << B << std::endl;
35  // backsubstitute to get the inverse
36  lu_substitute<const matrix<double>,matrix<double> >(A,B);//const matrix<T>, matrix<T> >(A, inverse);
37  //std::cout << "B2: " << B << std::endl;
38  return B;
39 }
40 
41 double determinant(Matrix3x3 a)
42 {
43  double det = -a(0,2)*a(1,1)*a(2,0) + a(0,1)*a(1,2)*a(2,0) + a(0,2)*a(1,0)*a(2,1) - a(0,0)*a(1,2)*a(2,1) - a(0,1)*a(1,0)*a(2,2) + a(0,0)*a(1,1)*a(2,2);
44 
45  return det;
46 }
47 
48 Matrix3x3 InvertMatrix(Matrix3x3 a)//const ublas::matrix<T>& input, ublas::matrix<T>& inverse)
49 {
50  using namespace boost::numeric::ublas;
51  //std::cout.precision(100);
52  //std::cout << a << std::endl;
53  //a = a * (10000.0*10000.0); //Convert to Microns to avoid computational inaccuracy
54  //std::cout << a << std::endl;
55  //double det = a(0,0)*((a(1,1)*a(2,2))-(a(2,1)*a(1,2))) + a(0,1)*((a(1,2)*a(2,0))-(a(2,2)*a(1,0))) + a(0,2)*((a(1,0)*a(2,1))-(a(2,0)*a(1,1)));
56 
57  double det = determinant( a );
58 
59  //std::cout << -a(0,2)*a(1,1)*a(2,0) << " " << a(0,1)*a(1,2)*a(2,0) << " " << a(0,2)*a(1,0)*a(2,1) << " " << a(0,0)*a(1,2)*a(2,1) << " " << a(0,1)*a(1,0)*a(2,2) << " " << a(0,0)*a(1,1)*a(2,2) << std::endl;
60  //std::cout << det << std::endl;
61  Matrix3x3 inverse;
62 
63  for (short j=0;j<3;j++)
64  {
65  for (short i=0;i<3;i++)
66  {
67 
68  //Form the adjoint a_ij */
69  matrix<double> c(2,2);
70  short i1 = 0;
71  for (short ii=0;ii<3;ii++)
72  {
73  if (ii == i)
74  continue;
75  short j1 = 0;
76  for (short jj=0;jj<3;jj++)
77  {
78  if (jj == j)
79  continue;
80  c(i1,j1) = a(ii,jj);
81  j1++;
82  }
83  i1++;
84  }
85 
86  /* Calculate the determinate */
87  double tempdet = (c(0,0)*c(1,1)) - (c(1,0)*c(0,1));
88  /* Fill in the elements of the cofactor */
89  inverse(j,i) = (pow(-1.0,i+j+2.0) * tempdet)/det; //Note inline transposition
90  }
91  }
92  //std::cout << inverse <<std::endl<<std::endl;
93  //double f;
94  //std::cin >> f;
95  //inverse = inverse * (10000*10000);//Convert back to cm
96  //std::cout << inverse <<std::endl<<std::endl;
97  return inverse;
98 
99  /*// create a working copy of the input
100  matrix<double> A(input);
101  //std::cout << "A1: " << A << std::endl;
102  // perform LU-factorization
103  lu_factorize(A);
104  //std::cout << "A2: " << A << std::endl;
105 
106  // create identity matrix of "inverse"
107  matrix<double> B(3,3);
108  B.clear();
109  for (unsigned int i = 0; i < A.size1(); i++)
110  B(i,i) = 1;
111  //std::cout << "B1: " << B << std::endl;
112  // backsubstitute to get the inverse
113  lu_substitute<const matrix<double>,matrix<double> >(A,B);//const matrix<T>, matrix<T> >(A, inverse);
114  //std::cout << "B2: " << B << std::endl;
115  return B;
116  */
117 }
118 
119 Matrix2x2 InvertMatrix2(Matrix2x2 a)//const ublas::matrix<T>& input, ublas::matrix<T>& inverse)
120 {
121  Matrix2x2 inverse;
122  double det = (a(0,0)*a(1,1))-(a(0,1)*a(1,0));
123  inverse(0,0) = a(1,1)/det;
124  inverse(0,1) = -a(0,1)/det;
125  inverse(1,0) = -a(1,0)/det;
126  inverse(1,1) = a(0,0)/det;
127  return inverse;
128 
129 }
130 }
131 }
132