11 for (
unsigned int i=0;i<5;i++)
12 for (
unsigned int j=0;j<5;j++)
17 for (
unsigned int i=0;i<5;i++){
18 for (
unsigned int j=0;j<5;j++){
32 for (
unsigned int i=0;i<2;i++){
33 for (
unsigned int j=0;j<3;j++){
37 for (
unsigned int i=0;i<3;i++){
38 for (
unsigned int j=0;j<2;j++){
57 for (
unsigned int i=0;i<5;i++){
58 for (
unsigned int j=0;j<5;j++){
68 for (
unsigned int i=0;i<5;i++){
69 for (
unsigned int j=0;j<5;j++){
79 for (
unsigned int row=0;row<5;row++){
80 for (
unsigned int col=0;col<5;col++){
81 temp(row,col)=
mA[row][col]+m2(row,col);
87 for (
unsigned int i=0;i<5;i++){
88 for (
unsigned int j=0;j<5;j++){
98 for (
unsigned int i=0;i<5;i++){
99 for (
unsigned int j=i;j<5;j++){
100 temp(i,j)=
mA[i][j]+m2(i,j);
110 for (
unsigned int i=0;i<5;i++){
111 for (
unsigned int j=i;j<5;j++){
112 temp(i,j)=
mA[i][j]-m2(i,j);
123 for (
unsigned int row=0;row<5;row++){
124 for (
unsigned int col=0;col<5;col++){
125 temp(row,col)=
mA[row][col]-m2(row,col);
131 for (
unsigned int i=0;i<5;i++){
132 for (
unsigned int j=0;j<5;j++){
140 for (
unsigned int i=0;i<5;i++){
141 for (
unsigned int j=0;j<5;j++){
151 +
mA[0][3]*m2(3)+
mA[0][4]*m2(4),
152 mA[1][0]*m2(0)+
mA[1][1]*m2(1)+
mA[1][2]*m2(2)
153 +
mA[1][3]*m2(3)+
mA[1][4]*m2(4),
154 mA[2][0]*m2(0)+
mA[2][1]*m2(1)+
mA[2][2]*m2(2)
155 +
mA[2][3]*m2(3)+
mA[2][4]*m2(4),
156 mA[3][0]*m2(0)+
mA[3][1]*m2(1)+
mA[3][2]*m2(2)
157 +
mA[3][3]*m2(3)+
mA[3][4]*m2(4),
158 mA[4][0]*m2(0)+
mA[4][1]*m2(1)+
mA[4][2]*m2(2)
159 +
mA[4][3]*m2(3)+
mA[4][4]*m2(4));
166 +
mA[0][3]*m2(3,0)+
mA[0][4]*m2(4,0),
167 mA[0][0]*m2(0,1)+
mA[0][1]*m2(1,1)+
mA[0][2]*m2(2,1)
168 +
mA[0][3]*m2(3,1)+
mA[0][4]*m2(4,1),
170 mA[1][0]*m2(0,0)+
mA[1][1]*m2(1,0)+
mA[1][2]*m2(2,0)
171 +
mA[1][3]*m2(3,0)+
mA[1][4]*m2(4,0),
172 mA[1][0]*m2(0,1)+
mA[1][1]*m2(1,1)+
mA[1][2]*m2(2,1)
173 +
mA[1][3]*m2(3,1)+
mA[1][4]*m2(4,1),
175 mA[2][0]*m2(0,0)+
mA[2][1]*m2(1,0)+
mA[2][2]*m2(2,0)
176 +
mA[2][3]*m2(3,0)+
mA[2][4]*m2(4,0),
177 mA[2][0]*m2(0,1)+
mA[2][1]*m2(1,1)+
mA[2][2]*m2(2,1)
178 +
mA[2][3]*m2(3,1)+
mA[2][4]*m2(4,1),
180 mA[3][0]*m2(0,0)+
mA[3][1]*m2(1,0)+
mA[3][2]*m2(2,0)
181 +
mA[3][3]*m2(3,0)+
mA[3][4]*m2(4,0),
182 mA[3][0]*m2(0,1)+
mA[3][1]*m2(1,1)+
mA[3][2]*m2(2,1)
183 +
mA[3][3]*m2(3,1)+
mA[3][4]*m2(4,1),
185 mA[4][0]*m2(0,0)+
mA[4][1]*m2(1,0)+
mA[4][2]*m2(2,0)
186 +
mA[4][3]*m2(3,0)+
mA[4][4]*m2(4,0),
187 mA[4][0]*m2(0,1)+
mA[4][1]*m2(1,1)+
mA[4][2]*m2(2,1)
188 +
mA[4][3]*m2(3,1)+
mA[4][4]*m2(4,1)
197 for (
unsigned int j=0;j<5;j++){
198 for (
unsigned int i=0;i<5;i++){
200 for (
unsigned int k=0;k<5;k++){
201 temp2+=
mA[j][k]*m2(k,i);
211 for (
unsigned int i=0;i<5;i++){
212 for (
unsigned int j=0;j<5;j++){
226 mA[4][2],
mA[4][3],
mA[4][4]);
241 sums[0]=
mA[0][0]*A(0,0);
242 sums[0]+=
mA[0][1]*A(0,1);
243 sums[0]+=
mA[0][2]*A(0,2);
244 sums[0]+=
mA[0][3]*A(0,3);
245 sums[0]+=
mA[0][4]*A(0,4);
247 sums[1]=
mA[1][0]*A(0,0);
248 sums[1]+=
mA[1][1]*A(0,1);
249 sums[1]+=
mA[1][2]*A(0,2);
250 sums[1]+=
mA[1][3]*A(0,3);
251 sums[1]+=
mA[1][4]*A(0,4);
253 sums[2]=
mA[2][0]*A(0,0);
254 sums[2]+=
mA[2][1]*A(0,1);
255 sums[2]+=
mA[2][2]*A(0,2);
256 sums[2]+=
mA[2][3]*A(0,3);
257 sums[2]+=
mA[2][4]*A(0,4);
259 sums[3]=
mA[3][0]*A(0,0);
260 sums[3]+=
mA[3][1]*A(0,1);
261 sums[3]+=
mA[3][2]*A(0,2);
262 sums[3]+=
mA[3][3]*A(0,3);
263 sums[3]+=
mA[3][4]*A(0,4);
265 sums[4]=
mA[4][0]*A(0,0);
266 sums[4]+=
mA[4][1]*A(0,1);
267 sums[4]+=
mA[4][2]*A(0,2);
268 sums[4]+=
mA[4][3]*A(0,3);
269 sums[4]+=
mA[4][4]*A(0,4);
271 temp(0,0)+=A(0,0)*sums[0];
272 temp(0,0)+=A(0,1)*sums[1];
273 temp(0,0)+=A(0,2)*sums[2];
274 temp(0,0)+=A(0,3)*sums[3];
275 temp(0,0)+=A(0,4)*sums[4];
277 temp(1,0)+=A(1,0)*sums[0];
278 temp(1,0)+=A(1,1)*sums[1];
279 temp(1,0)+=A(1,2)*sums[2];
280 temp(1,0)+=A(1,3)*sums[3];
281 temp(1,0)+=A(1,4)*sums[4];
283 temp(2,0)+=A(2,0)*sums[0];
284 temp(2,0)+=A(2,1)*sums[1];
285 temp(2,0)+=A(2,2)*sums[2];
286 temp(2,0)+=A(2,3)*sums[3];
287 temp(2,0)+=A(2,4)*sums[4];
289 temp(3,0)+=A(3,0)*sums[0];
290 temp(3,0)+=A(3,1)*sums[1];
291 temp(3,0)+=A(3,2)*sums[2];
292 temp(3,0)+=A(3,3)*sums[3];
293 temp(3,0)+=A(3,4)*sums[4];
295 temp(4,0)+=A(4,0)*sums[0];
296 temp(4,0)+=A(4,1)*sums[1];
297 temp(4,0)+=A(4,2)*sums[2];
298 temp(4,0)+=A(4,3)*sums[3];
299 temp(4,0)+=A(4,4)*sums[4];
307 sums[0] =
mA[0][0]*A(1,0);
308 sums[0]+=
mA[0][1]*A(1,1);
309 sums[0]+=
mA[0][2]*A(1,2);
310 sums[0]+=
mA[0][3]*A(1,3);
311 sums[0]+=
mA[0][4]*A(1,4);
313 sums[1]=
mA[1][0]*A(1,0);
314 sums[1]+=
mA[1][1]*A(1,1);
315 sums[1]+=
mA[1][2]*A(1,2);
316 sums[1]+=
mA[1][3]*A(1,3);
317 sums[1]+=
mA[1][4]*A(1,4);
319 sums[2]=
mA[2][0]*A(1,0);
320 sums[2]+=
mA[2][1]*A(1,1);
321 sums[2]+=
mA[2][2]*A(1,2);
322 sums[2]+=
mA[2][3]*A(1,3);
323 sums[2]+=
mA[2][4]*A(1,4);
325 sums[3]=
mA[3][0]*A(1,0);
326 sums[3]+=
mA[3][1]*A(1,1);
327 sums[3]+=
mA[3][2]*A(1,2);
328 sums[3]+=
mA[3][3]*A(1,3);
329 sums[3]+=
mA[3][4]*A(1,4);
331 sums[4]=
mA[4][0]*A(1,0);
332 sums[4]+=
mA[4][1]*A(1,1);
333 sums[4]+=
mA[4][2]*A(1,2);
334 sums[4]+=
mA[4][3]*A(1,3);
335 sums[4]+=
mA[4][4]*A(1,4);
337 temp(1,1)+=A(1,0)*sums[0];
338 temp(1,1)+=A(1,1)*sums[1];
339 temp(1,1)+=A(1,2)*sums[2];
340 temp(1,1)+=A(1,3)*sums[3];
341 temp(1,1)+=A(1,4)*sums[4];
343 temp(2,1)+=A(2,0)*sums[0];
344 temp(2,1)+=A(2,1)*sums[1];
345 temp(2,1)+=A(2,2)*sums[2];
346 temp(2,1)+=A(2,3)*sums[3];
347 temp(2,1)+=A(2,4)*sums[4];
349 temp(3,1)+=A(3,0)*sums[0];
350 temp(3,1)+=A(3,1)*sums[1];
351 temp(3,1)+=A(3,2)*sums[2];
352 temp(3,1)+=A(3,3)*sums[3];
353 temp(3,1)+=A(3,4)*sums[4];
355 temp(4,1)+=A(4,0)*sums[0];
356 temp(4,1)+=A(4,1)*sums[1];
357 temp(4,1)+=A(4,2)*sums[2];
358 temp(4,1)+=A(4,3)*sums[3];
359 temp(4,1)+=A(4,4)*sums[4];
366 sums[0] =
mA[0][0]*A(2,0);
367 sums[0]+=
mA[0][1]*A(2,1);
368 sums[0]+=
mA[0][2]*A(2,2);
369 sums[0]+=
mA[0][3]*A(2,3);
370 sums[0]+=
mA[0][4]*A(2,4);
372 sums[1] =
mA[1][0]*A(2,0);
373 sums[1]+=
mA[1][1]*A(2,1);
374 sums[1]+=
mA[1][2]*A(2,2);
375 sums[1]+=
mA[1][3]*A(2,3);
376 sums[1]+=
mA[1][4]*A(2,4);
378 sums[2] =
mA[2][0]*A(2,0);
379 sums[2]+=
mA[2][1]*A(2,1);
380 sums[2]+=
mA[2][2]*A(2,2);
381 sums[2]+=
mA[2][3]*A(2,3);
382 sums[2]+=
mA[2][4]*A(2,4);
384 sums[3] =
mA[3][0]*A(2,0);
385 sums[3]+=
mA[3][1]*A(2,1);
386 sums[3]+=
mA[3][2]*A(2,2);
387 sums[3]+=
mA[3][3]*A(2,3);
388 sums[3]+=
mA[3][4]*A(2,4);
390 sums[4] =
mA[4][0]*A(2,0);
391 sums[4]+=
mA[4][1]*A(2,1);
392 sums[4]+=
mA[4][2]*A(2,2);
393 sums[4]+=
mA[4][3]*A(2,3);
394 sums[4]+=
mA[4][4]*A(2,4);
396 temp(2,2)+=A(2,0)*sums[0];
397 temp(2,2)+=A(2,1)*sums[1];
398 temp(2,2)+=A(2,2)*sums[2];
399 temp(2,2)+=A(2,3)*sums[3];
400 temp(2,2)+=A(2,4)*sums[4];
402 temp(3,2)+=A(3,0)*sums[0];
403 temp(3,2)+=A(3,1)*sums[1];
404 temp(3,2)+=A(3,2)*sums[2];
405 temp(3,2)+=A(3,3)*sums[3];
406 temp(3,2)+=A(3,4)*sums[4];
408 temp(4,2)+=A(4,0)*sums[0];
409 temp(4,2)+=A(4,1)*sums[1];
410 temp(4,2)+=A(4,2)*sums[2];
411 temp(4,2)+=A(4,3)*sums[3];
412 temp(4,2)+=A(4,4)*sums[4];
418 sums[0] =
mA[0][0]*A(3,0);
419 sums[0]+=
mA[0][1]*A(3,1);
420 sums[0]+=
mA[0][2]*A(3,2);
421 sums[0]+=
mA[0][3]*A(3,3);
422 sums[0]+=
mA[0][4]*A(3,4);
424 sums[1] =
mA[1][0]*A(3,0);
425 sums[1]+=
mA[1][1]*A(3,1);
426 sums[1]+=
mA[1][2]*A(3,2);
427 sums[1]+=
mA[1][3]*A(3,3);
428 sums[1]+=
mA[1][4]*A(3,4);
430 sums[2] =
mA[2][0]*A(3,0);
431 sums[2]+=
mA[2][1]*A(3,1);
432 sums[2]+=
mA[2][2]*A(3,2);
433 sums[2]+=
mA[2][3]*A(3,3);
434 sums[2]+=
mA[2][4]*A(3,4);
436 sums[3] =
mA[3][0]*A(3,0);
437 sums[3]+=
mA[3][1]*A(3,1);
438 sums[3]+=
mA[3][2]*A(3,2);
439 sums[3]+=
mA[3][3]*A(3,3);
440 sums[3]+=
mA[3][4]*A(3,4);
442 sums[4] =
mA[4][0]*A(3,0);
443 sums[4]+=
mA[4][1]*A(3,1);
444 sums[4]+=
mA[4][2]*A(3,2);
445 sums[4]+=
mA[4][3]*A(3,3);
446 sums[4]+=
mA[4][4]*A(3,4);
448 temp(3,3)+=A(3,0)*sums[0];
449 temp(3,3)+=A(3,1)*sums[1];
450 temp(3,3)+=A(3,2)*sums[2];
451 temp(3,3)+=A(3,3)*sums[3];
452 temp(3,3)+=A(3,4)*sums[4];
454 temp(4,3)+=A(4,0)*sums[0];
455 temp(4,3)+=A(4,1)*sums[1];
456 temp(4,3)+=A(4,2)*sums[2];
457 temp(4,3)+=A(4,3)*sums[3];
458 temp(4,3)+=A(4,4)*sums[4];
463 sums[0] =
mA[0][0]*A(4,0);
464 sums[0]+=
mA[0][1]*A(4,1);
465 sums[0]+=
mA[0][2]*A(4,2);
466 sums[0]+=
mA[0][3]*A(4,3);
467 sums[0]+=
mA[0][4]*A(4,4);
469 sums[1] =
mA[1][0]*A(4,0);
470 sums[1]+=
mA[1][1]*A(4,1);
471 sums[1]+=
mA[1][2]*A(4,2);
472 sums[1]+=
mA[1][3]*A(4,3);
473 sums[1]+=
mA[1][4]*A(4,4);
475 sums[2] =
mA[2][0]*A(4,0);
476 sums[2]+=
mA[2][1]*A(4,1);
477 sums[2]+=
mA[2][2]*A(4,2);
478 sums[2]+=
mA[2][3]*A(4,3);
479 sums[2]+=
mA[2][4]*A(4,4);
481 sums[3] =
mA[3][0]*A(4,0);
482 sums[3]+=
mA[3][1]*A(4,1);
483 sums[3]+=
mA[3][2]*A(4,2);
484 sums[3]+=
mA[3][3]*A(4,3);
485 sums[3]+=
mA[3][4]*A(4,4);
487 sums[4] =
mA[4][0]*A(4,0);
488 sums[4]+=
mA[4][1]*A(4,1);
489 sums[4]+=
mA[4][2]*A(4,2);
490 sums[4]+=
mA[4][3]*A(4,3);
491 sums[4]+=
mA[4][4]*A(4,4);
493 temp(4,4)+=A(4,0)*sums[0];
494 temp(4,4)+=A(4,1)*sums[1];
495 temp(4,4)+=A(4,2)*sums[2];
496 temp(4,4)+=A(4,3)*sums[3];
497 temp(4,4)+=A(4,4)*sums[4];
510 for (
unsigned int i=0;i<5;i++){
511 for (
unsigned int k=0;k<5;k++){
512 sums[i]+=
mA[i][k]*A(0,k);
515 for (
unsigned int i=0;i<5;i++){
516 for (
unsigned int k=0;k<5;k++){
517 temp(i,0)+=A(i,k)*sums[k];
522 for (
unsigned int i=0;i<5;i++){
524 for (
unsigned int k=0;k<5;k++){
525 sums[i]+=
mA[i][k]*A(1,k);
528 for (
unsigned int i=1;i<5;i++){
529 for (
unsigned int k=0;k<5;k++){
530 temp(i,1)+=A(i,k)*sums[k];
535 for (
unsigned int i=0;i<5;i++){
537 for (
unsigned int k=0;k<5;k++){
538 sums[i]+=
mA[i][k]*A(2,k);
541 for (
unsigned int i=2;i<5;i++){
542 for (
unsigned int k=0;k<5;k++){
543 temp(i,2)+=A(i,k)*sums[k];
548 for (
unsigned int i=0;i<5;i++){
550 for (
unsigned int k=0;k<5;k++){
551 sums[i]+=
mA[i][k]*A(3,k);
554 for (
unsigned int i=3;i<5;i++){
555 for (
unsigned int k=0;k<5;k++){
556 temp(i,3)+=A(i,k)*sums[k];
561 for (
unsigned int i=0;i<5;i++){
563 for (
unsigned int k=0;k<5;k++){
564 sums[i]+=
mA[i][k]*A(4,k);
567 for (
unsigned int k=0;k<5;k++){
568 temp(4,4)+=A(4,k)*sums[k];
574 double a1=A(0),a2=A(1),a3=A(2),a4=A(3),a5=A(4);
575 return a1*(a1*
mA[0][0]+2.*a2*
mA[0][1]+2.*a3*
mA[0][2]+2.*a4*
mA[0][3]
577 +a2*(a2*
mA[1][1]+2.*a3*
mA[1][2]+2.*a4*
mA[1][3]+2.*a5*
mA[1][4])
578 +a3*(a3*mA[2][2]+2.*a4*mA[2][3]+2.*a5*mA[2][4])
579 +a4*(a4*mA[3][3]+2.*a5*mA[3][4])
585 if (upperBound >= lowerBound)
return DMatrixDSym();
587 for (
unsigned int i=lowerBound; i <= upperBound; i++){
588 for (
unsigned int j=lowerBound; j <= upperBound; j++){
589 subMatrix(i,j) =
mA[i][j];
597 GetSub(0,1).Determinant() > 0. &&
GetSub(0,2).Determinant() > 0. &&
598 GetSub(0,3).Determinant() > 0. &&
GetSub(0,4).Determinant() > 0.)
604 cout <<
"DMatrix5x5:" <<endl;
605 cout <<
" | 0 | 1 | 2 | 3 | 4 |" <<endl;
606 cout <<
"----------------------------------------------------------------------" <<endl;
608 for (
unsigned int i=0;i<5;i++){
609 cout <<
" "<< i <<
" |";
610 for (
unsigned int j=0;j<5;j++){
611 cout << setw(11)<<setprecision(6)<<
mA[i][j] <<
" ";
626 for (
unsigned int i=0;i<5;i++){
627 for (
unsigned int j=0;j<5;j++){
644 for (
unsigned int i=0;i<5;i++){
645 for (
unsigned int j=0;j<3;j++){
646 mA[i].v[j]=_mm_setzero_pd();
650 DMatrix5x5( __m128d m11, __m128d m12, __m128d m13, __m128d m14, __m128d m15,
651 __m128d m21, __m128d m22, __m128d m23, __m128d m24, __m128d m25,
652 __m128d m31, __m128d m32, __m128d m33, __m128d m34, __m128d m35)
672 DMatrix5x5( __m128d m11, __m128d m12, __m128d m13, __m128d m14, __m128d m15,
673 __m128d m23, __m128d m24, __m128d m25,
682 mA[0].v[1]=_mm_setr_pd(
mA[2].d[0],
mA[3].d[0]);
683 mA[1].v[1]=_mm_setr_pd(
mA[2].d[1],
mA[3].d[1]);
687 mA[0].v[2]=_mm_setr_pd(
mA[4].d[0],0.);
688 mA[1].v[2]=_mm_setr_pd(
mA[4].d[1],0.);
689 mA[2].v[2]=_mm_setr_pd(
mA[4].d[2],0.);
690 mA[3].v[2]=_mm_setr_pd(
mA[4].d[3],0.);
694 DMatrix5x5(
double C11,
double C12,
const double C13,
double C14,
double C15,
695 double C22,
const double C23,
double C24,
double C25,
696 double C33,
double C34,
double C35,
697 double C44,
double C45,
701 mA[0].v[0]=_mm_setr_pd(C11,C12);
702 mA[1].v[0]=_mm_setr_pd(C12,C22);
703 mA[2].v[0]=_mm_setr_pd(C13,C23);
704 mA[3].v[0]=_mm_setr_pd(C14,C24);
705 mA[4].v[0]=_mm_setr_pd(C15,C25);
706 mA[0].v[1]=_mm_setr_pd(C13,C14);
707 mA[1].v[1]=_mm_setr_pd(C23,C24);
708 mA[2].v[1]=_mm_setr_pd(C33,C34);
709 mA[3].v[1]=_mm_setr_pd(C34,C44);
710 mA[4].v[1]=_mm_setr_pd(C35,C45);
711 mA[0].v[2]=_mm_setr_pd(C15,0);
712 mA[1].v[2]=_mm_setr_pd(C25,0);
713 mA[2].v[2]=_mm_setr_pd(C35,0);
714 mA[3].v[2]=_mm_setr_pd(C45,0);
715 mA[4].v[2]=_mm_setr_pd(C55,0);
724 mA[0].v[0]=A.GetV(0);
725 mA[1].v[0]=A.GetV(1);
726 for (
unsigned int i=0;i<3;i++){
727 mA[2+i].v[0]=B.GetV(i);
728 for (
unsigned int j=0;j<2;j++){
729 mA[2+i].v[j+1]=D.GetV(j,i);
732 mA[0].v[1]=C.GetV(0,0);
733 mA[0].v[2]=C.GetV(1,0);
734 mA[1].v[1]=C.GetV(0,1);
735 mA[1].v[2]=C.GetV(1,1);
739 __m128d GetV(
int pair,
int col)
const{
740 return mA[col].v[pair];
742 void SetV(
int pair,
int col, __m128d v){
750 for (
unsigned int i=0;i<5;i++){
751 for (
unsigned int j=0;j<3;j++){
752 mA[i].v[j]=m2.GetV(j,i);
764 return(
mA[0].d[0]+
mA[1].d[1]+
mA[2].d[2]+
mA[3].d[3]+
mA[4].d[4]);
770 return mA[col].d[row];
773 return mA[col].d[row];
778 for (
unsigned int i=0;i<5;i++){
779 mA[i].v[0]=m1.GetV(0,i);
780 mA[i].v[1]=m1.GetV(1,i);
781 mA[i].v[2]=m1.GetV(2,i);
788 #define ADD(i,j) _mm_add_pd(GetV((i),(j)),m2.GetV((i),(j)))
790 return DMatrix5x5(ADD(0,0),ADD(0,1),ADD(0,2),ADD(0,3),ADD(0,4),ADD(1,0),ADD(1,1),ADD(1,2),
791 ADD(1,3),ADD(1,4),ADD(2,0),ADD(2,1),ADD(2,2),ADD(2,3),ADD(2,4));
794 for (
unsigned int i=0;i<5;i++){
795 for (
unsigned int j=0;j<3;j++){
803 return DMatrix5x5(ADD(0,0),ADD(0,1),ADD(0,2),ADD(0,3),ADD(0,4),ADD(1,2),
804 ADD(1,3),ADD(1,4),ADD(2,4));
809 #define SUB(i,j) _mm_sub_pd(GetV((i),(j)),m2.GetV((i),(j)))
811 return DMatrix5x5(SUB(0,0),SUB(0,1),SUB(0,2),SUB(0,3),SUB(0,4),SUB(1,0),SUB(1,1),SUB(1,2),
812 SUB(1,3),SUB(1,4),SUB(2,0),SUB(2,1),SUB(2,2),SUB(2,3),SUB(2,4));
816 return DMatrix5x5(SUB(0,0),SUB(0,1),SUB(0,2),SUB(0,3),SUB(0,4),SUB(1,2),
817 SUB(1,3),SUB(1,4),SUB(2,4));
828 a1=_mm_set1_pd(m2(0));
829 a2=_mm_set1_pd(m2(1));
830 a3=_mm_set1_pd(m2(2));
831 a4=_mm_set1_pd(m2(3));
832 a5=_mm_set1_pd(m2(4));
834 DMatrix5x1(_mm_add_pd(_mm_mul_pd(GetV(0,0),a1),
835 _mm_add_pd(_mm_mul_pd(GetV(0,1),a2),
836 _mm_add_pd(_mm_mul_pd(GetV(0,2),a3),
837 _mm_add_pd(_mm_mul_pd(GetV(0,3),a4),
838 _mm_mul_pd(GetV(0,4),a5))))),
839 _mm_add_pd(_mm_mul_pd(GetV(1,0),a1),
840 _mm_add_pd(_mm_mul_pd(GetV(1,1),a2),
841 _mm_add_pd(_mm_mul_pd(GetV(1,2),a3),
842 _mm_add_pd(_mm_mul_pd(GetV(1,3),a4),
843 _mm_mul_pd(GetV(1,4),a5))))),
844 _mm_add_pd(_mm_mul_pd(GetV(2,0),a1),
845 _mm_add_pd(_mm_mul_pd(GetV(2,1),a2),
846 _mm_add_pd(_mm_mul_pd(GetV(2,2),a3),
847 _mm_add_pd(_mm_mul_pd(GetV(2,3),a4),
848 _mm_mul_pd(GetV(2,4),a5))))));
866 m11=_mm_set1_pd(m2(0,0));
867 m12=_mm_set1_pd(m2(0,1));
868 m21=_mm_set1_pd(m2(1,0));
869 m22=_mm_set1_pd(m2(1,1));
870 m31=_mm_set1_pd(m2(2,0));
871 m32=_mm_set1_pd(m2(2,1));
872 m41=_mm_set1_pd(m2(3,0));
873 m42=_mm_set1_pd(m2(3,1));
874 m51=_mm_set1_pd(m2(4,0));
875 m52=_mm_set1_pd(m2(4,1));
877 DMatrix5x2(_mm_add_pd(_mm_mul_pd(GetV(0,0),m11),
878 _mm_add_pd(_mm_mul_pd(GetV(0,1),m21),
879 _mm_add_pd(_mm_mul_pd(GetV(0,2),m31),
880 _mm_add_pd(_mm_mul_pd(GetV(0,3),m41),
881 _mm_mul_pd(GetV(0,4),m51))))),
882 _mm_add_pd(_mm_mul_pd(GetV(0,0),m12),
883 _mm_add_pd(_mm_mul_pd(GetV(0,1),m22),
884 _mm_add_pd(_mm_mul_pd(GetV(0,2),m32),
885 _mm_add_pd(_mm_mul_pd(GetV(0,3),m42),
886 _mm_mul_pd(GetV(0,4),m52))))),
887 _mm_add_pd(_mm_mul_pd(GetV(1,0),m11),
888 _mm_add_pd(_mm_mul_pd(GetV(1,1),m21),
889 _mm_add_pd(_mm_mul_pd(GetV(1,2),m31),
890 _mm_add_pd(_mm_mul_pd(GetV(1,3),m41),
891 _mm_mul_pd(GetV(1,4),m51))))),
892 _mm_add_pd(_mm_mul_pd(GetV(1,0),m12),
893 _mm_add_pd(_mm_mul_pd(GetV(1,1),m22),
894 _mm_add_pd(_mm_mul_pd(GetV(1,2),m32),
895 _mm_add_pd(_mm_mul_pd(GetV(1,3),m42),
896 _mm_mul_pd(GetV(1,4),m52))))),
897 _mm_add_pd(_mm_mul_pd(GetV(2,0),m11),
898 _mm_add_pd(_mm_mul_pd(GetV(2,1),m21),
899 _mm_add_pd(_mm_mul_pd(GetV(2,2),m31),
900 _mm_add_pd(_mm_mul_pd(GetV(2,3),m41),
901 _mm_mul_pd(GetV(2,4),m51))))),
902 _mm_add_pd(_mm_mul_pd(GetV(2,0),m12),
903 _mm_add_pd(_mm_mul_pd(GetV(2,1),m22),
904 _mm_add_pd(_mm_mul_pd(GetV(2,2),m32),
905 _mm_add_pd(_mm_mul_pd(GetV(2,3),m42),
906 _mm_mul_pd(GetV(2,4),m52))))));
911 double a1=A(0),a2=A(1),a3=A(2),a4=A(3),a5=A(4);
912 return a1*(a1*
mA[0].d[0]+2.*a2*
mA[0].d[1]+2.*a3*
mA[0].d[2]+2.*a4*
mA[0].d[3]
914 +a2*(a2*
mA[1].d[1]+2.*a3*
mA[1].d[2]+2.*a4*
mA[1].d[3]+2.*a5*
mA[1].d[4])
915 +a3*(a3*
mA[2].d[2]+2.*a4*
mA[2].d[3]+2.*a5*
mA[2].d[4])
916 +a4*(a4*
mA[3].d[3]+2.*a5*
mA[3].d[4])
929 #define A1112 _mm_setr_pd(A(0,0),A(0,1))
930 #define A1314 _mm_setr_pd(A(0,2),A(0,3))
944 union dvec2 &temp2=
p2[0];
945 temp2.v=_mm_set1_pd(A(0,4));
948 temp.v[0]=_mm_add_pd(_mm_add_pd(_mm_hadd_pd(_mm_mul_pd(GetV(0,0),A1112),
949 _mm_mul_pd(GetV(0,1),A1112)),
950 _mm_hadd_pd(_mm_mul_pd(GetV(1,0),A1314),
951 _mm_mul_pd(GetV(1,1),A1314))),
952 _mm_mul_pd(GetV(0,4),temp2.v));
953 temp.v[1]=_mm_add_pd(_mm_add_pd(_mm_hadd_pd(_mm_mul_pd(GetV(0,2),A1112),
954 _mm_mul_pd(GetV(0,3),A1112)),
955 _mm_hadd_pd(_mm_mul_pd(GetV(1,2),A1314),
956 _mm_mul_pd(GetV(1,3),A1314))),
957 _mm_mul_pd(GetV(1,4),temp2.v));
958 temp.v[2] =_mm_set1_pd(
mA[4].d[0]*A(0,0)+
mA[4].d[1]*A(0,1)+
mA[4].d[2]*A(0,2)+
mA[4].d[3]*A(0,3)+
mA[4].d[4]*A(0,4));
963 #define A2122 _mm_setr_pd(A(1,0),A(1,1))
964 #define A2324 _mm_setr_pd(A(1,2),A(1,3))
967 temp2.v=_mm_add_pd(_mm_add_pd(_mm_hadd_pd(_mm_mul_pd(A1112,
temp.v[0]),_mm_mul_pd(A2122,
temp.v[0])),
968 _mm_hadd_pd(_mm_mul_pd(A1314,
temp.v[1]),_mm_mul_pd(A2324,
temp.v[1]))),
969 _mm_mul_pd(A.GetV(0,4),
temp.v[2]));
970 double C11=temp2.d[0];
971 double C12=temp2.d[1];
974 #define A3132 _mm_setr_pd(A(2,0),A(2,1))
975 #define A3334 _mm_setr_pd(A(2,2),A(2,3))
976 #define A4142 _mm_setr_pd(A(3,0),A(3,1))
977 #define A4344 _mm_setr_pd(A(3,2),A(3,3))
982 temp2.v=_mm_add_pd(_mm_add_pd(_mm_hadd_pd(_mm_mul_pd(A3132,
temp.v[0]),_mm_mul_pd(A4142,
temp.v[0])),
983 _mm_hadd_pd(_mm_mul_pd(A3334,
temp.v[1]),_mm_mul_pd(A4344,
temp.v[1]))),
984 _mm_mul_pd(A.GetV(1,4),
temp.v[2]));
985 double C13=temp2.d[0];
986 double C14=temp2.d[1];
990 temp2.v=_mm_set1_pd(A(1,4));
991 temp.v[0]=_mm_add_pd(_mm_add_pd(_mm_hadd_pd(_mm_mul_pd(GetV(0,0),A2122),
992 _mm_mul_pd(GetV(0,1),A2122)),
993 _mm_hadd_pd(_mm_mul_pd(GetV(1,0),A2324),
994 _mm_mul_pd(GetV(1,1),A2324))),
995 _mm_mul_pd(GetV(0,4),temp2.v));
996 temp.v[1]=_mm_add_pd(_mm_add_pd(_mm_hadd_pd(_mm_mul_pd(GetV(0,2),A2122),
997 _mm_mul_pd(GetV(0,3),A2122)),
998 _mm_hadd_pd(_mm_mul_pd(GetV(1,2),A2324),
999 _mm_mul_pd(GetV(1,3),A2324))),
1000 _mm_mul_pd(GetV(1,4),temp2.v));
1001 temp.v[2]=_mm_set1_pd(
mA[4].d[0]*A(1,0)+
mA[4].d[1]*A(1,1)+
mA[4].d[2]*A(1,2)+
mA[4].d[3]*A(1,3)+
mA[4].d[4]*A(1,4));
1004 #define A2535 _mm_setr_pd(A(1,4),A(2,4))
1005 temp2.v=_mm_add_pd(_mm_add_pd(_mm_hadd_pd(_mm_mul_pd(A2122,
temp.v[0]),_mm_mul_pd(A3132,
temp.v[0])),
1006 _mm_hadd_pd(_mm_mul_pd(A2324,
temp.v[1]),_mm_mul_pd(A3334,
temp.v[1]))),
1007 _mm_mul_pd(A2535,
temp.v[2]));
1008 double C22=temp2.d[0];
1009 double C23=temp2.d[1];
1012 #define A5152 _mm_setr_pd(A(4,0),A(4,1))
1013 #define A5354 _mm_setr_pd(A(4,2),A(4,3))
1014 #define A4555 _mm_setr_pd(A(3,4),A(4,4))
1018 temp2.v=_mm_add_pd(_mm_add_pd(_mm_hadd_pd(_mm_mul_pd(A4142,
temp.v[0]),_mm_mul_pd(A5152,
temp.v[0])),
1019 _mm_hadd_pd(_mm_mul_pd(A4344,
temp.v[1]),_mm_mul_pd(A5354,
temp.v[1]))),
1020 _mm_mul_pd(A4555,
temp.v[2]));
1021 double C24=temp2.d[0];
1022 double C25=temp2.d[1];
1026 temp2.v=_mm_set1_pd(A(2,4));
1027 temp.v[0]=_mm_add_pd(_mm_add_pd(_mm_hadd_pd(_mm_mul_pd(GetV(0,0),A3132),
1028 _mm_mul_pd(GetV(0,1),A3132)),
1029 _mm_hadd_pd(_mm_mul_pd(GetV(1,0),A3334),
1030 _mm_mul_pd(GetV(1,1),A3334))),
1031 _mm_mul_pd(GetV(0,4),temp2.v));
1032 temp.v[1]=_mm_add_pd(_mm_add_pd(_mm_hadd_pd(_mm_mul_pd(GetV(0,2),A3132),
1033 _mm_mul_pd(GetV(0,3),A3132)),
1034 _mm_hadd_pd(_mm_mul_pd(GetV(1,2),A3334),
1035 _mm_mul_pd(GetV(1,3),A3334))),
1036 _mm_mul_pd(GetV(1,4),temp2.v));
1037 temp.v[2]=_mm_set1_pd(
mA[4].d[0]*A(2,0)+
mA[4].d[1]*A(2,1)+
mA[4].d[2]*A(2,2)+
mA[4].d[3]*A(2,3)+
mA[4].d[4]*A(2,4));
1041 #define A3545 _mm_setr_pd(A(2,4),A(3,4))
1042 temp2.v=_mm_add_pd(_mm_add_pd(_mm_hadd_pd(_mm_mul_pd(A3132,
temp.v[0]),_mm_mul_pd(A4142,
temp.v[0])),
1043 _mm_hadd_pd(_mm_mul_pd(A3334,
temp.v[1]),_mm_mul_pd(A4344,
temp.v[1]))),
1044 _mm_mul_pd(A3545,
temp.v[2]));
1045 double C33=temp2.d[0];
1046 double C34=temp2.d[1];
1047 double C35=A(4,0)*
temp.d[0]+A(4,1)*
temp.d[1]+A(4,2)*
temp.d[2]+A(4,3)*
temp.d[3]+A(4,4)*
temp.d[4];
1051 temp2.v=_mm_set1_pd(A(3,4));
1052 temp.v[0]=_mm_add_pd(_mm_add_pd(_mm_hadd_pd(_mm_mul_pd(GetV(0,0),A4142),
1053 _mm_mul_pd(GetV(0,1),A4142)),
1054 _mm_hadd_pd(_mm_mul_pd(GetV(1,0),A4344),
1055 _mm_mul_pd(GetV(1,1),A4344))),
1056 _mm_mul_pd(GetV(0,4),temp2.v));
1057 temp.v[1]=_mm_add_pd(_mm_add_pd(_mm_hadd_pd(_mm_mul_pd(GetV(0,2),A4142),
1058 _mm_mul_pd(GetV(0,3),A4142)),
1059 _mm_hadd_pd(_mm_mul_pd(GetV(1,2),A4344),
1060 _mm_mul_pd(GetV(1,3),A4344))),
1061 _mm_mul_pd(GetV(1,4),temp2.v));
1062 temp.v[2]=_mm_set1_pd(
mA[4].d[0]*A(3,0)+
mA[4].d[1]*A(3,1)+
mA[4].d[2]*A(3,2)+
mA[4].d[3]*A(3,3)+
mA[4].d[4]*A(3,4));
1065 temp2.v=_mm_add_pd(_mm_add_pd(_mm_hadd_pd(_mm_mul_pd(A4142,
temp.v[0]),_mm_mul_pd(A5152,
temp.v[0])),
1066 _mm_hadd_pd(_mm_mul_pd(A4344,
temp.v[1]),_mm_mul_pd(A5354,
temp.v[1]))),
1067 _mm_mul_pd(A4555,
temp.v[2]));
1068 double C44=temp2.d[0];
1069 double C45=temp2.d[1];
1073 temp2.v=_mm_set1_pd(A(4,4));
1074 temp.v[0]=_mm_add_pd(_mm_add_pd(_mm_hadd_pd(_mm_mul_pd(GetV(0,0),A5152),
1075 _mm_mul_pd(GetV(0,1),A5152)),
1076 _mm_hadd_pd(_mm_mul_pd(GetV(1,0),A5354),
1077 _mm_mul_pd(GetV(1,1),A5354))),
1078 _mm_mul_pd(GetV(0,4),temp2.v));
1079 temp.v[1]=_mm_add_pd(_mm_add_pd(_mm_hadd_pd(_mm_mul_pd(GetV(0,2),A5152),
1080 _mm_mul_pd(GetV(0,3),A5152)),
1081 _mm_hadd_pd(_mm_mul_pd(GetV(1,2),A5354),
1082 _mm_mul_pd(GetV(1,3),A5354))),
1083 _mm_mul_pd(GetV(1,4),temp2.v));
1084 temp.v[2]=_mm_set1_pd(
mA[4].d[0]*A(4,0)+
mA[4].d[1]*A(4,1)+
mA[4].d[2]*A(4,2)+
mA[4].d[3]*A(4,3)+
mA[4].d[4]*A(4,4));
1088 #define A1555 _mm_setr_pd(A(0,4),A(4,4))
1089 temp2.v=_mm_add_pd(_mm_add_pd(_mm_hadd_pd(_mm_mul_pd(A1112,
temp.v[0]),_mm_mul_pd(A5152,
temp.v[0])),
1090 _mm_hadd_pd(_mm_mul_pd(A1314,
temp.v[1]),_mm_mul_pd(A5354,
temp.v[1]))),
1091 _mm_mul_pd(A1555,
temp.v[2]));
1092 double C15=temp2.d[0];
1093 double C55=temp2.d[1];
1095 return DMatrix5x5(C11,C12,C13,C14,C15,C22,C23,C24,C25,C33,C34,C35,C44,C45,C55);
1101 __m128d &A11A12=p[0];
1102 __m128d &A13A14=p[1];
1103 __m128d &A21A22=p[2];
1104 __m128d &A23A24=p[3];
1105 __m128d &A31A32=p[4];
1106 __m128d &A33A34=p[5];
1107 __m128d &A41A42=p[6];
1108 __m128d &A43A44=p[7];
1109 A11A12=_mm_setr_pd(
mA[0].d[0],
mA[1].d[0]);
1110 A13A14=_mm_setr_pd(
mA[2].d[0],
mA[3].d[0]);
1111 A21A22=_mm_setr_pd(
mA[0].d[1],
mA[1].d[1]);
1112 A23A24=_mm_setr_pd(
mA[2].d[1],
mA[3].d[1]);
1113 A31A32=_mm_setr_pd(
mA[0].d[2],
mA[1].d[2]);
1114 A33A34=_mm_setr_pd(
mA[2].d[2],
mA[3].d[2]);
1115 A41A42=_mm_setr_pd(
mA[0].d[3],
mA[1].d[3]);
1116 A43A44=_mm_setr_pd(
mA[2].d[3],
mA[3].d[3]);
1118 DMatrix5x5(_mm_add_pd(_mm_add_pd(_mm_hadd_pd(_mm_mul_pd(A11A12,m2.GetV(0,0)),_mm_mul_pd(A21A22,m2.GetV(0,0))),
1119 _mm_hadd_pd(_mm_mul_pd(A13A14,m2.GetV(1,0)),_mm_mul_pd(A23A24,m2.GetV(1,0)))),
1120 _mm_mul_pd(
mA[4].v[0],_mm_set1_pd(m2(4,0)))),
1121 _mm_add_pd(_mm_add_pd(_mm_hadd_pd(_mm_mul_pd(A11A12,m2.GetV(0,1)),_mm_mul_pd(A21A22,m2.GetV(0,1))),
1122 _mm_hadd_pd(_mm_mul_pd(A13A14,m2.GetV(1,1)),_mm_mul_pd(A23A24,m2.GetV(1,1)))),
1123 _mm_mul_pd(
mA[4].v[0],_mm_set1_pd(m2(4,1)))),
1124 _mm_add_pd(_mm_add_pd(_mm_hadd_pd(_mm_mul_pd(A11A12,m2.GetV(0,2)),_mm_mul_pd(A21A22,m2.GetV(0,2))),
1125 _mm_hadd_pd(_mm_mul_pd(A13A14,m2.GetV(1,2)),_mm_mul_pd(A23A24,m2.GetV(1,2)))),
1126 _mm_mul_pd(
mA[4].v[0],_mm_set1_pd(m2(4,2)))),
1127 _mm_add_pd(_mm_add_pd(_mm_hadd_pd(_mm_mul_pd(A11A12,m2.GetV(0,3)),_mm_mul_pd(A21A22,m2.GetV(0,3))),
1128 _mm_hadd_pd(_mm_mul_pd(A13A14,m2.GetV(1,3)),_mm_mul_pd(A23A24,m2.GetV(1,3)))),
1129 _mm_mul_pd(
mA[4].v[0],_mm_set1_pd(m2(4,3)))),
1130 _mm_add_pd(_mm_add_pd(_mm_hadd_pd(_mm_mul_pd(A11A12,m2.GetV(0,4)),_mm_mul_pd(A21A22,m2.GetV(0,4))),
1131 _mm_hadd_pd(_mm_mul_pd(A13A14,m2.GetV(1,4)),_mm_mul_pd(A23A24,m2.GetV(1,4)))),
1132 _mm_mul_pd(
mA[4].v[0],_mm_set1_pd(m2(4,4)))),
1134 _mm_add_pd(_mm_add_pd(_mm_hadd_pd(_mm_mul_pd(A31A32,m2.GetV(0,0)),_mm_mul_pd(A41A42,m2.GetV(0,0))),
1135 _mm_hadd_pd(_mm_mul_pd(A33A34,m2.GetV(1,0)),_mm_mul_pd(A43A44,m2.GetV(1,0)))),
1136 _mm_mul_pd(
mA[4].v[1],_mm_set1_pd(m2(4,0)))),
1137 _mm_add_pd(_mm_add_pd(_mm_hadd_pd(_mm_mul_pd(A31A32,m2.GetV(0,1)),_mm_mul_pd(A41A42,m2.GetV(0,1))),
1138 _mm_hadd_pd(_mm_mul_pd(A33A34,m2.GetV(1,1)),_mm_mul_pd(A43A44,m2.GetV(1,1)))),
1139 _mm_mul_pd(
mA[4].v[1],_mm_set1_pd(m2(4,1)))),
1140 _mm_add_pd(_mm_add_pd(_mm_hadd_pd(_mm_mul_pd(A31A32,m2.GetV(0,2)),_mm_mul_pd(A41A42,m2.GetV(0,2))),
1141 _mm_hadd_pd(_mm_mul_pd(A33A34,m2.GetV(1,2)),_mm_mul_pd(A43A44,m2.GetV(1,2)))),
1142 _mm_mul_pd(
mA[4].v[1],_mm_set1_pd(m2(4,2)))),
1143 _mm_add_pd(_mm_add_pd(_mm_hadd_pd(_mm_mul_pd(A31A32,m2.GetV(0,3)),_mm_mul_pd(A41A42,m2.GetV(0,3))),
1144 _mm_hadd_pd(_mm_mul_pd(A33A34,m2.GetV(1,3)),_mm_mul_pd(A43A44,m2.GetV(1,3)))),
1145 _mm_mul_pd(
mA[4].v[1],_mm_set1_pd(m2(4,3)))),
1146 _mm_add_pd(_mm_add_pd(_mm_hadd_pd(_mm_mul_pd(A31A32,m2.GetV(0,4)),_mm_mul_pd(A41A42,m2.GetV(0,4))),
1147 _mm_hadd_pd(_mm_mul_pd(A33A34,m2.GetV(1,4)),_mm_mul_pd(A43A44,m2.GetV(1,4)))),
1148 _mm_mul_pd(
mA[4].v[1],_mm_set1_pd(m2(4,4)))),
1150 _mm_set_sd(
mA[0].d[4]*m2(0,0)+
mA[1].d[4]*m2(1,0)+
mA[2].d[4]*m2(2,0)+
mA[3].d[4]*m2(3,0)+
mA[4].d[4]*m2(4,0)),
1151 _mm_set_sd(
mA[0].d[4]*m2(0,1)+
mA[1].d[4]*m2(1,1)+
mA[2].d[4]*m2(2,1)+
mA[3].d[4]*m2(3,1)+
mA[4].d[4]*m2(4,1)),
1152 _mm_set_sd(
mA[0].d[4]*m2(0,2)+
mA[1].d[4]*m2(1,2)+
mA[2].d[4]*m2(2,2)+
mA[3].d[4]*m2(3,2)+
mA[4].d[4]*m2(4,2)),
1153 _mm_set_sd(
mA[0].d[4]*m2(0,3)+
mA[1].d[4]*m2(1,3)+
mA[2].d[4]*m2(2,3)+
mA[3].d[4]*m2(3,3)+
mA[4].d[4]*m2(4,3)),
1154 _mm_set_sd(
mA[0].d[4]*m2(0,4)+
mA[1].d[4]*m2(1,4)+
mA[2].d[4]*m2(2,4)+
mA[3].d[4]*m2(3,4)+
mA[4].d[4]*m2(4,4))
1166 struct dvec1 &
temp=p[0];
1167 for (
unsigned int i=0;i<5;i++){
1168 for (
unsigned int j=0;j<5;j++){
1169 temp.v[i][j]=_mm_set1_pd(m2(i,j));
1173 #define MUL(i,j,k) _mm_mul_pd(GetV((i),(j)),temp.v[(j)][(k)])
1176 _mm_add_pd(MUL(0,1,0),
1177 _mm_add_pd(MUL(0,2,0),
1178 _mm_add_pd(MUL(0,3,0),
1180 _mm_add_pd(MUL(0,0,1),
1181 _mm_add_pd(MUL(0,1,1),
1182 _mm_add_pd(MUL(0,2,1),
1183 _mm_add_pd(MUL(0,3,1),
1185 _mm_add_pd(MUL(0,0,2),
1186 _mm_add_pd(MUL(0,1,2),
1187 _mm_add_pd(MUL(0,2,2),
1188 _mm_add_pd(MUL(0,3,2),
1190 _mm_add_pd(MUL(0,0,3),
1191 _mm_add_pd(MUL(0,1,3),
1192 _mm_add_pd(MUL(0,2,3),
1193 _mm_add_pd(MUL(0,3,3),
1195 _mm_add_pd(MUL(0,0,4),
1196 _mm_add_pd(MUL(0,1,4),
1197 _mm_add_pd(MUL(0,2,4),
1198 _mm_add_pd(MUL(0,3,4),
1200 _mm_add_pd(MUL(1,0,0),
1201 _mm_add_pd(MUL(1,1,0),
1202 _mm_add_pd(MUL(1,2,0),
1203 _mm_add_pd(MUL(1,3,0),
1205 _mm_add_pd(MUL(1,0,1),
1206 _mm_add_pd(MUL(1,1,1),
1207 _mm_add_pd(MUL(1,2,1),
1208 _mm_add_pd(MUL(1,3,1),
1210 _mm_add_pd(MUL(1,0,2),
1211 _mm_add_pd(MUL(1,1,2),
1212 _mm_add_pd(MUL(1,2,2),
1213 _mm_add_pd(MUL(1,3,2),
1215 _mm_add_pd(MUL(1,0,3),
1216 _mm_add_pd(MUL(1,1,3),
1217 _mm_add_pd(MUL(1,2,3),
1218 _mm_add_pd(MUL(1,3,3),
1220 _mm_add_pd(MUL(1,0,4),
1221 _mm_add_pd(MUL(1,1,4),
1222 _mm_add_pd(MUL(1,2,4),
1223 _mm_add_pd(MUL(1,3,4),
1225 _mm_add_pd(MUL(2,0,0),
1226 _mm_add_pd(MUL(2,1,0),
1227 _mm_add_pd(MUL(2,2,0),
1228 _mm_add_pd(MUL(2,3,0),
1230 _mm_add_pd(MUL(2,0,1),
1231 _mm_add_pd(MUL(2,1,1),
1232 _mm_add_pd(MUL(2,2,1),
1233 _mm_add_pd(MUL(2,3,1),
1235 _mm_add_pd(MUL(2,0,2),
1236 _mm_add_pd(MUL(2,1,2),
1237 _mm_add_pd(MUL(2,2,2),
1238 _mm_add_pd(MUL(2,3,2),
1240 _mm_add_pd(MUL(2,0,3),
1241 _mm_add_pd(MUL(2,1,3),
1242 _mm_add_pd(MUL(2,2,3),
1243 _mm_add_pd(MUL(2,3,3),
1245 _mm_add_pd(MUL(2,0,4),
1246 _mm_add_pd(MUL(2,1,4),
1247 _mm_add_pd(MUL(2,2,4),
1248 _mm_add_pd(MUL(2,3,4),
1260 A5=_mm_set1_pd(A(0,4));
1261 A4=_mm_set1_pd(A(0,3));
1262 A3=_mm_set1_pd(A(0,2));
1263 A2=_mm_set1_pd(A(0,1));
1264 A1=_mm_set1_pd(A(0,0));
1272 union dvec2 &temp2=p2[0];
1273 temp2.v=_mm_add_pd(_mm_mul_pd(A1,GetV(0,0)),
1274 _mm_add_pd(_mm_mul_pd(A2,GetV(0,1)),
1275 _mm_add_pd(_mm_mul_pd(A3,GetV(0,2)),
1276 _mm_add_pd(_mm_mul_pd(A4,GetV(0,3)),
1277 _mm_mul_pd(A5,GetV(0,4))))));
1284 BA1=_mm_set1_pd(temp2.d[0]);
1285 BA2=_mm_set1_pd(temp2.d[1]);
1286 temp2.v=_mm_add_pd(_mm_mul_pd(A1,GetV(1,0)),
1287 _mm_add_pd(_mm_mul_pd(A2,GetV(1,1)),
1288 _mm_add_pd(_mm_mul_pd(A3,GetV(1,2)),
1289 _mm_add_pd(_mm_mul_pd(A4,GetV(1,3)),
1290 _mm_mul_pd(A5,GetV(1,4))))));
1291 BA3=_mm_set1_pd(temp2.d[0]);
1292 BA4=_mm_set1_pd(temp2.d[1]);
1293 BA5=_mm_set1_pd(
mA[0].d[4]*A(0,0)+
mA[1].d[4]*A(0,1)+
mA[2].d[4]*A(0,2)+
mA[3].d[4]*A(0,3)+
mA[4].d[4]*A(0,4));
1295 temp2.v=_mm_add_pd(_mm_mul_pd(A.GetV(0,0),BA1),
1296 _mm_add_pd(_mm_mul_pd(A.GetV(0,1),BA2),
1297 _mm_add_pd(_mm_mul_pd(A.GetV(0,2),BA3),
1298 _mm_add_pd(_mm_mul_pd(A.GetV(0,3),BA4),
1299 _mm_mul_pd(A.GetV(0,4),BA5)))));
1300 double C11=temp2.d[0];
1301 double C12=temp2.d[1];
1303 temp2.v=_mm_add_pd(_mm_mul_pd(A.GetV(1,0),BA1),
1304 _mm_add_pd(_mm_mul_pd(A.GetV(1,1),BA2),
1305 _mm_add_pd(_mm_mul_pd(A.GetV(1,2),BA3),
1306 _mm_add_pd(_mm_mul_pd(A.GetV(1,3),BA4),
1307 _mm_mul_pd(A.GetV(1,4),BA5)))));
1309 double C13=temp2.d[0];
1310 double C14=temp2.d[1];
1312 temp2.v=_mm_add_pd(_mm_mul_pd(A.GetV(2,0),BA1),
1313 _mm_add_pd(_mm_mul_pd(A.GetV(2,1),BA2),
1314 _mm_add_pd(_mm_mul_pd(A.GetV(2,2),BA3),
1315 _mm_add_pd(_mm_mul_pd(A.GetV(2,3),BA4),
1316 _mm_mul_pd(A.GetV(2,4),BA5)))));
1318 double C15=temp2.d[0];
1322 A5=_mm_set1_pd(A(1,4));
1323 A4=_mm_set1_pd(A(1,3));
1324 A3=_mm_set1_pd(A(1,2));
1325 A2=_mm_set1_pd(A(1,1));
1326 A1=_mm_set1_pd(A(1,0));
1327 temp2.v=_mm_add_pd(_mm_mul_pd(A1,GetV(0,0)),
1328 _mm_add_pd(_mm_mul_pd(A2,GetV(0,1)),
1329 _mm_add_pd(_mm_mul_pd(A3,GetV(0,2)),
1330 _mm_add_pd(_mm_mul_pd(A4,GetV(0,3)),
1331 _mm_mul_pd(A5,GetV(0,4))))));
1332 BA1=_mm_set1_pd(temp2.d[0]);
1333 BA2=_mm_set1_pd(temp2.d[1]);
1334 temp2.v=_mm_add_pd(_mm_mul_pd(A1,GetV(1,0)),
1335 _mm_add_pd(_mm_mul_pd(A2,GetV(1,1)),
1336 _mm_add_pd(_mm_mul_pd(A3,GetV(1,2)),
1337 _mm_add_pd(_mm_mul_pd(A4,GetV(1,3)),
1338 _mm_mul_pd(A5,GetV(1,4))))));
1339 BA3=_mm_set1_pd(temp2.d[0]);
1340 BA4=_mm_set1_pd(temp2.d[1]);
1341 BA5=_mm_set1_pd(
mA[0].d[4]*A(1,0)+
mA[1].d[4]*A(1,1)+
mA[2].d[4]*A(1,2)+
mA[3].d[4]*A(1,3)+
mA[4].d[4]*A(1,4));
1343 temp2.v=_mm_add_pd(_mm_mul_pd(A.GetV(0,0),BA1),
1344 _mm_add_pd(_mm_mul_pd(A.GetV(0,1),BA2),
1345 _mm_add_pd(_mm_mul_pd(A.GetV(0,2),BA3),
1346 _mm_add_pd(_mm_mul_pd(A.GetV(0,3),BA4),
1347 _mm_mul_pd(A.GetV(0,4),BA5)))));
1349 double C22=temp2.d[1];
1351 temp2.v=_mm_add_pd(_mm_mul_pd(A.GetV(1,0),BA1),
1352 _mm_add_pd(_mm_mul_pd(A.GetV(1,1),BA2),
1353 _mm_add_pd(_mm_mul_pd(A.GetV(1,2),BA3),
1354 _mm_add_pd(_mm_mul_pd(A.GetV(1,3),BA4),
1355 _mm_mul_pd(A.GetV(1,4),BA5)))));
1357 double C23=temp2.d[0];
1358 double C24=temp2.d[1];
1360 temp2.v=_mm_add_pd(_mm_mul_pd(A.GetV(2,0),BA1),
1361 _mm_add_pd(_mm_mul_pd(A.GetV(2,1),BA2),
1362 _mm_add_pd(_mm_mul_pd(A.GetV(2,2),BA3),
1363 _mm_add_pd(_mm_mul_pd(A.GetV(2,3),BA4),
1364 _mm_mul_pd(A.GetV(2,4),BA5)))));
1366 double C25=temp2.d[0];
1370 A5=_mm_set1_pd(A(2,4));
1371 A4=_mm_set1_pd(A(2,3));
1372 A3=_mm_set1_pd(A(2,2));
1373 A2=_mm_set1_pd(A(2,1));
1374 A1=_mm_set1_pd(A(2,0));
1375 temp2.v=_mm_add_pd(_mm_mul_pd(A1,GetV(0,0)),
1376 _mm_add_pd(_mm_mul_pd(A2,GetV(0,1)),
1377 _mm_add_pd(_mm_mul_pd(A3,GetV(0,2)),
1378 _mm_add_pd(_mm_mul_pd(A4,GetV(0,3)),
1379 _mm_mul_pd(A5,GetV(0,4))))));
1380 BA1=_mm_set1_pd(temp2.d[0]);
1381 BA2=_mm_set1_pd(temp2.d[1]);
1382 temp2.v=_mm_add_pd(_mm_mul_pd(A1,GetV(1,0)),
1383 _mm_add_pd(_mm_mul_pd(A2,GetV(1,1)),
1384 _mm_add_pd(_mm_mul_pd(A3,GetV(1,2)),
1385 _mm_add_pd(_mm_mul_pd(A4,GetV(1,3)),
1386 _mm_mul_pd(A5,GetV(1,4))))));
1387 BA3=_mm_set1_pd(temp2.d[0]);
1388 BA4=_mm_set1_pd(temp2.d[1]);
1389 BA5=_mm_set1_pd(
mA[0].d[4]*A(2,0)+
mA[1].d[4]*A(2,1)+
mA[2].d[4]*A(2,2)+
mA[3].d[4]*A(2,3)+
mA[4].d[4]*A(2,4));
1391 temp2.v=_mm_add_pd(_mm_mul_pd(A.GetV(1,0),BA1),
1392 _mm_add_pd(_mm_mul_pd(A.GetV(1,1),BA2),
1393 _mm_add_pd(_mm_mul_pd(A.GetV(1,2),BA3),
1394 _mm_add_pd(_mm_mul_pd(A.GetV(1,3),BA4),
1395 _mm_mul_pd(A.GetV(1,4),BA5)))));
1397 double C33=temp2.d[0];
1398 double C34=temp2.d[1];
1400 temp2.v=_mm_add_pd(_mm_mul_pd(A.GetV(2,0),BA1),
1401 _mm_add_pd(_mm_mul_pd(A.GetV(2,1),BA2),
1402 _mm_add_pd(_mm_mul_pd(A.GetV(2,2),BA3),
1403 _mm_add_pd(_mm_mul_pd(A.GetV(2,3),BA4),
1404 _mm_mul_pd(A.GetV(2,4),BA5)))));
1406 double C35=temp2.d[0];
1410 A5=_mm_set1_pd(A(3,4));
1411 A4=_mm_set1_pd(A(3,3));
1412 A3=_mm_set1_pd(A(3,2));
1413 A2=_mm_set1_pd(A(3,1));
1414 A1=_mm_set1_pd(A(3,0));
1415 temp2.v=_mm_add_pd(_mm_mul_pd(A1,GetV(0,0)),
1416 _mm_add_pd(_mm_mul_pd(A2,GetV(0,1)),
1417 _mm_add_pd(_mm_mul_pd(A3,GetV(0,2)),
1418 _mm_add_pd(_mm_mul_pd(A4,GetV(0,3)),
1419 _mm_mul_pd(A5,GetV(0,4))))));
1420 BA1=_mm_set1_pd(temp2.d[0]);
1421 BA2=_mm_set1_pd(temp2.d[1]);
1422 temp2.v=_mm_add_pd(_mm_mul_pd(A1,GetV(1,0)),
1423 _mm_add_pd(_mm_mul_pd(A2,GetV(1,1)),
1424 _mm_add_pd(_mm_mul_pd(A3,GetV(1,2)),
1425 _mm_add_pd(_mm_mul_pd(A4,GetV(1,3)),
1426 _mm_mul_pd(A5,GetV(1,4))))));
1427 BA3=_mm_set1_pd(temp2.d[0]);
1428 BA4=_mm_set1_pd(temp2.d[1]);
1429 BA5=_mm_set1_pd(
mA[0].d[4]*A(3,0)+
mA[1].d[4]*A(3,1)+
mA[2].d[4]*A(3,2)+
mA[3].d[4]*A(3,3)+
mA[4].d[4]*A(3,4));
1431 temp2.v=_mm_add_pd(_mm_mul_pd(A.GetV(1,0),BA1),
1432 _mm_add_pd(_mm_mul_pd(A.GetV(1,1),BA2),
1433 _mm_add_pd(_mm_mul_pd(A.GetV(1,2),BA3),
1434 _mm_add_pd(_mm_mul_pd(A.GetV(1,3),BA4),
1435 _mm_mul_pd(A.GetV(1,4),BA5)))));
1436 double C44=temp2.d[1];
1439 temp2.v=_mm_add_pd(_mm_mul_pd(A.GetV(2,0),BA1),
1440 _mm_add_pd(_mm_mul_pd(A.GetV(2,1),BA2),
1441 _mm_add_pd(_mm_mul_pd(A.GetV(2,2),BA3),
1442 _mm_add_pd(_mm_mul_pd(A.GetV(2,3),BA4),
1443 _mm_mul_pd(A.GetV(2,4),BA5)))));
1445 double C45=temp2.d[0];
1448 A5=_mm_set1_pd(A(4,4));
1449 A4=_mm_set1_pd(A(4,3));
1450 A3=_mm_set1_pd(A(4,2));
1451 A2=_mm_set1_pd(A(4,1));
1452 A1=_mm_set1_pd(A(4,0));
1453 temp2.v=_mm_add_pd(_mm_mul_pd(A1,GetV(0,0)),
1454 _mm_add_pd(_mm_mul_pd(A2,GetV(0,1)),
1455 _mm_add_pd(_mm_mul_pd(A3,GetV(0,2)),
1456 _mm_add_pd(_mm_mul_pd(A4,GetV(0,3)),
1457 _mm_mul_pd(A5,GetV(0,4))))));
1458 BA1=_mm_set1_pd(temp2.d[0]);
1459 BA2=_mm_set1_pd(temp2.d[1]);
1460 temp2.v=_mm_add_pd(_mm_mul_pd(A1,GetV(1,0)),
1461 _mm_add_pd(_mm_mul_pd(A2,GetV(1,1)),
1462 _mm_add_pd(_mm_mul_pd(A3,GetV(1,2)),
1463 _mm_add_pd(_mm_mul_pd(A4,GetV(1,3)),
1464 _mm_mul_pd(A5,GetV(1,4))))));
1466 BA3=_mm_set1_pd(temp2.d[0]);
1467 BA4=_mm_set1_pd(temp2.d[1]);
1468 BA5=_mm_set1_pd(
mA[0].d[4]*A(4,0)+
mA[1].d[4]*A(4,1)+
mA[2].d[4]*A(4,2)+
mA[3].d[4]*A(4,3)+
mA[4].d[4]*A(4,4));
1471 temp2.v=_mm_add_pd(_mm_mul_pd(A.GetV(2,0),BA1),
1472 _mm_add_pd(_mm_mul_pd(A.GetV(2,1),BA2),
1473 _mm_add_pd(_mm_mul_pd(A.GetV(2,2),BA3),
1474 _mm_add_pd(_mm_mul_pd(A.GetV(2,3),BA4),
1475 _mm_mul_pd(A.GetV(2,4),BA5)))));
1476 double C55=temp2.d[0];
1478 return DMatrix5x5(C11,C12,C13,C14,C15,C22,C23,C24,C25,C33,C34,C35,C44,C45,C55);
1486 #define SWAP(i,j,k,m) _mm_setr_pd(mA[(j)].d[(i)],mA[(m)].d[(k)])
1488 return DMatrix5x5(SWAP(0,0,0,1),SWAP(1,0,1,1),SWAP(2,0,2,1),SWAP(3,0,3,1),SWAP(4,0,4,1),
1489 SWAP(0,2,0,3),SWAP(1,2,1,3),SWAP(2,2,2,3),SWAP(3,2,3,3),SWAP(4,2,4,3),
1491 _mm_setr_pd(
mA[4].d[0],0.),_mm_setr_pd(
mA[4].d[1],0.),
1492 _mm_setr_pd(
mA[4].d[2],0.),_mm_setr_pd(
mA[4].d[3],0.),
1493 _mm_setr_pd(
mA[4].d[4],0.));
1499 DMatrix3x2 C(GetV(1,0),GetV(1,1),GetV(2,0),GetV(2,1));
1501 DMatrix3x3 D(GetV(1,2),GetV(1,3),GetV(1,4),GetV(2,2),GetV(2,3),GetV(2,4));
1506 return DMatrix5x5(AA,-AA*BDinv,-DD*CAinv,DD);
1511 DMatrix3x2 C(GetV(1,0),GetV(1,1),GetV(2,0),GetV(2,1));
1513 DMatrix3x3 D(GetV(1,2),GetV(1,3),GetV(1,4),GetV(2,2),GetV(2,3),GetV(2,4));
1518 return DMatrix5x5(AA,-AA*BDinv,-DD*CAinv,DD);
1525 for (
unsigned int i=0;i<5;i++){
1526 for (
unsigned int j=0;j<3;j++){
1527 mA[i].v[j]=_mm_setzero_pd();
1536 __m128d &scale=p[0];
1537 scale=_mm_set1_pd(c);
1539 for (
unsigned int i=0;i<5;i++){
1540 for (
unsigned int j=0;j<3;j++){
1541 mA[i].v[j]=_mm_mul_pd(scale,
mA[i].v[j]);
1548 cout <<
"DMatrix5x5:" <<endl;
1549 cout <<
" | 0 | 1 | 2 | 3 | 4 |" <<endl;
1550 cout <<
"----------------------------------------------------------------------" <<endl;
1552 for (
unsigned int i=0;i<5;i++){
1553 cout <<
" "<< i <<
" |";
1554 for (
unsigned int j=0;j<5;j++){
1555 cout << setw(11)<<setprecision(4)<<
mA[j].d[i] <<
" ";
1572 __m128d &scale=p[0];
1573 scale=_mm_set1_pd(c);
1575 #define SCALE(i,j) _mm_mul_pd(scale,M.GetV((i),(j)))
1576 return DMatrix5x5(SCALE(0,0),SCALE(0,1),SCALE(0,2),SCALE(0,3),SCALE(0,4),
1577 SCALE(1,0),SCALE(1,1),SCALE(1,2),SCALE(1,3),SCALE(1,4),
1578 SCALE(2,0),SCALE(2,1),SCALE(2,2),SCALE(2,3),SCALE(2,4));
DMatrix3x3 Invert() const
double SandwichMultiply(const DMatrix5x1 &A)
DMatrix5x5(const DMatrix5x5 &m2)
DMatrix2x1 operator*(const double c, const DMatrix2x1 &M)
#define ALIGNED_16_BLOCK_PTR(TYPE, NUM, PTR)
#define ALIGNED_16_BLOCK_WITH_PTR(TYPE, NUM, PTR)
DMatrix5x5 & operator+=(const DMatrix5x5 &m2)
DMatrixDSym GetSub(unsigned int lowerBound, unsigned int upperBound)
DMatrix5x2 operator*(const DMatrix5x2 &m2)
DMatrix5x5 & operator*=(const double c)
DMatrix5x5(const DMatrix2x2 &A, const DMatrix2x3 &B, const DMatrix3x2 &C, const DMatrix3x3 &D)
DMatrix5x5 operator+(const DMatrix5x5 &m2) const
DMatrix5x5 & operator=(const DMatrix5x5 &m1)
DMatrix5x5 SubSym(const DMatrix5x5 &m2) const
DMatrix5x5 SandwichMultiply(const DMatrix5x5 &A)
DMatrix5x5 SandwichMultiply2(const DMatrix5x5 &A)
double & operator()(int row, int col)
DMatrix5x5 operator*(const DMatrix5x5 &m2)
DMatrix5x5 operator-(const DMatrix5x5 &m2) const
DMatrix5x1 operator*(const DMatrix5x1 &m2)
#define ALIGNED_16_BLOCK(TYPE, NUM, PTR)
for(auto locVertexInfo:dStepVertexInfos)
DMatrix5x5 AddSym(const DMatrix5x5 &m2) const
DMatrix5x5 & operator-=(const DMatrix5x5 &m2)