Hall-D Software  alpha
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
DMatrix3x3.h
Go to the documentation of this file.
1 #include <align_16.h>
2 
3 #ifndef USE_SSE2
4 
5 // Matrix class without SIMD instructions
6 
7 class DMatrix3x3{
8  public:
9  DMatrix3x3(){
10  for (unsigned int i=0;i<3;i++){
11  for (unsigned int j=0;j<3;j++){
12  mA[i][j]=0.;
13  }
14  }
15  }
16  DMatrix3x3(double c11, double c12, double c13,
17  double c21, double c22, double c23,
18  double c31, double c32, double c33){
19  mA[0][0]=c11;
20  mA[0][1]=c12;
21  mA[0][2]=c13;
22  mA[1][0]=c21;
23  mA[1][1]=c22;
24  mA[1][2]=c23;
25  mA[2][0]=c31;
26  mA[2][1]=c32;
27  mA[2][2]=c33;
28 
29  }
30  ~DMatrix3x3(){};
31 
32  double &operator() (int row, int col){
33  return mA[row][col];
34  }
35  double operator() (int row, int col) const{
36  return mA[row][col];
37  }
38  // unary minus
39  DMatrix3x3 operator-(){
40  return DMatrix3x3(-mA[0][0],-mA[0][1],-mA[0][2],
41  -mA[1][0],-mA[1][1],-mA[1][2],
42  -mA[2][0],-mA[2][1],-mA[2][2]);
43  }
44  // Matrix subtraction
45  DMatrix3x3 operator-(const DMatrix3x3 &m2){
46  return DMatrix3x3(mA[0][0]-m2(0,0),mA[0][1]-m2(0,1),mA[0][2]-m2(0,2),
47  mA[1][0]-m2(1,0),mA[1][1]-m2(1,1),mA[1][2]-m2(1,2),
48  mA[2][0]-m2(2,0),mA[2][1]-m2(2,1),mA[2][2]-m2(2,2)
49  );
50  }
51 
52  // Matrix multiplication: (3x3) x (3x2)
53  DMatrix3x2 operator*(const DMatrix3x2 &m2){
54  return DMatrix3x2(mA[0][0]*m2(0,0)+mA[0][1]*m2(1,0)+mA[0][2]*m2(2,0),
55  mA[0][0]*m2(0,1)+mA[0][1]*m2(1,1)+mA[0][2]*m2(2,1),
56 
57  mA[1][0]*m2(0,0)+mA[1][1]*m2(1,0)+mA[1][2]*m2(2,0),
58  mA[1][0]*m2(0,1)+mA[1][1]*m2(1,1)+mA[1][2]*m2(2,1),
59 
60  mA[2][0]*m2(0,0)+mA[2][1]*m2(1,0)+mA[2][2]*m2(2,0),
61  mA[2][0]*m2(0,1)+mA[2][1]*m2(1,1)+mA[2][2]*m2(2,1)
62  );
63  }
64 
65  // Matrix multiplication: (3x3) x (3x1)
66  DMatrix3x1 operator*(const DMatrix3x1 &m2){
67  return DMatrix3x1(mA[0][0]*m2(0)+mA[0][1]*m2(1)+mA[0][2]*m2(2),
68  mA[1][0]*m2(0)+mA[1][1]*m2(1)+mA[1][2]*m2(2),
69  mA[2][0]*m2(0)+mA[2][1]*m2(1)+mA[2][2]*m2(2)
70  );
71  }
72 
73  // Matrix multiplication: (3x3) x (3x3)
74  DMatrix3x3 operator*(const DMatrix3x3 &m2){
75  return DMatrix3x3(mA[0][0]*m2(0,0)+mA[0][1]*m2(1,0)+mA[0][2]*m2(2,0),
76  mA[0][0]*m2(0,1)+mA[0][1]*m2(1,1)+mA[0][2]*m2(2,1),
77  mA[0][0]*m2(0,2)+mA[0][1]*m2(1,2)+mA[0][2]*m2(2,2),
78 
79  mA[1][0]*m2(0,0)+mA[1][1]*m2(1,0)+mA[1][2]*m2(2,0),
80  mA[1][0]*m2(0,1)+mA[1][1]*m2(1,1)+mA[1][2]*m2(2,1),
81  mA[1][0]*m2(0,2)+mA[1][1]*m2(1,2)+mA[1][2]*m2(2,2),
82 
83  mA[2][0]*m2(0,0)+mA[2][1]*m2(1,0)+mA[2][2]*m2(2,0),
84  mA[2][0]*m2(0,1)+mA[2][1]*m2(1,1)+mA[2][2]*m2(2,1),
85  mA[2][0]*m2(0,2)+mA[2][1]*m2(1,2)+mA[2][2]*m2(2,2)
86  );
87  }
88 
89  // Matrix inversion
90  DMatrix3x3 Invert() const{
91  double b11=mA[1][1]*mA[2][2]-mA[1][2]*mA[2][1];
92  double b12=mA[0][2]*mA[2][1]-mA[0][1]*mA[2][2];
93  double b13=mA[0][1]*mA[1][2]-mA[0][2]*mA[1][1];
94  double b21=mA[1][2]*mA[2][0]-mA[1][0]*mA[2][2];
95  double b22=mA[0][0]*mA[2][2]-mA[0][2]*mA[2][0];
96  double b23=mA[0][2]*mA[1][0]-mA[0][0]*mA[1][2];
97  double b31=mA[1][0]*mA[2][1]-mA[1][1]*mA[2][0];
98  double b32=mA[0][1]*mA[2][0]-mA[0][0]*mA[2][1];
99  double b33=mA[0][0]*mA[1][1]-mA[0][1]*mA[1][0];
100  double one_over_det=1./(mA[0][0]*b11+mA[0][1]*b21+mA[0][2]*b31);
101  return DMatrix3x3(one_over_det*b11,one_over_det*b12,one_over_det*b13,
102  one_over_det*b21,one_over_det*b22,one_over_det*b23,
103  one_over_det*b31,one_over_det*b32,one_over_det*b33);
104 
105 
106  }
107 
108 
109  // Matrix inversion for a symmetric matrix
110  DMatrix3x3 InvertSym(){
111  double b11=mA[1][1]*mA[2][2]-mA[1][2]*mA[2][1];
112  double b21=mA[1][2]*mA[2][0]-mA[1][0]*mA[2][2];
113  double b22=mA[0][0]*mA[2][2]-mA[0][2]*mA[2][0];
114  double b31=mA[1][0]*mA[2][1]-mA[1][1]*mA[2][0];
115  double b32=mA[0][1]*mA[2][0]-mA[0][0]*mA[2][1];
116  double b33=mA[0][0]*mA[1][1]-mA[0][1]*mA[1][0];
117  double one_over_det=1./(mA[0][0]*b11+mA[0][1]*b21+mA[0][2]*b31);
118  return DMatrix3x3(one_over_det*b11,one_over_det*b21,one_over_det*b31,
119  one_over_det*b21,one_over_det*b22,one_over_det*b32,
120  one_over_det*b31,one_over_det*b32,one_over_det*b33);
121 
122  }
123 
124  void Print(){
125  cout << "DMatrix3x3:" <<endl;
126  cout << " | 0 | 1 | 2 |" <<endl;
127  cout << "----------------------------------------------" <<endl;
128 
129  for (unsigned int i=0;i<3;i++){
130  cout << " " << i <<" |";
131  for (unsigned int j=0;j<3;j++){
132  cout <<setw(11)<<setprecision(4)<< mA[i][j] <<" ";
133  }
134  cout << endl;
135  }
136  }
137 
138 
139 private:
140  double mA[3][3];
141 
142 };
143 
144 #else
145 
146 // Matrix class with SIMD instructions
147 
148 class DMatrix3x3{
149  public:
150  DMatrix3x3()
151  : mA( ALIGNED_16_BLOCK_PTR(union dvec, 3, mA) )
152  {
153  for (unsigned int i=0;i<3;i++){
154  mA[i].v[0]=_mm_setzero_pd();
155  mA[i].v[1]=_mm_setzero_pd();
156  }
157  }
158  DMatrix3x3(__m128d m11, __m128d m12, __m128d m13,
159  __m128d m21, __m128d m22, __m128d m23)
160  : mA( ALIGNED_16_BLOCK_PTR(union dvec, 3, mA) )
161  {
162  mA[0].v[0]=m11;
163  mA[1].v[0]=m12;
164  mA[2].v[0]=m13;
165  mA[0].v[1]=m21;
166  mA[1].v[1]=m22;
167  mA[2].v[1]=m23;
168  }
169  DMatrix3x3(const DMatrix3x3& dm)
170  : mA( ALIGNED_16_BLOCK_PTR(union dvec, 3, mA) )
171  {
172  mA[0].v[0]=dm.mA[0].v[0];
173  mA[1].v[0]=dm.mA[1].v[0];
174  mA[2].v[0]=dm.mA[2].v[0];
175  mA[0].v[1]=dm.mA[0].v[1];
176  mA[1].v[1]=dm.mA[1].v[1];
177  mA[2].v[1]=dm.mA[2].v[1];
178  }
179  ~DMatrix3x3(){};
180 
181  __m128d GetV(int pair, int col) const{
182  return mA[col].v[pair];
183  }
184 
185  // Assignment
186  DMatrix3x3& operator=(const DMatrix3x3& dm){
187  mA[0].v[0]=dm.mA[0].v[0];
188  mA[1].v[0]=dm.mA[1].v[0];
189  mA[2].v[0]=dm.mA[2].v[0];
190  mA[0].v[1]=dm.mA[0].v[1];
191  mA[1].v[1]=dm.mA[1].v[1];
192  mA[2].v[1]=dm.mA[2].v[1];
193  return *this;
194  }
195 
196  double &operator() (int row, int col){
197  return mA[col].d[row];
198  }
199  double operator() (int row, int col) const{
200  return mA[col].d[row];
201  }
202 
203  // unary minus
204  DMatrix3x3 operator-(){
205  ALIGNED_16_BLOCK_WITH_PTR(__m128d, 1, p)
206  __m128d &zero=p[0];
207  zero=_mm_setzero_pd();
208  return DMatrix3x3(_mm_sub_pd(zero,GetV(0,0)),
209  _mm_sub_pd(zero,GetV(0,1)),
210  _mm_sub_pd(zero,GetV(0,2)),
211  _mm_sub_pd(zero,GetV(1,0)),
212  _mm_sub_pd(zero,GetV(1,1)),
213  _mm_sub_pd(zero,GetV(1,2)));
214  }
215 
216  // Matrix subtraction
217  DMatrix3x3 operator-(const DMatrix3x3 &m2){
218  return DMatrix3x3(_mm_sub_pd(GetV(0,0),m2.GetV(0,0)),
219  _mm_sub_pd(GetV(0,1),m2.GetV(0,1)),
220  _mm_sub_pd(GetV(0,2),m2.GetV(0,2)),
221  _mm_sub_pd(GetV(1,0),m2.GetV(1,0)),
222  _mm_sub_pd(GetV(1,1),m2.GetV(1,1)),
223  _mm_sub_pd(GetV(1,2),m2.GetV(1,2)));
224  }
225 
226 
227 
228 
229  // Matrix multiplication: (3x3) x (3x2)
230  DMatrix3x2 operator*(const DMatrix3x2 &m2){
231  ALIGNED_16_BLOCK_WITH_PTR(__m128d, 6, p)
232  __m128d &m11=p[0];
233  __m128d &m12=p[1];
234  __m128d &m21=p[2];
235  __m128d &m22=p[3];
236  __m128d &m31=p[4];
237  __m128d &m32=p[5];
238  m11=_mm_set1_pd(m2(0,0));
239  m12=_mm_set1_pd(m2(0,1));
240  m21=_mm_set1_pd(m2(1,0));
241  m22=_mm_set1_pd(m2(1,1));
242  m31=_mm_set1_pd(m2(2,0));
243  m32=_mm_set1_pd(m2(2,1));
244  return
245  DMatrix3x2(_mm_add_pd(_mm_mul_pd(GetV(0,0),m11),
246  _mm_add_pd(_mm_mul_pd(GetV(0,1),m21),
247  _mm_mul_pd(GetV(0,2),m31))),
248  _mm_add_pd(_mm_mul_pd(GetV(0,0),m12),
249  _mm_add_pd(_mm_mul_pd(GetV(0,1),m22),
250  _mm_mul_pd(GetV(0,2),m32))),
251  _mm_add_pd(_mm_mul_pd(GetV(1,0),m11),
252  _mm_add_pd(_mm_mul_pd(GetV(1,1),m21),
253  _mm_mul_pd(GetV(1,2),m31))),
254  _mm_add_pd(_mm_mul_pd(GetV(1,0),m12),
255  _mm_add_pd(_mm_mul_pd(GetV(1,1),m22),
256  _mm_mul_pd(GetV(1,2),m32))));
257  }
258 
259 
260  // Matrix inversion
261  DMatrix3x3 Invert() const{
262  double b11=mA[1].d[1]*mA[2].d[2]-mA[1].d[2]*mA[2].d[1];
263  double b12=mA[2].d[0]*mA[1].d[2]-mA[1].d[0]*mA[2].d[2];
264  double b13=mA[1].d[0]*mA[2].d[1]-mA[1].d[1]*mA[2].d[0];
265  double b21=mA[2].d[1]*mA[0].d[2]-mA[0].d[1]*mA[2].d[2];
266  double b22=mA[0].d[0]*mA[2].d[2]-mA[0].d[2]*mA[2].d[0];
267  double b23=mA[2].d[0]*mA[0].d[1]-mA[0].d[0]*mA[2].d[1];
268  double b31=mA[0].d[1]*mA[1].d[2]-mA[1].d[1]*mA[0].d[2];
269  double b32=mA[1].d[0]*mA[0].d[2]-mA[0].d[0]*mA[1].d[2];
270  double b33=mA[0].d[0]*mA[1].d[1]-mA[0].d[1]*mA[1].d[0];
271  ALIGNED_16_BLOCK_WITH_PTR(__m128d, 1, p)
272  __m128d &one_over_detA=p[0];
273  one_over_detA=_mm_set1_pd(1./(mA[0].d[0]*b11+mA[1].d[0]*b21+mA[2].d[0]*b31));
274  return DMatrix3x3(_mm_mul_pd(one_over_detA,_mm_setr_pd(b11,b21)),
275  _mm_mul_pd(one_over_detA,_mm_setr_pd(b12,b22)),
276  _mm_mul_pd(one_over_detA,_mm_setr_pd(b13,b23)),
277  _mm_mul_pd(one_over_detA,_mm_setr_pd(b31,0.)),
278  _mm_mul_pd(one_over_detA,_mm_setr_pd(b32,0.)),
279  _mm_mul_pd(one_over_detA,_mm_setr_pd(b33,0.)));
280 
281  }
282  // Matrix inversion for a symmetric matrix
283  DMatrix3x3 InvertSym(){
284  double b11=mA[1].d[1]*mA[2].d[2]-mA[1].d[2]*mA[2].d[1];
285  double b21=mA[2].d[1]*mA[0].d[2]-mA[0].d[1]*mA[2].d[2];
286  double b22=mA[0].d[0]*mA[2].d[2]-mA[0].d[2]*mA[2].d[0];
287  double b31=mA[0].d[1]*mA[1].d[2]-mA[1].d[1]*mA[0].d[2];
288  double b32=mA[1].d[0]*mA[0].d[2]-mA[0].d[0]*mA[1].d[2];
289  double b33=mA[0].d[0]*mA[1].d[1]-mA[0].d[1]*mA[1].d[0];
290  ALIGNED_16_BLOCK_WITH_PTR(__m128d, 1, p)
291  __m128d &one_over_detA=p[0];
292  one_over_detA=_mm_set1_pd(1./(mA[0].d[0]*b11+mA[1].d[0]*b21+mA[2].d[0]*b31));
293  return DMatrix3x3(_mm_mul_pd(one_over_detA,_mm_setr_pd(b11,b21)),
294  _mm_mul_pd(one_over_detA,_mm_setr_pd(b21,b22)),
295  _mm_mul_pd(one_over_detA,_mm_setr_pd(b31,b32)),
296  _mm_mul_pd(one_over_detA,_mm_setr_pd(b31,0.)),
297  _mm_mul_pd(one_over_detA,_mm_setr_pd(b32,0.)),
298  _mm_mul_pd(one_over_detA,_mm_setr_pd(b33,0.)));
299 
300  }
301 
302 
303 
304 
305  void Print(){
306  cout << "DMatrix3x3:" <<endl;
307  cout << " | 0 | 1 | 2 |" <<endl;
308  cout << "----------------------------------------------" <<endl;
309 
310  for (unsigned int i=0;i<3;i++){
311  cout << " " << i <<" |";
312  for (unsigned int j=0;j<3;j++){
313  cout <<setw(11)<<setprecision(4)<< mA[j].d[i] <<" ";
314  }
315  cout << endl;
316  }
317  }
318 
319 
320 
321  private:
322  union dvec{
323  __m128d v[2];
324  double d[4];
325  };
326  ALIGNED_16_BLOCK(union dvec, 3, mA)
327 };
328 #endif
DMatrix3x3::Invert
DMatrix3x3 Invert() const
Definition: DMatrix3x3.h:90
ALIGNED_16_BLOCK_PTR
#define ALIGNED_16_BLOCK_PTR(TYPE, NUM, PTR)
Definition: align_16.h:24
DMatrix3x3::operator*
DMatrix3x2 operator*(const DMatrix3x2 &m2)
Definition: DMatrix3x3.h:53
DMatrix3x3::operator()
double & operator()(int row, int col)
Definition: DMatrix3x3.h:32
DMatrix3x3::operator-
DMatrix3x3 operator-()
Definition: DMatrix3x3.h:39
DMatrix3x3::operator*
DMatrix3x3 operator*(const DMatrix3x3 &m2)
Definition: DMatrix3x3.h:74
ALIGNED_16_BLOCK_WITH_PTR
#define ALIGNED_16_BLOCK_WITH_PTR(TYPE, NUM, PTR)
Definition: align_16.h:27
DMatrix3x3::operator*
DMatrix3x1 operator*(const DMatrix3x1 &m2)
Definition: DMatrix3x3.h:66
DMatrix3x3::Print
void Print()
Definition: DMatrix3x3.h:124
DMatrix3x3::DMatrix3x3
DMatrix3x3(double c11, double c12, double c13, double c21, double c22, double c23, double c31, double c32, double c33)
Definition: DMatrix3x3.h:16
DMatrix3x3::InvertSym
DMatrix3x3 InvertSym()
Definition: DMatrix3x3.h:110
align_16.h
DMatrix3x3::DMatrix3x3
DMatrix3x3()
Definition: DMatrix3x3.h:9
return
return
Definition: HistMacro_BCALReconstruction_p1.C:11
DMatrix3x3::mA
double mA[3][3]
Definition: DMatrix3x3.h:140
DMatrix3x3::operator-
DMatrix3x3 operator-(const DMatrix3x3 &m2)
Definition: DMatrix3x3.h:45
ALIGNED_16_BLOCK
#define ALIGNED_16_BLOCK(TYPE, NUM, PTR)
Definition: align_16.h:19