10 for (
unsigned int i=0;i<3;i++){
11 for (
unsigned int j=0;j<3;j++){
17 double c21,
double c22,
double c23,
18 double c31,
double c32,
double c33){
41 -
mA[1][0],-
mA[1][1],-
mA[1][2],
42 -
mA[2][0],-
mA[2][1],-
mA[2][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)
55 mA[0][0]*m2(0,1)+
mA[0][1]*m2(1,1)+
mA[0][2]*m2(2,1),
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),
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)
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)
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),
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),
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)
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);
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);
125 cout <<
"DMatrix3x3:" <<endl;
126 cout <<
" | 0 | 1 | 2 |" <<endl;
127 cout <<
"----------------------------------------------" <<endl;
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] <<
" ";
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();
158 DMatrix3x3(__m128d m11, __m128d m12, __m128d m13,
159 __m128d m21, __m128d m22, __m128d m23)
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];
181 __m128d GetV(
int pair,
int col)
const{
182 return mA[col].v[pair];
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];
197 return mA[col].d[row];
200 return mA[col].d[row];
207 zero=_mm_setzero_pd();
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)));
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)));
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));
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))));
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];
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));
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.)));
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];
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));
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.)));
306 cout <<
"DMatrix3x3:" <<endl;
307 cout <<
" | 0 | 1 | 2 |" <<endl;
308 cout <<
"----------------------------------------------" <<endl;
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] <<
" ";
DMatrix3x3 Invert() const
#define ALIGNED_16_BLOCK_PTR(TYPE, NUM, PTR)
DMatrix3x2 operator*(const DMatrix3x2 &m2)
double & operator()(int row, int col)
DMatrix3x3 operator*(const DMatrix3x3 &m2)
#define ALIGNED_16_BLOCK_WITH_PTR(TYPE, NUM, PTR)
DMatrix3x1 operator*(const DMatrix3x1 &m2)
DMatrix3x3(double c11, double c12, double c13, double c21, double c22, double c23, double c31, double c32, double c33)
DMatrix3x3 operator-(const DMatrix3x3 &m2)
#define ALIGNED_16_BLOCK(TYPE, NUM, PTR)