10 for (
unsigned int i=0;i<4;i++){
11 for (
unsigned int j=0;j<4;j++){
16 DMatrix4x4(
double c11,
double c12,
double c13,
double c14,
17 double c21,
double c22,
double c23,
double c24,
18 double c31,
double c32,
double c33,
double c34,
19 double c41,
double c42,
double c43,
double c44
69 for (
unsigned int i=0;i<4;i++){
70 for (
unsigned int j=0;j<4;j++){
83 -
mA[1][0],-
mA[1][1],-
mA[1][2],-
mA[1][3],
84 -
mA[2][0],-
mA[2][1],-
mA[2][2],-
mA[2][3],
85 -
mA[3][0],-
mA[3][1],-
mA[3][2],-
mA[3][3]
91 mA[1][0]+m2(1,0),
mA[1][1]+m2(1,1),
mA[1][2]+m2(1,2),
mA[1][3]+m2(1,3),
92 mA[2][0]+m2(2,0),
mA[2][1]+m2(2,1),
mA[2][2]+m2(2,2),
mA[2][3]+m2(2,3),
93 mA[3][0]+m2(3,0),
mA[3][1]+m2(3,1),
mA[3][2]+m2(3,2),
mA[3][3]+m2(3,3)
100 return DMatrix4x4(
mA[0][0]-m2(0,0),
mA[0][1]-m2(0,1),
mA[0][2]-m2(0,2),
mA[0][3]-m2(0,3),
101 mA[1][0]-m2(1,0),
mA[1][1]-m2(1,1),
mA[1][2]-m2(1,2),
mA[1][3]-m2(1,3),
102 mA[2][0]-m2(2,0),
mA[2][1]-m2(2,1),
mA[2][2]-m2(2,2),
mA[2][3]-m2(2,3),
103 mA[3][0]-m2(3,0),
mA[3][1]-m2(3,1),
mA[3][2]-m2(3,2),
mA[3][3]-m2(3,3)
109 return DMatrix4x4(
mA[0][0]*m2(0,0)+
mA[0][1]*m2(1,0)+
mA[0][2]*m2(2,0)+
mA[0][3]*m2(3,0),
110 mA[0][0]*m2(0,1)+
mA[0][1]*m2(1,1)+
mA[0][2]*m2(2,1)+
mA[0][3]*m2(3,1),
111 mA[0][0]*m2(0,2)+
mA[0][1]*m2(1,2)+
mA[0][2]*m2(2,2)+
mA[0][3]*m2(3,2),
112 mA[0][0]*m2(0,3)+
mA[0][1]*m2(1,3)+
mA[0][2]*m2(2,3)+
mA[0][3]*m2(3,3),
114 mA[1][0]*m2(0,0)+
mA[1][1]*m2(1,0)+
mA[1][2]*m2(2,0)+
mA[1][3]*m2(3,0),
115 mA[1][0]*m2(0,1)+
mA[1][1]*m2(1,1)+
mA[1][2]*m2(2,1)+
mA[1][3]*m2(3,1),
116 mA[1][0]*m2(0,2)+
mA[1][1]*m2(1,2)+
mA[1][2]*m2(2,2)+
mA[1][3]*m2(3,2),
117 mA[1][0]*m2(0,3)+
mA[1][1]*m2(1,3)+
mA[1][2]*m2(2,3)+
mA[1][3]*m2(3,3),
119 mA[2][0]*m2(0,0)+
mA[2][1]*m2(1,0)+
mA[2][2]*m2(2,0)+
mA[2][3]*m2(3,0),
120 mA[2][0]*m2(0,1)+
mA[2][1]*m2(1,1)+
mA[2][2]*m2(2,1)+
mA[2][3]*m2(3,1),
121 mA[2][0]*m2(0,2)+
mA[2][1]*m2(1,2)+
mA[2][2]*m2(2,2)+
mA[2][3]*m2(3,2),
122 mA[2][0]*m2(0,3)+
mA[2][1]*m2(1,3)+
mA[2][2]*m2(2,3)+
mA[2][3]*m2(3,3),
124 mA[3][0]*m2(0,0)+
mA[3][1]*m2(1,0)+
mA[3][2]*m2(2,0)+
mA[3][3]*m2(3,0),
125 mA[3][0]*m2(0,1)+
mA[3][1]*m2(1,1)+
mA[3][2]*m2(2,1)+
mA[3][3]*m2(3,1),
126 mA[3][0]*m2(0,2)+
mA[3][1]*m2(1,2)+
mA[3][2]*m2(2,2)+
mA[3][3]*m2(3,2),
127 mA[3][0]*m2(0,3)+
mA[3][1]*m2(1,3)+
mA[3][2]*m2(2,3)+
mA[3][3]*m2(3,3)
133 return DMatrix4x2(
mA[0][0]*m2(0,0)+
mA[0][1]*m2(1,0)+
mA[0][2]*m2(2,0)+
mA[0][3]*m2(3,0),
134 mA[0][0]*m2(0,1)+
mA[0][1]*m2(1,1)+
mA[0][2]*m2(2,1)+
mA[0][3]*m2(3,1),
136 mA[1][0]*m2(0,0)+
mA[1][1]*m2(1,0)+
mA[1][2]*m2(2,0)+
mA[1][3]*m2(3,0),
137 mA[1][0]*m2(0,1)+
mA[1][1]*m2(1,1)+
mA[1][2]*m2(2,1)+
mA[1][3]*m2(3,1),
139 mA[2][0]*m2(0,0)+
mA[2][1]*m2(1,0)+
mA[2][2]*m2(2,0)+
mA[2][3]*m2(3,0),
140 mA[2][0]*m2(0,1)+
mA[2][1]*m2(1,1)+
mA[2][2]*m2(2,1)+
mA[2][3]*m2(3,1),
142 mA[3][0]*m2(0,0)+
mA[3][1]*m2(1,0)+
mA[3][2]*m2(2,0)+
mA[3][3]*m2(3,0),
143 mA[3][0]*m2(0,1)+
mA[3][1]*m2(1,1)+
mA[3][2]*m2(2,1)+
mA[3][3]*m2(3,1)
152 mA[1][0]*m2(0)+
mA[1][1]*m2(1)+
mA[1][2]*m2(2)+
mA[1][3]*m2(3),
153 mA[2][0]*m2(0)+
mA[2][1]*m2(1)+
mA[2][2]*m2(2)+
mA[2][3]*m2(3),
154 mA[3][0]*m2(0)+
mA[3][1]*m2(1)+
mA[3][2]*m2(2)+
mA[3][3]*m2(3)
170 return DMatrix4x4(FF,-FF*G*Jinv,-JJ*H*Finv,JJ);
176 for (
unsigned int i=0;i<4;i++){
177 for (
unsigned int j=0;j<4;j++){
185 if (upperBound >= lowerBound)
return DMatrixDSym();
187 for (
unsigned int i=lowerBound; i <= upperBound; i++){
188 for (
unsigned int j=lowerBound; j <= upperBound; j++){
189 subMatrix(i,j) =
mA[i][j];
197 GetSub(0,1).Determinant() > 0. &&
GetSub(0,2).Determinant() > 0. &&
198 GetSub(0,3).Determinant() > 0.)
204 cout <<
"DMatrix4x4:" <<endl;
205 cout <<
" | 0 | 1 | 2 | 3 |" <<endl;
206 cout <<
"------------------------------------------------------" <<endl;
208 for (
unsigned int i=0;i<4;i++){
209 cout <<
" " << i <<
" |";
210 for (
unsigned int j=0;j<4;j++){
211 cout <<setw(11)<<setprecision(4)<<
mA[i][j] <<
" ";
230 for (
unsigned int i=0;i<4;i++){
231 mA[i].v[0]=_mm_setzero_pd();
232 mA[i].v[1]=_mm_setzero_pd();
235 DMatrix4x4(
double c11,
double c12,
double c13,
double c14,
236 double c21,
double c22,
double c23,
double c24,
237 double c31,
double c32,
double c33,
double c34,
238 double c41,
double c42,
double c43,
double c44)
241 mA[0].v[0]=_mm_setr_pd(c11,c21);
242 mA[0].v[1]=_mm_setr_pd(c31,c41);
243 mA[1].v[0]=_mm_setr_pd(c12,c22);
244 mA[1].v[1]=_mm_setr_pd(c32,c42);
245 mA[2].v[0]=_mm_setr_pd(c13,c23);
246 mA[2].v[1]=_mm_setr_pd(c33,c43);
247 mA[3].v[0]=_mm_setr_pd(c14,c24);
248 mA[3].v[1]=_mm_setr_pd(c34,c44);
251 DMatrix4x4(__m128d m11, __m128d m12, __m128d m13, __m128d m14,
252 __m128d m21, __m128d m22, __m128d m23, __m128d m24)
268 mA[0].v[0]=m1.GetV(0);
269 mA[1].v[0]=m1.GetV(1);
270 mA[0].v[1]=m3.GetV(0);
271 mA[1].v[1]=m3.GetV(1);
272 mA[2].v[0]=m2.GetV(0);
273 mA[3].v[0]=m2.GetV(1);
274 mA[2].v[1]=m4.GetV(0);
275 mA[3].v[1]=m4.GetV(1);
280 mA[0].v[0]=dm.
mA[0].v[0];
281 mA[1].v[0]=dm.
mA[1].v[0];
282 mA[2].v[0]=dm.
mA[2].v[0];
283 mA[3].v[0]=dm.
mA[3].v[0];
284 mA[0].v[1]=dm.
mA[0].v[1];
285 mA[1].v[1]=dm.
mA[1].v[1];
286 mA[2].v[1]=dm.
mA[2].v[1];
287 mA[3].v[1]=dm.
mA[3].v[1];
291 __m128d GetV(
int pair,
int col)
const{
292 return mA[col].v[pair];
294 void SetV(
int pair,
int col, __m128d v){
299 return mA[col].d[row];
302 return mA[col].d[row];
308 zero=_mm_setzero_pd();
310 _mm_sub_pd(zero,GetV(0,1)),
311 _mm_sub_pd(zero,GetV(0,2)),
312 _mm_sub_pd(zero,GetV(0,3)),
313 _mm_sub_pd(zero,GetV(1,0)),
314 _mm_sub_pd(zero,GetV(1,1)),
315 _mm_sub_pd(zero,GetV(1,2)),
316 _mm_sub_pd(zero,GetV(1,3)));
320 return DMatrix4x4(_mm_sub_pd(GetV(0,0),m2.GetV(0,0)),
321 _mm_sub_pd(GetV(0,1),m2.GetV(0,1)),
322 _mm_sub_pd(GetV(0,2),m2.GetV(0,2)),
323 _mm_sub_pd(GetV(0,3),m2.GetV(0,3)),
324 _mm_sub_pd(GetV(1,0),m2.GetV(1,0)),
325 _mm_sub_pd(GetV(1,1),m2.GetV(1,1)),
326 _mm_sub_pd(GetV(1,2),m2.GetV(1,2)),
327 _mm_sub_pd(GetV(1,3),m2.GetV(1,3)));
331 return DMatrix4x4(_mm_add_pd(GetV(0,0),m2.GetV(0,0)),
332 _mm_add_pd(GetV(0,1),m2.GetV(0,1)),
333 _mm_add_pd(GetV(0,2),m2.GetV(0,2)),
334 _mm_add_pd(GetV(0,3),m2.GetV(0,3)),
335 _mm_add_pd(GetV(1,0),m2.GetV(1,0)),
336 _mm_add_pd(GetV(1,1),m2.GetV(1,1)),
337 _mm_add_pd(GetV(1,2),m2.GetV(1,2)),
338 _mm_add_pd(GetV(1,3),m2.GetV(1,3)));
352 m11=_mm_set1_pd(m2(0,0));
353 m12=_mm_set1_pd(m2(0,1));
354 m21=_mm_set1_pd(m2(1,0));
355 m22=_mm_set1_pd(m2(1,1));
356 m31=_mm_set1_pd(m2(2,0));
357 m32=_mm_set1_pd(m2(2,1));
358 m41=_mm_set1_pd(m2(3,0));
359 m42=_mm_set1_pd(m2(3,1));
361 DMatrix4x2(_mm_add_pd(_mm_mul_pd(GetV(0,0),m11),
362 _mm_add_pd(_mm_mul_pd(GetV(0,1),m21),
363 _mm_add_pd(_mm_mul_pd(GetV(0,2),m31),
364 _mm_mul_pd(GetV(0,3),m41)))),
365 _mm_add_pd(_mm_mul_pd(GetV(0,0),m12),
366 _mm_add_pd(_mm_mul_pd(GetV(0,1),m22),
367 _mm_add_pd(_mm_mul_pd(GetV(0,2),m32),
368 _mm_mul_pd(GetV(0,3),m42)))),
369 _mm_add_pd(_mm_mul_pd(GetV(1,0),m11),
370 _mm_add_pd(_mm_mul_pd(GetV(1,1),m21),
371 _mm_add_pd(_mm_mul_pd(GetV(1,2),m31),
372 _mm_mul_pd(GetV(1,3),m41)))),
373 _mm_add_pd(_mm_mul_pd(GetV(1,0),m12),
374 _mm_add_pd(_mm_mul_pd(GetV(1,1),m22),
375 _mm_add_pd(_mm_mul_pd(GetV(1,2),m32),
376 _mm_mul_pd(GetV(1,3),m42)))));
382 for (
unsigned int i=0;i<4;i++){
383 mA[i].v[0]=m1.GetV(0,i);
384 mA[i].v[1]=m1.GetV(1,i);
403 #define SWAP(i,j,k,m) _mm_setr_pd(mA[(j)].d[(i)],mA[(m)].d[(k)])
405 return DMatrix4x4(SWAP(0,0,0,1),SWAP(1,0,1,1),SWAP(2,0,2,1),SWAP(3,0,3,1),
406 SWAP(0,2,0,3),SWAP(1,2,1,3),SWAP(2,2,2,3),SWAP(3,2,3,3));
417 a1=_mm_set1_pd(m2(0));
418 a2=_mm_set1_pd(m2(1));
419 a3=_mm_set1_pd(m2(2));
420 a4=_mm_set1_pd(m2(3));
422 DMatrix4x1(_mm_add_pd(_mm_mul_pd(GetV(0,0),a1),
423 _mm_add_pd(_mm_mul_pd(GetV(0,1),a2),
424 _mm_add_pd(_mm_mul_pd(GetV(0,2),a3),
425 _mm_mul_pd(GetV(0,3),a4)))),
426 _mm_add_pd(_mm_mul_pd(GetV(1,0),a1),
427 _mm_add_pd(_mm_mul_pd(GetV(1,1),a2),
428 _mm_add_pd(_mm_mul_pd(GetV(1,2),a3),
429 _mm_mul_pd(GetV(1,3),a4)))));
438 struct dvec1 &
temp=p[0];
439 for (
unsigned int i=0;i<4;i++){
440 for (
unsigned int j=0;j<4;j++){
441 temp.v[i][j]=_mm_set1_pd(m2(i,j));
445 #define MUL(i,j,k) _mm_mul_pd(GetV((i),(j)),temp.v[(j)][(k)])
448 _mm_add_pd(MUL(0,1,0),
449 _mm_add_pd(MUL(0,2,0),
451 _mm_add_pd(MUL(0,0,1),
452 _mm_add_pd(MUL(0,1,1),
453 _mm_add_pd(MUL(0,2,1),
455 _mm_add_pd(MUL(0,0,2),
456 _mm_add_pd(MUL(0,1,2),
457 _mm_add_pd(MUL(0,2,2),
459 _mm_add_pd(MUL(0,0,3),
460 _mm_add_pd(MUL(0,1,3),
461 _mm_add_pd(MUL(0,2,3),
463 _mm_add_pd(MUL(1,0,0),
464 _mm_add_pd(MUL(1,1,0),
465 _mm_add_pd(MUL(1,2,0),
467 _mm_add_pd(MUL(1,0,1),
468 _mm_add_pd(MUL(1,1,1),
469 _mm_add_pd(MUL(1,2,1),
471 _mm_add_pd(MUL(1,0,2),
472 _mm_add_pd(MUL(1,1,2),
473 _mm_add_pd(MUL(1,2,2),
475 _mm_add_pd(MUL(1,0,3),
476 _mm_add_pd(MUL(1,1,3),
477 _mm_add_pd(MUL(1,2,3),
483 cout <<
"DMatrix4x4:" <<endl;
484 cout <<
" | 0 | 1 | 2 | 3 |" <<endl;
485 cout <<
"----------------------------------------------------------" <<endl;
487 for (
unsigned int i=0;i<4;i++){
488 for (
unsigned int j=0;j<4;j++){
489 cout <<
mA[j].d[i] <<
" ";
DMatrix4x4(const DMatrix2x2 &m1, const DMatrix2x2 &m2, const DMatrix2x2 &m3, const DMatrix2x2 &m4)
#define ALIGNED_16_BLOCK_PTR(TYPE, NUM, PTR)
DMatrix4x4 operator-(const DMatrix4x4 &m2)
#define ALIGNED_16_BLOCK_WITH_PTR(TYPE, NUM, PTR)
DMatrix4x4(double c11, double c12, double c13, double c14, double c21, double c22, double c23, double c24, double c31, double c32, double c33, double c34, double c41, double c42, double c43, double c44)
double & operator()(int row, int col)
DMatrix4x1 operator*(const DMatrix4x1 &m2)
DMatrix4x4 operator+(const DMatrix4x4 &m2)
DMatrix4x2 operator*(const DMatrix4x2 &m2)
DMatrix4x4 & operator=(const DMatrix4x4 &m1)
DMatrix4x4 operator*(const DMatrix4x4 &m2)
#define ALIGNED_16_BLOCK(TYPE, NUM, PTR)
DMatrixDSym GetSub(unsigned int lowerBound, unsigned int upperBound)
for(auto locVertexInfo:dStepVertexInfos)