35#ifndef _BLAZE_MATH_DENSE_INVERSION_H_
36#define _BLAZE_MATH_DENSE_INVERSION_H_
95template<
typename MT >
138inline void invertGeneral2x2( DenseMatrix<MT,SO>& dm )
146 using ET = ElementType_t<MT>;
150 const ET
det( A(0,0)*A(1,1) - A(0,1)*A(1,0) );
156 const ET idet( ET(1) /
det );
157 const ET a11( A(0,0) * idet );
159 A(0,0) = A(1,1) * idet;
160 A(1,0) = -A(1,0) * idet;
161 A(0,1) = -A(0,1) * idet;
187inline void invertSymmetric2x2( DenseMatrix<MT,SO>& dm )
194 using ET = ElementType_t<MT>;
199 const ET
det( A(0,0)*A(1,1) - A(0,1)*A(1,0) );
205 const ET idet( ET(1) /
det );
206 const ET a11( A(0,0) * idet );
208 B(0,0) = A(1,1) * idet;
209 B(1,0) = -A(1,0) * idet;
235inline void invertHermitian2x2( DenseMatrix<MT,SO>& dm )
242 using ET = ElementType_t<MT>;
247 const ET
det(
real( A(0,0)*A(1,1) - A(0,1)*A(1,0) ) );
253 const ET idet( ET(1) /
det );
254 const ET a11( A(0,0) * idet );
256 B(0,0) = ET( A(1,1) * idet );
257 B(1,0) = -A(1,0) * idet;
258 B(0,1) =
conj( B(1,0) );
283inline void invertLower2x2( DenseMatrix<MT,SO>& dm )
290 using ET = ElementType_t<MT>;
294 const ET
det( A(0,0) * A(1,1) );
300 const ET idet( ET(1) /
det );
301 const ET a11( A(0,0) * idet );
303 A(0,0) = A(1,1) * idet;
304 A(1,0) = -A(1,0) * idet;
327inline void invertUniLower2x2( DenseMatrix<MT,SO>& dm )
360inline void invertUpper2x2( DenseMatrix<MT,SO>& dm )
367 using ET = ElementType_t<MT>;
371 const ET
det( A(0,0) * A(1,1) );
377 const ET idet( ET(1) /
det );
378 const ET a11( A(0,0) * idet );
380 A(0,0) = A(1,1) * idet;
381 A(0,1) = -A(0,1) * idet;
404inline void invertUniUpper2x2( DenseMatrix<MT,SO>& dm )
437inline void invertDiagonal2x2( DenseMatrix<MT,SO>& dm )
444 using ET = ElementType_t<MT>;
448 const ET
det( A(0,0) * A(1,1) );
454 const ET idet( ET(1) /
det );
455 const ET a11( A(0,0) * idet );
457 A(0,0) = A(1,1) * idet;
490inline void invert2x2( DenseMatrix<MT,SO>& dm )
498 case byLU : invertGeneral2x2 ( *dm );
break;
499 case byLDLT : invertSymmetric2x2( *dm );
break;
500 case byLDLH : invertHermitian2x2( *dm );
break;
501 case byLLH : invertHermitian2x2( *dm );
break;
502 case asGeneral : invertGeneral2x2 ( *dm );
break;
503 case asSymmetric: invertSymmetric2x2( *dm );
break;
504 case asHermitian: invertHermitian2x2( *dm );
break;
505 case asLower : invertLower2x2 ( *dm );
break;
506 case asUniLower : invertUniLower2x2 ( *dm );
break;
507 case asUpper : invertUpper2x2 ( *dm );
break;
508 case asUniUpper : invertUniUpper2x2 ( *dm );
break;
509 case asDiagonal : invertDiagonal2x2 ( *dm );
break;
546inline void invertGeneral3x3( DenseMatrix<MT,SO>& dm )
554 using ET = ElementType_t<MT>;
556 const StaticMatrix<ET,3UL,3UL,SO> A( *dm );
559 B(0,0) = A(1,1)*A(2,2) - A(1,2)*A(2,1);
560 B(1,0) = A(1,2)*A(2,0) - A(1,0)*A(2,2);
561 B(2,0) = A(1,0)*A(2,1) - A(1,1)*A(2,0);
563 const ET
det( A(0,0)*B(0,0) + A(0,1)*B(1,0) + A(0,2)*B(2,0) );
569 B(0,1) = A(0,2)*A(2,1) - A(0,1)*A(2,2);
570 B(1,1) = A(0,0)*A(2,2) - A(0,2)*A(2,0);
571 B(2,1) = A(0,1)*A(2,0) - A(0,0)*A(2,1);
572 B(0,2) = A(0,1)*A(1,2) - A(0,2)*A(1,1);
573 B(1,2) = A(0,2)*A(1,0) - A(0,0)*A(1,2);
574 B(2,2) = A(0,0)*A(1,1) - A(0,1)*A(1,0);
601inline void invertSymmetric3x3( DenseMatrix<MT,SO>& dm )
608 using ET = ElementType_t<MT>;
610 const StaticMatrix<ET,3UL,3UL,SO> A( *dm );
613 B(0,0) = A(1,1)*A(2,2) - A(1,2)*A(2,1);
614 B(1,0) = A(1,2)*A(2,0) - A(1,0)*A(2,2);
615 B(2,0) = A(1,0)*A(2,1) - A(1,1)*A(2,0);
617 const ET
det( A(0,0)*B(0,0) + A(0,1)*B(1,0) + A(0,2)*B(2,0) );
624 B(1,1) = A(0,0)*A(2,2) - A(0,2)*A(2,0);
625 B(2,1) = A(0,1)*A(2,0) - A(0,0)*A(2,1);
628 B(2,2) = A(0,0)*A(1,1) - A(0,1)*A(1,0);
654inline void invertHermitian3x3( DenseMatrix<MT,SO>& dm )
661 using ET = ElementType_t<MT>;
663 const StaticMatrix<ET,3UL,3UL,SO> A( *dm );
666 B(0,0) = ET(
real( A(1,1)*A(2,2) - A(1,2)*A(2,1) ) );
667 B(1,0) = A(1,2)*A(2,0) - A(1,0)*A(2,2);
668 B(2,0) = A(1,0)*A(2,1) - A(1,1)*A(2,0);
670 const ET
det(
real( A(0,0)*B(0,0) + A(0,1)*B(1,0) + A(0,2)*B(2,0) ) );
676 B(0,1) =
conj( B(1,0) );
677 B(1,1) = ET(
real( A(0,0)*A(2,2) - A(0,2)*A(2,0) ) );
678 B(2,1) = A(0,1)*A(2,0) - A(0,0)*A(2,1);
679 B(0,2) =
conj( B(2,0) );
680 B(1,2) =
conj( B(2,1) );
681 B(2,2) = ET(
real( A(0,0)*A(1,1) - A(0,1)*A(1,0) ) );
707inline void invertLower3x3( DenseMatrix<MT,SO>& dm )
714 using ET = ElementType_t<MT>;
716 const StaticMatrix<ET,3UL,3UL,SO> A( *dm );
719 const ET tmp( A(1,1)*A(2,2) );
720 const ET
det( A(0,0)*tmp );
727 B(1,0) = - A(1,0)*A(2,2);
728 B(2,0) = A(1,0)*A(2,1) - A(1,1)*A(2,0);
729 B(1,1) = A(0,0)*A(2,2);
730 B(2,1) = - A(0,0)*A(2,1);
731 B(2,2) = A(0,0)*A(1,1);
755inline void invertUniLower3x3( DenseMatrix<MT,SO>& dm )
762 using ET = ElementType_t<MT>;
764 const StaticMatrix<ET,3UL,3UL,SO> A( *dm );
768 B(2,0) = A(1,0)*A(2,1) - A(2,0);
793inline void invertUpper3x3( DenseMatrix<MT,SO>& dm )
800 using ET = ElementType_t<MT>;
802 const StaticMatrix<ET,3UL,3UL,SO> A( *dm );
805 const ET tmp( A(1,1)*A(2,2) );
806 const ET
det( A(0,0)*tmp );
813 B(0,1) = - A(0,1)*A(2,2);
814 B(1,1) = A(0,0)*A(2,2);
815 B(0,2) = A(0,1)*A(1,2) - A(0,2)*A(1,1);
816 B(1,2) = - A(0,0)*A(1,2);
817 B(2,2) = A(0,0)*A(1,1);
841inline void invertUniUpper3x3( DenseMatrix<MT,SO>& dm )
848 using ET = ElementType_t<MT>;
850 const StaticMatrix<ET,3UL,3UL,SO> A( *dm );
854 B(0,2) = A(0,1)*A(1,2) - A(0,2);
879inline void invertDiagonal3x3( DenseMatrix<MT,SO>& dm )
886 using ET = ElementType_t<MT>;
890 const ET tmp1( A(0,0)*A(1,1) );
891 const ET tmp2( A(0,0)*A(2,2) );
893 const ET
det( tmp1*A(2,2) );
899 const ET idet( ET(1) /
det );
901 A(0,0) = A(1,1)*A(2,2)*idet;
935inline void invert3x3( DenseMatrix<MT,SO>& dm )
943 case byLU : invertGeneral3x3 ( *dm );
break;
944 case byLDLT : invertSymmetric3x3( *dm );
break;
945 case byLDLH : invertHermitian3x3( *dm );
break;
946 case byLLH : invertHermitian3x3( *dm );
break;
947 case asGeneral : invertGeneral3x3 ( *dm );
break;
948 case asSymmetric: invertSymmetric3x3( *dm );
break;
949 case asHermitian: invertHermitian3x3( *dm );
break;
950 case asLower : invertLower3x3 ( *dm );
break;
951 case asUniLower : invertUniLower3x3 ( *dm );
break;
952 case asUpper : invertUpper3x3 ( *dm );
break;
953 case asUniUpper : invertUniUpper3x3 ( *dm );
break;
954 case asDiagonal : invertDiagonal3x3 ( *dm );
break;
991inline void invertGeneral4x4( DenseMatrix<MT,SO>& dm )
999 using ET = ElementType_t<MT>;
1001 const StaticMatrix<ET,4UL,4UL,SO> A( *dm );
1004 ET tmp1( A(2,2)*A(3,3) - A(2,3)*A(3,2) );
1005 ET tmp2( A(2,1)*A(3,3) - A(2,3)*A(3,1) );
1006 ET tmp3( A(2,1)*A(3,2) - A(2,2)*A(3,1) );
1008 B(0,0) = A(1,1)*tmp1 - A(1,2)*tmp2 + A(1,3)*tmp3;
1009 B(0,1) = A(0,2)*tmp2 - A(0,1)*tmp1 - A(0,3)*tmp3;
1011 ET tmp4( A(2,0)*A(3,3) - A(2,3)*A(3,0) );
1012 ET tmp5( A(2,0)*A(3,2) - A(2,2)*A(3,0) );
1014 B(1,0) = A(1,2)*tmp4 - A(1,0)*tmp1 - A(1,3)*tmp5;
1015 B(1,1) = A(0,0)*tmp1 - A(0,2)*tmp4 + A(0,3)*tmp5;
1017 tmp1 = A(2,0)*A(3,1) - A(2,1)*A(3,0);
1019 B(2,0) = A(1,0)*tmp2 - A(1,1)*tmp4 + A(1,3)*tmp1;
1020 B(2,1) = A(0,1)*tmp4 - A(0,0)*tmp2 - A(0,3)*tmp1;
1021 B(3,0) = A(1,1)*tmp5 - A(1,0)*tmp3 - A(1,2)*tmp1;
1022 B(3,1) = A(0,0)*tmp3 - A(0,1)*tmp5 + A(0,2)*tmp1;
1024 tmp1 = A(0,2)*A(1,3) - A(0,3)*A(1,2);
1025 tmp2 = A(0,1)*A(1,3) - A(0,3)*A(1,1);
1026 tmp3 = A(0,1)*A(1,2) - A(0,2)*A(1,1);
1028 B(0,2) = A(3,1)*tmp1 - A(3,2)*tmp2 + A(3,3)*tmp3;
1029 B(0,3) = A(2,2)*tmp2 - A(2,1)*tmp1 - A(2,3)*tmp3;
1031 tmp4 = A(0,0)*A(1,3) - A(0,3)*A(1,0);
1032 tmp5 = A(0,0)*A(1,2) - A(0,2)*A(1,0);
1034 B(1,2) = A(3,2)*tmp4 - A(3,0)*tmp1 - A(3,3)*tmp5;
1035 B(1,3) = A(2,0)*tmp1 - A(2,2)*tmp4 + A(2,3)*tmp5;
1037 tmp1 = A(0,0)*A(1,1) - A(0,1)*A(1,0);
1039 B(2,2) = A(3,0)*tmp2 - A(3,1)*tmp4 + A(3,3)*tmp1;
1040 B(2,3) = A(2,1)*tmp4 - A(2,0)*tmp2 - A(2,3)*tmp1;
1041 B(3,2) = A(3,1)*tmp5 - A(3,0)*tmp3 - A(3,2)*tmp1;
1042 B(3,3) = A(2,0)*tmp3 - A(2,1)*tmp5 + A(2,2)*tmp1;
1044 const ET
det( A(0,0)*B(0,0) + A(0,1)*B(1,0) + A(0,2)*B(2,0) + A(0,3)*B(3,0) );
1073template<
typename MT
1075inline void invertSymmetric4x4( DenseMatrix<MT,SO>& dm )
1082 using ET = ElementType_t<MT>;
1084 const StaticMatrix<ET,4UL,4UL,SO> A( *dm );
1087 ET tmp1( A(2,2)*A(3,3) - A(2,3)*A(3,2) );
1088 ET tmp2( A(2,1)*A(3,3) - A(2,3)*A(3,1) );
1089 ET tmp3( A(2,1)*A(3,2) - A(2,2)*A(3,1) );
1091 B(0,0) = A(1,1)*tmp1 - A(1,2)*tmp2 + A(1,3)*tmp3;
1092 B(0,1) = A(0,2)*tmp2 - A(0,1)*tmp1 - A(0,3)*tmp3;
1094 ET tmp4( A(2,0)*A(3,3) - A(2,3)*A(3,0) );
1095 ET tmp5( A(2,0)*A(3,2) - A(2,2)*A(3,0) );
1097 B(1,1) = A(0,0)*tmp1 - A(0,2)*tmp4 + A(0,3)*tmp5;
1099 tmp1 = A(2,0)*A(3,1) - A(2,1)*A(3,0);
1101 B(2,0) = A(1,0)*tmp2 - A(1,1)*tmp4 + A(1,3)*tmp1;
1102 B(2,1) = A(0,1)*tmp4 - A(0,0)*tmp2 - A(0,3)*tmp1;
1103 B(3,0) = A(1,1)*tmp5 - A(1,0)*tmp3 - A(1,2)*tmp1;
1104 B(3,1) = A(0,0)*tmp3 - A(0,1)*tmp5 + A(0,2)*tmp1;
1106 tmp1 = A(0,1)*A(1,3) - A(0,3)*A(1,1);
1107 tmp2 = A(0,1)*A(1,2) - A(0,2)*A(1,1);
1108 tmp3 = A(0,0)*A(1,3) - A(0,3)*A(1,0);
1109 tmp4 = A(0,0)*A(1,2) - A(0,2)*A(1,0);
1110 tmp5 = A(0,0)*A(1,1) - A(0,1)*A(1,0);
1112 B(2,2) = A(3,0)*tmp1 - A(3,1)*tmp3 + A(3,3)*tmp5;
1113 B(2,3) = A(2,1)*tmp3 - A(2,0)*tmp1 - A(2,3)*tmp5;
1114 B(3,3) = A(2,0)*tmp2 - A(2,1)*tmp4 + A(2,2)*tmp5;
1123 const ET
det( A(0,0)*B(0,0) + A(0,1)*B(1,0) + A(0,2)*B(2,0) + A(0,3)*B(3,0) );
1151template<
typename MT
1153inline void invertHermitian4x4( DenseMatrix<MT,SO>& dm )
1160 using ET = ElementType_t<MT>;
1162 const StaticMatrix<ET,4UL,4UL,SO> A( *dm );
1165 ET tmp1( A(2,2)*A(3,3) - A(2,3)*A(3,2) );
1166 ET tmp2( A(2,1)*A(3,3) - A(2,3)*A(3,1) );
1167 ET tmp3( A(2,1)*A(3,2) - A(2,2)*A(3,1) );
1169 B(0,0) = ET(
real( A(1,1)*tmp1 - A(1,2)*tmp2 + A(1,3)*tmp3 ) );
1170 B(0,1) = A(0,2)*tmp2 - A(0,1)*tmp1 - A(0,3)*tmp3;
1172 ET tmp4( A(2,0)*A(3,3) - A(2,3)*A(3,0) );
1173 ET tmp5( A(2,0)*A(3,2) - A(2,2)*A(3,0) );
1175 B(1,1) = ET(
real( A(0,0)*tmp1 - A(0,2)*tmp4 + A(0,3)*tmp5 ) );
1177 tmp1 = A(2,0)*A(3,1) - A(2,1)*A(3,0);
1179 B(2,0) = A(1,0)*tmp2 - A(1,1)*tmp4 + A(1,3)*tmp1;
1180 B(2,1) = A(0,1)*tmp4 - A(0,0)*tmp2 - A(0,3)*tmp1;
1181 B(3,0) = A(1,1)*tmp5 - A(1,0)*tmp3 - A(1,2)*tmp1;
1182 B(3,1) = A(0,0)*tmp3 - A(0,1)*tmp5 + A(0,2)*tmp1;
1184 tmp1 = A(0,1)*A(1,3) - A(0,3)*A(1,1);
1185 tmp2 = A(0,1)*A(1,2) - A(0,2)*A(1,1);
1186 tmp3 = A(0,0)*A(1,3) - A(0,3)*A(1,0);
1187 tmp4 = A(0,0)*A(1,2) - A(0,2)*A(1,0);
1188 tmp5 = A(0,0)*A(1,1) - A(0,1)*A(1,0);
1190 B(2,2) = ET(
real( A(3,0)*tmp1 - A(3,1)*tmp3 + A(3,3)*tmp5 ) );
1191 B(2,3) = A(2,1)*tmp3 - A(2,0)*tmp1 - A(2,3)*tmp5;
1192 B(3,3) = ET(
real( A(2,0)*tmp2 - A(2,1)*tmp4 + A(2,2)*tmp5 ) );
1194 B(0,2) =
conj( B(2,0) );
1195 B(0,3) =
conj( B(3,0) );
1196 B(1,0) =
conj( B(0,1) );
1197 B(1,2) =
conj( B(2,1) );
1198 B(1,3) =
conj( B(3,1) );
1199 B(3,2) =
conj( B(2,3) );
1201 const ET
det(
real( A(0,0)*B(0,0) + A(0,1)*B(1,0) + A(0,2)*B(2,0) + A(0,3)*B(3,0) ) );
1229template<
typename MT
1231inline void invertLower4x4( DenseMatrix<MT,SO>& dm )
1238 using ET = ElementType_t<MT>;
1240 const StaticMatrix<ET,4UL,4UL,SO> A( *dm );
1243 const ET tmp1( A(2,2)*A(3,3) );
1244 const ET tmp2( A(2,1)*A(3,3) );
1245 const ET tmp3( A(2,1)*A(3,2) - A(2,2)*A(3,1) );
1246 const ET tmp4( A(0,0)*A(1,1) );
1248 const ET
det( tmp4 * A(2,2) * A(3,3) );
1254 B(0,0) = A(1,1)*tmp1;
1255 B(1,0) = - A(1,0)*tmp1;
1256 B(2,0) = A(1,0)*tmp2 - A(1,1)*A(2,0)*A(3,3);
1257 B(3,0) = A(1,1)*( A(2,0)*A(3,2) - A(2,2)*A(3,0) ) - A(1,0)*tmp3;
1258 B(1,1) = A(0,0)*tmp1;
1259 B(2,1) = - A(0,0)*tmp2;
1260 B(3,1) = A(0,0)*tmp3;
1261 B(2,2) = A(3,3)*tmp4;
1262 B(3,2) = - A(3,2)*tmp4;
1263 B(3,3) = A(2,2)*tmp4;
1285template<
typename MT
1287inline void invertUniLower4x4( DenseMatrix<MT,SO>& dm )
1294 using ET = ElementType_t<MT>;
1296 const StaticMatrix<ET,4UL,4UL,SO> A( *dm );
1299 const ET tmp( A(2,1)*A(3,2) - A(3,1) );
1302 B(2,0) = A(1,0)*A(2,1) - A(2,0);
1303 B(3,0) = A(2,0)*A(3,2) - A(3,0) - A(1,0)*tmp;
1328template<
typename MT
1330inline void invertUpper4x4( DenseMatrix<MT,SO>& dm )
1337 using ET = ElementType_t<MT>;
1339 const StaticMatrix<ET,4UL,4UL,SO> A( *dm );
1342 ET tmp1( A(2,2)*A(3,3) );
1343 ET tmp2( A(0,1)*A(1,2) - A(0,2)*A(1,1) );
1344 ET tmp3( A(0,0)*A(1,2) );
1345 ET tmp4( A(0,0)*A(1,1) );
1347 const ET
det( A(0,0)*A(1,1)*tmp1 );
1353 B(0,0) = A(1,1)*tmp1;
1354 B(0,1) = - A(0,1)*tmp1;
1355 B(1,1) = A(0,0)*tmp1;
1356 B(0,2) = A(3,3)*tmp2;
1357 B(1,2) = - A(3,3)*tmp3;
1358 B(2,2) = A(3,3)*tmp4;
1359 B(0,3) = A(2,2)*( A(0,1)*A(1,3) - A(0,3)*A(1,1) ) - A(2,3)*tmp2;
1360 B(1,3) = A(2,3)*tmp3 - A(2,2)*A(0,0)*A(1,3);
1361 B(2,3) = - A(2,3)*tmp4;
1362 B(3,3) = A(2,2)*tmp4;
1384template<
typename MT
1386inline void invertUniUpper4x4( DenseMatrix<MT,SO>& dm )
1393 using ET = ElementType_t<MT>;
1395 const StaticMatrix<ET,4UL,4UL,SO> A( *dm );
1398 ET tmp( A(0,1)*A(1,2) - A(0,2) );
1403 B(0,3) = A(0,1)*A(1,3) - A(0,3) - A(2,3)*tmp;
1404 B(1,3) = A(2,3)*A(1,2) - A(1,3);
1427template<
typename MT
1429inline void invertDiagonal4x4( DenseMatrix<MT,SO>& dm )
1436 using ET = ElementType_t<MT>;
1440 const ET tmp1( A(2,2)*A(3,3) );
1441 const ET tmp2( A(0,0)*A(1,1) );
1442 const ET tmp3( A(0,0)*tmp1 );
1443 const ET tmp4( A(2,2)*tmp2 );
1445 const ET
det( tmp1 * tmp2 );
1451 const ET idet( ET(1) /
det );
1453 A(0,0) = A(1,1)*tmp1*idet;
1455 A(2,2) = A(3,3)*tmp2*idet;
1488inline void invert4x4( DenseMatrix<MT,SO>& dm )
1496 case byLU : invertGeneral4x4 ( *dm );
break;
1497 case byLDLT : invertSymmetric4x4( *dm );
break;
1498 case byLDLH : invertHermitian4x4( *dm );
break;
1499 case byLLH : invertHermitian4x4( *dm );
break;
1500 case asGeneral : invertGeneral4x4 ( *dm );
break;
1501 case asSymmetric: invertSymmetric4x4( *dm );
break;
1502 case asHermitian: invertHermitian4x4( *dm );
break;
1503 case asLower : invertLower4x4 ( *dm );
break;
1504 case asUniLower : invertUniLower4x4 ( *dm );
break;
1505 case asUpper : invertUpper4x4 ( *dm );
break;
1506 case asUniUpper : invertUniUpper4x4 ( *dm );
break;
1507 case asDiagonal : invertDiagonal4x4 ( *dm );
break;
1542template<
typename MT
1544inline void invertGeneral5x5( DenseMatrix<MT,SO>& dm )
1552 using ET = ElementType_t<MT>;
1554 const StaticMatrix<ET,5UL,5UL,SO> A( *dm );
1557 ET tmp1 ( A(3,3)*A(4,4) - A(3,4)*A(4,3) );
1558 ET tmp2 ( A(3,2)*A(4,4) - A(3,4)*A(4,2) );
1559 ET tmp3 ( A(3,2)*A(4,3) - A(3,3)*A(4,2) );
1560 ET tmp4 ( A(3,1)*A(4,4) - A(3,4)*A(4,1) );
1561 ET tmp5 ( A(3,1)*A(4,3) - A(3,3)*A(4,1) );
1562 ET tmp6 ( A(3,1)*A(4,2) - A(3,2)*A(4,1) );
1563 ET tmp7 ( A(3,0)*A(4,4) - A(3,4)*A(4,0) );
1564 ET tmp8 ( A(3,0)*A(4,3) - A(3,3)*A(4,0) );
1565 ET tmp9 ( A(3,0)*A(4,2) - A(3,2)*A(4,0) );
1566 ET tmp10( A(3,0)*A(4,1) - A(3,1)*A(4,0) );
1568 ET tmp11( A(2,2)*tmp1 - A(2,3)*tmp2 + A(2,4)*tmp3 );
1569 ET tmp12( A(2,1)*tmp1 - A(2,3)*tmp4 + A(2,4)*tmp5 );
1570 ET tmp13( A(2,1)*tmp2 - A(2,2)*tmp4 + A(2,4)*tmp6 );
1571 ET tmp14( A(2,1)*tmp3 - A(2,2)*tmp5 + A(2,3)*tmp6 );
1572 ET tmp15( A(2,0)*tmp1 - A(2,3)*tmp7 + A(2,4)*tmp8 );
1573 ET tmp16( A(2,0)*tmp2 - A(2,2)*tmp7 + A(2,4)*tmp9 );
1574 ET tmp17( A(2,0)*tmp3 - A(2,2)*tmp8 + A(2,3)*tmp9 );
1576 B(0,0) = A(1,1)*tmp11 - A(1,2)*tmp12 + A(1,3)*tmp13 - A(1,4)*tmp14;
1577 B(0,1) = - A(0,1)*tmp11 + A(0,2)*tmp12 - A(0,3)*tmp13 + A(0,4)*tmp14;
1578 B(1,0) = - A(1,0)*tmp11 + A(1,2)*tmp15 - A(1,3)*tmp16 + A(1,4)*tmp17;
1579 B(1,1) = A(0,0)*tmp11 - A(0,2)*tmp15 + A(0,3)*tmp16 - A(0,4)*tmp17;
1581 ET tmp18( A(2,0)*tmp4 - A(2,1)*tmp7 + A(2,4)*tmp10 );
1582 ET tmp19( A(2,0)*tmp5 - A(2,1)*tmp8 + A(2,3)*tmp10 );
1583 ET tmp20( A(2,0)*tmp6 - A(2,1)*tmp9 + A(2,2)*tmp10 );
1585 B(2,0) = A(1,0)*tmp12 - A(1,1)*tmp15 + A(1,3)*tmp18 - A(1,4)*tmp19;
1586 B(2,1) = - A(0,0)*tmp12 + A(0,1)*tmp15 - A(0,3)*tmp18 + A(0,4)*tmp19;
1587 B(3,0) = - A(1,0)*tmp13 + A(1,1)*tmp16 - A(1,2)*tmp18 + A(1,4)*tmp20;
1588 B(3,1) = A(0,0)*tmp13 - A(0,1)*tmp16 + A(0,2)*tmp18 - A(0,4)*tmp20;
1589 B(4,0) = A(1,0)*tmp14 - A(1,1)*tmp17 + A(1,2)*tmp19 - A(1,3)*tmp20;
1590 B(4,1) = - A(0,0)*tmp14 + A(0,1)*tmp17 - A(0,2)*tmp19 + A(0,3)*tmp20;
1592 tmp11 = A(1,2)*tmp1 - A(1,3)*tmp2 + A(1,4)*tmp3;
1593 tmp12 = A(1,1)*tmp1 - A(1,3)*tmp4 + A(1,4)*tmp5;
1594 tmp13 = A(1,1)*tmp2 - A(1,2)*tmp4 + A(1,4)*tmp6;
1595 tmp14 = A(1,1)*tmp3 - A(1,2)*tmp5 + A(1,3)*tmp6;
1596 tmp15 = A(1,0)*tmp1 - A(1,3)*tmp7 + A(1,4)*tmp8;
1597 tmp16 = A(1,0)*tmp2 - A(1,2)*tmp7 + A(1,4)*tmp9;
1598 tmp17 = A(1,0)*tmp3 - A(1,2)*tmp8 + A(1,3)*tmp9;
1599 tmp18 = A(1,0)*tmp4 - A(1,1)*tmp7 + A(1,4)*tmp10;
1600 tmp19 = A(1,0)*tmp5 - A(1,1)*tmp8 + A(1,3)*tmp10;
1602 B(0,2) = A(0,1)*tmp11 - A(0,2)*tmp12 + A(0,3)*tmp13 - A(0,4)*tmp14;
1603 B(1,2) = - A(0,0)*tmp11 + A(0,2)*tmp15 - A(0,3)*tmp16 + A(0,4)*tmp17;
1604 B(2,2) = A(0,0)*tmp12 - A(0,1)*tmp15 + A(0,3)*tmp18 - A(0,4)*tmp19;
1606 tmp1 = A(0,2)*A(1,3) - A(0,3)*A(1,2);
1607 tmp2 = A(0,1)*A(1,3) - A(0,3)*A(1,1);
1608 tmp3 = A(0,1)*A(1,2) - A(0,2)*A(1,1);
1609 tmp4 = A(0,0)*A(1,3) - A(0,3)*A(1,0);
1610 tmp5 = A(0,0)*A(1,2) - A(0,2)*A(1,0);
1611 tmp6 = A(0,0)*A(1,1) - A(0,1)*A(1,0);
1612 tmp7 = A(0,2)*A(1,4) - A(0,4)*A(1,2);
1613 tmp8 = A(0,1)*A(1,4) - A(0,4)*A(1,1);
1614 tmp9 = A(0,0)*A(1,4) - A(0,4)*A(1,0);
1615 tmp10 = A(0,3)*A(1,4) - A(0,4)*A(1,3);
1617 tmp11 = A(2,2)*tmp10 - A(2,3)*tmp7 + A(2,4)*tmp1;
1618 tmp12 = A(2,1)*tmp10 - A(2,3)*tmp8 + A(2,4)*tmp2;
1619 tmp13 = A(2,1)*tmp7 - A(2,2)*tmp8 + A(2,4)*tmp3;
1620 tmp14 = A(2,1)*tmp1 - A(2,2)*tmp2 + A(2,3)*tmp3;
1621 tmp15 = A(2,0)*tmp10 - A(2,3)*tmp9 + A(2,4)*tmp4;
1622 tmp16 = A(2,0)*tmp7 - A(2,2)*tmp9 + A(2,4)*tmp5;
1623 tmp17 = A(2,0)*tmp1 - A(2,2)*tmp4 + A(2,3)*tmp5;
1625 B(0,3) = A(4,1)*tmp11 - A(4,2)*tmp12 + A(4,3)*tmp13 - A(4,4)*tmp14;
1626 B(0,4) = - A(3,1)*tmp11 + A(3,2)*tmp12 - A(3,3)*tmp13 + A(3,4)*tmp14;
1627 B(1,3) = - A(4,0)*tmp11 + A(4,2)*tmp15 - A(4,3)*tmp16 + A(4,4)*tmp17;
1628 B(1,4) = A(3,0)*tmp11 - A(3,2)*tmp15 + A(3,3)*tmp16 - A(3,4)*tmp17;
1630 tmp18 = A(2,0)*tmp8 - A(2,1)*tmp9 + A(2,4)*tmp6;
1631 tmp19 = A(2,0)*tmp2 - A(2,1)*tmp4 + A(2,3)*tmp6;
1632 tmp20 = A(2,0)*tmp3 - A(2,1)*tmp5 + A(2,2)*tmp6;
1634 B(2,3) = A(4,0)*tmp12 - A(4,1)*tmp15 + A(4,3)*tmp18 - A(4,4)*tmp19;
1635 B(2,4) = - A(3,0)*tmp12 + A(3,1)*tmp15 - A(3,3)*tmp18 + A(3,4)*tmp19;
1636 B(3,3) = - A(4,0)*tmp13 + A(4,1)*tmp16 - A(4,2)*tmp18 + A(4,4)*tmp20;
1637 B(3,4) = A(3,0)*tmp13 - A(3,1)*tmp16 + A(3,2)*tmp18 - A(3,4)*tmp20;
1638 B(4,3) = A(4,0)*tmp14 - A(4,1)*tmp17 + A(4,2)*tmp19 - A(4,3)*tmp20;
1639 B(4,4) = - A(3,0)*tmp14 + A(3,1)*tmp17 - A(3,2)*tmp19 + A(3,3)*tmp20;
1641 tmp11 = A(3,1)*tmp7 - A(3,2)*tmp8 + A(3,4)*tmp3;
1642 tmp12 = A(3,0)*tmp7 - A(3,2)*tmp9 + A(3,4)*tmp5;
1643 tmp13 = A(3,0)*tmp8 - A(3,1)*tmp9 + A(3,4)*tmp6;
1644 tmp14 = A(3,0)*tmp3 - A(3,1)*tmp5 + A(3,2)*tmp6;
1646 tmp15 = A(3,1)*tmp1 - A(3,2)*tmp2 + A(3,3)*tmp3;
1647 tmp16 = A(3,0)*tmp1 - A(3,2)*tmp4 + A(3,3)*tmp5;
1648 tmp17 = A(3,0)*tmp2 - A(3,1)*tmp4 + A(3,3)*tmp6;
1650 B(3,2) = A(4,0)*tmp11 - A(4,1)*tmp12 + A(4,2)*tmp13 - A(4,4)*tmp14;
1651 B(4,2) = - A(4,0)*tmp15 + A(4,1)*tmp16 - A(4,2)*tmp17 + A(4,3)*tmp14;
1653 const ET
det( A(0,0)*B(0,0) + A(0,1)*B(1,0) + A(0,2)*B(2,0) + A(0,3)*B(3,0) + A(0,4)*B(4,0) );
1682template<
typename MT
1684inline void invertSymmetric5x5( DenseMatrix<MT,SO>& dm )
1691 using ET = ElementType_t<MT>;
1693 const StaticMatrix<ET,5UL,5UL,SO> A( *dm );
1696 ET tmp1 ( A(3,3)*A(4,4) - A(3,4)*A(4,3) );
1697 ET tmp2 ( A(3,2)*A(4,4) - A(3,4)*A(4,2) );
1698 ET tmp3 ( A(3,2)*A(4,3) - A(3,3)*A(4,2) );
1699 ET tmp4 ( A(3,1)*A(4,4) - A(3,4)*A(4,1) );
1700 ET tmp5 ( A(3,1)*A(4,3) - A(3,3)*A(4,1) );
1701 ET tmp6 ( A(3,1)*A(4,2) - A(3,2)*A(4,1) );
1702 ET tmp7 ( A(3,0)*A(4,4) - A(3,4)*A(4,0) );
1703 ET tmp8 ( A(3,0)*A(4,3) - A(3,3)*A(4,0) );
1704 ET tmp9 ( A(3,0)*A(4,2) - A(3,2)*A(4,0) );
1705 ET tmp10( A(3,0)*A(4,1) - A(3,1)*A(4,0) );
1707 ET tmp11( A(2,2)*tmp1 - A(2,3)*tmp2 + A(2,4)*tmp3 );
1708 ET tmp12( A(2,1)*tmp1 - A(2,3)*tmp4 + A(2,4)*tmp5 );
1709 ET tmp13( A(2,1)*tmp2 - A(2,2)*tmp4 + A(2,4)*tmp6 );
1710 ET tmp14( A(2,1)*tmp3 - A(2,2)*tmp5 + A(2,3)*tmp6 );
1711 ET tmp15( A(2,0)*tmp1 - A(2,3)*tmp7 + A(2,4)*tmp8 );
1712 ET tmp16( A(2,0)*tmp2 - A(2,2)*tmp7 + A(2,4)*tmp9 );
1713 ET tmp17( A(2,0)*tmp3 - A(2,2)*tmp8 + A(2,3)*tmp9 );
1715 B(0,0) = A(1,1)*tmp11 - A(1,2)*tmp12 + A(1,3)*tmp13 - A(1,4)*tmp14;
1716 B(0,1) = - A(0,1)*tmp11 + A(0,2)*tmp12 - A(0,3)*tmp13 + A(0,4)*tmp14;
1717 B(1,1) = A(0,0)*tmp11 - A(0,2)*tmp15 + A(0,3)*tmp16 - A(0,4)*tmp17;
1719 ET tmp18( A(2,0)*tmp4 - A(2,1)*tmp7 + A(2,4)*tmp10 );
1720 ET tmp19( A(2,0)*tmp5 - A(2,1)*tmp8 + A(2,3)*tmp10 );
1721 ET tmp20( A(2,0)*tmp6 - A(2,1)*tmp9 + A(2,2)*tmp10 );
1723 B(2,0) = A(1,0)*tmp12 - A(1,1)*tmp15 + A(1,3)*tmp18 - A(1,4)*tmp19;
1724 B(2,1) = - A(0,0)*tmp12 + A(0,1)*tmp15 - A(0,3)*tmp18 + A(0,4)*tmp19;
1725 B(3,0) = - A(1,0)*tmp13 + A(1,1)*tmp16 - A(1,2)*tmp18 + A(1,4)*tmp20;
1726 B(3,1) = A(0,0)*tmp13 - A(0,1)*tmp16 + A(0,2)*tmp18 - A(0,4)*tmp20;
1727 B(4,0) = A(1,0)*tmp14 - A(1,1)*tmp17 + A(1,2)*tmp19 - A(1,3)*tmp20;
1728 B(4,1) = - A(0,0)*tmp14 + A(0,1)*tmp17 - A(0,2)*tmp19 + A(0,3)*tmp20;
1730 tmp11 = A(1,1)*tmp1 - A(1,3)*tmp4 + A(1,4)*tmp5;
1731 tmp12 = A(1,0)*tmp1 - A(1,3)*tmp7 + A(1,4)*tmp8;
1732 tmp13 = A(1,0)*tmp4 - A(1,1)*tmp7 + A(1,4)*tmp10;
1733 tmp14 = A(1,0)*tmp5 - A(1,1)*tmp8 + A(1,3)*tmp10;
1735 B(2,2) = A(0,0)*tmp11 - A(0,1)*tmp12 + A(0,3)*tmp13 - A(0,4)*tmp14;
1737 tmp1 = A(0,2)*A(1,3) - A(0,3)*A(1,2);
1738 tmp2 = A(0,1)*A(1,3) - A(0,3)*A(1,1);
1739 tmp3 = A(0,1)*A(1,2) - A(0,2)*A(1,1);
1740 tmp4 = A(0,0)*A(1,3) - A(0,3)*A(1,0);
1741 tmp5 = A(0,0)*A(1,2) - A(0,2)*A(1,0);
1742 tmp6 = A(0,0)*A(1,1) - A(0,1)*A(1,0);
1743 tmp7 = A(0,2)*A(1,4) - A(0,4)*A(1,2);
1744 tmp8 = A(0,1)*A(1,4) - A(0,4)*A(1,1);
1745 tmp9 = A(0,0)*A(1,4) - A(0,4)*A(1,0);
1746 tmp10 = A(0,3)*A(1,4) - A(0,4)*A(1,3);
1748 tmp11 = A(2,1)*tmp10 - A(2,3)*tmp8 + A(2,4)*tmp2;
1749 tmp12 = A(2,1)*tmp7 - A(2,2)*tmp8 + A(2,4)*tmp3;
1750 tmp13 = A(2,1)*tmp1 - A(2,2)*tmp2 + A(2,3)*tmp3;
1751 tmp14 = A(2,0)*tmp10 - A(2,3)*tmp9 + A(2,4)*tmp4;
1752 tmp15 = A(2,0)*tmp7 - A(2,2)*tmp9 + A(2,4)*tmp5;
1753 tmp16 = A(2,0)*tmp1 - A(2,2)*tmp4 + A(2,3)*tmp5;
1754 tmp17 = A(2,0)*tmp8 - A(2,1)*tmp9 + A(2,4)*tmp6;
1755 tmp18 = A(2,0)*tmp2 - A(2,1)*tmp4 + A(2,3)*tmp6;
1756 tmp19 = A(2,0)*tmp3 - A(2,1)*tmp5 + A(2,2)*tmp6;
1758 B(2,3) = A(4,0)*tmp11 - A(4,1)*tmp14 + A(4,3)*tmp17 - A(4,4)*tmp18;
1759 B(2,4) = - A(3,0)*tmp11 + A(3,1)*tmp14 - A(3,3)*tmp17 + A(3,4)*tmp18;
1760 B(3,3) = - A(4,0)*tmp12 + A(4,1)*tmp15 - A(4,2)*tmp17 + A(4,4)*tmp19;
1761 B(3,4) = A(3,0)*tmp12 - A(3,1)*tmp15 + A(3,2)*tmp17 - A(3,4)*tmp19;
1762 B(4,4) = - A(3,0)*tmp13 + A(3,1)*tmp16 - A(3,2)*tmp18 + A(3,3)*tmp19;
1775 const ET
det( A(0,0)*B(0,0) + A(0,1)*B(1,0) + A(0,2)*B(2,0) + A(0,3)*B(3,0) + A(0,4)*B(4,0) );
1803template<
typename MT
1805inline void invertHermitian5x5( DenseMatrix<MT,SO>& dm )
1812 using ET = ElementType_t<MT>;
1814 const StaticMatrix<ET,5UL,5UL,SO> A( *dm );
1817 ET tmp1 ( A(3,3)*A(4,4) - A(3,4)*A(4,3) );
1818 ET tmp2 ( A(3,2)*A(4,4) - A(3,4)*A(4,2) );
1819 ET tmp3 ( A(3,2)*A(4,3) - A(3,3)*A(4,2) );
1820 ET tmp4 ( A(3,1)*A(4,4) - A(3,4)*A(4,1) );
1821 ET tmp5 ( A(3,1)*A(4,3) - A(3,3)*A(4,1) );
1822 ET tmp6 ( A(3,1)*A(4,2) - A(3,2)*A(4,1) );
1823 ET tmp7 ( A(3,0)*A(4,4) - A(3,4)*A(4,0) );
1824 ET tmp8 ( A(3,0)*A(4,3) - A(3,3)*A(4,0) );
1825 ET tmp9 ( A(3,0)*A(4,2) - A(3,2)*A(4,0) );
1826 ET tmp10( A(3,0)*A(4,1) - A(3,1)*A(4,0) );
1828 ET tmp11( A(2,2)*tmp1 - A(2,3)*tmp2 + A(2,4)*tmp3 );
1829 ET tmp12( A(2,1)*tmp1 - A(2,3)*tmp4 + A(2,4)*tmp5 );
1830 ET tmp13( A(2,1)*tmp2 - A(2,2)*tmp4 + A(2,4)*tmp6 );
1831 ET tmp14( A(2,1)*tmp3 - A(2,2)*tmp5 + A(2,3)*tmp6 );
1832 ET tmp15( A(2,0)*tmp1 - A(2,3)*tmp7 + A(2,4)*tmp8 );
1833 ET tmp16( A(2,0)*tmp2 - A(2,2)*tmp7 + A(2,4)*tmp9 );
1834 ET tmp17( A(2,0)*tmp3 - A(2,2)*tmp8 + A(2,3)*tmp9 );
1836 B(0,0) = ET(
real( A(1,1)*tmp11 - A(1,2)*tmp12 + A(1,3)*tmp13 - A(1,4)*tmp14 ) );
1837 B(0,1) = - A(0,1)*tmp11 + A(0,2)*tmp12 - A(0,3)*tmp13 + A(0,4)*tmp14;
1838 B(1,1) = ET(
real( A(0,0)*tmp11 - A(0,2)*tmp15 + A(0,3)*tmp16 - A(0,4)*tmp17 ) );
1840 ET tmp18( A(2,0)*tmp4 - A(2,1)*tmp7 + A(2,4)*tmp10 );
1841 ET tmp19( A(2,0)*tmp5 - A(2,1)*tmp8 + A(2,3)*tmp10 );
1842 ET tmp20( A(2,0)*tmp6 - A(2,1)*tmp9 + A(2,2)*tmp10 );
1844 B(2,0) = A(1,0)*tmp12 - A(1,1)*tmp15 + A(1,3)*tmp18 - A(1,4)*tmp19;
1845 B(2,1) = - A(0,0)*tmp12 + A(0,1)*tmp15 - A(0,3)*tmp18 + A(0,4)*tmp19;
1846 B(3,0) = - A(1,0)*tmp13 + A(1,1)*tmp16 - A(1,2)*tmp18 + A(1,4)*tmp20;
1847 B(3,1) = A(0,0)*tmp13 - A(0,1)*tmp16 + A(0,2)*tmp18 - A(0,4)*tmp20;
1848 B(4,0) = A(1,0)*tmp14 - A(1,1)*tmp17 + A(1,2)*tmp19 - A(1,3)*tmp20;
1849 B(4,1) = - A(0,0)*tmp14 + A(0,1)*tmp17 - A(0,2)*tmp19 + A(0,3)*tmp20;
1851 tmp11 = A(1,1)*tmp1 - A(1,3)*tmp4 + A(1,4)*tmp5;
1852 tmp12 = A(1,0)*tmp1 - A(1,3)*tmp7 + A(1,4)*tmp8;
1853 tmp13 = A(1,0)*tmp4 - A(1,1)*tmp7 + A(1,4)*tmp10;
1854 tmp14 = A(1,0)*tmp5 - A(1,1)*tmp8 + A(1,3)*tmp10;
1856 B(2,2) = ET(
real( A(0,0)*tmp11 - A(0,1)*tmp12 + A(0,3)*tmp13 - A(0,4)*tmp14 ) );
1858 tmp1 = A(0,2)*A(1,3) - A(0,3)*A(1,2);
1859 tmp2 = A(0,1)*A(1,3) - A(0,3)*A(1,1);
1860 tmp3 = A(0,1)*A(1,2) - A(0,2)*A(1,1);
1861 tmp4 = A(0,0)*A(1,3) - A(0,3)*A(1,0);
1862 tmp5 = A(0,0)*A(1,2) - A(0,2)*A(1,0);
1863 tmp6 = A(0,0)*A(1,1) - A(0,1)*A(1,0);
1864 tmp7 = A(0,2)*A(1,4) - A(0,4)*A(1,2);
1865 tmp8 = A(0,1)*A(1,4) - A(0,4)*A(1,1);
1866 tmp9 = A(0,0)*A(1,4) - A(0,4)*A(1,0);
1867 tmp10 = A(0,3)*A(1,4) - A(0,4)*A(1,3);
1869 tmp11 = A(2,1)*tmp10 - A(2,3)*tmp8 + A(2,4)*tmp2;
1870 tmp12 = A(2,1)*tmp7 - A(2,2)*tmp8 + A(2,4)*tmp3;
1871 tmp13 = A(2,1)*tmp1 - A(2,2)*tmp2 + A(2,3)*tmp3;
1872 tmp14 = A(2,0)*tmp10 - A(2,3)*tmp9 + A(2,4)*tmp4;
1873 tmp15 = A(2,0)*tmp7 - A(2,2)*tmp9 + A(2,4)*tmp5;
1874 tmp16 = A(2,0)*tmp1 - A(2,2)*tmp4 + A(2,3)*tmp5;
1875 tmp17 = A(2,0)*tmp8 - A(2,1)*tmp9 + A(2,4)*tmp6;
1876 tmp18 = A(2,0)*tmp2 - A(2,1)*tmp4 + A(2,3)*tmp6;
1877 tmp19 = A(2,0)*tmp3 - A(2,1)*tmp5 + A(2,2)*tmp6;
1879 B(2,3) = A(4,0)*tmp11 - A(4,1)*tmp14 + A(4,3)*tmp17 - A(4,4)*tmp18;
1880 B(2,4) = - A(3,0)*tmp11 + A(3,1)*tmp14 - A(3,3)*tmp17 + A(3,4)*tmp18;
1881 B(3,3) = - ET(
real( A(4,0)*tmp12 - A(4,1)*tmp15 + A(4,2)*tmp17 - A(4,4)*tmp19 ) );
1882 B(3,4) = A(3,0)*tmp12 - A(3,1)*tmp15 + A(3,2)*tmp17 - A(3,4)*tmp19;
1883 B(4,4) = - ET(
real( A(3,0)*tmp13 - A(3,1)*tmp16 + A(3,2)*tmp18 - A(3,3)*tmp19 ) );
1885 B(0,2) =
conj( B(2,0) );
1886 B(0,3) =
conj( B(3,0) );
1887 B(0,4) =
conj( B(4,0) );
1888 B(1,0) =
conj( B(0,1) );
1889 B(1,2) =
conj( B(2,1) );
1890 B(1,3) =
conj( B(3,1) );
1891 B(1,4) =
conj( B(4,1) );
1892 B(3,2) =
conj( B(2,3) );
1893 B(4,2) =
conj( B(2,4) );
1894 B(4,3) =
conj( B(3,4) );
1896 const ET
det(
real( A(0,0)*B(0,0) + A(0,1)*B(1,0) + A(0,2)*B(2,0) + A(0,3)*B(3,0) + A(0,4)*B(4,0) ) );
1924template<
typename MT
1926inline void invertLower5x5( DenseMatrix<MT,SO>& dm )
1933 using ET = ElementType_t<MT>;
1935 const StaticMatrix<ET,5UL,5UL,SO> A( *dm );
1938 const ET tmp1( A(3,3)*A(4,4) );
1939 const ET tmp2( A(3,2)*A(4,4) );
1940 const ET tmp3( A(3,2)*A(4,3) - A(3,3)*A(4,2) );
1941 const ET tmp4( A(0,0)*A(1,1) );
1943 const ET tmp5 ( A(2,2)*tmp1 );
1944 const ET tmp6 ( A(2,1)*tmp1 );
1945 const ET tmp7 ( A(2,1)*tmp2 - A(2,2)*A(3,1)*A(4,4) );
1946 const ET tmp8 ( A(2,1)*tmp3 - A(2,2)*( A(3,1)*A(4,3) - A(3,3)*A(4,1) ) );
1947 const ET tmp9 ( A(3,2)*tmp4 );
1948 const ET tmp10( A(2,2)*tmp4 );
1950 B(0,0) = A(1,1)*tmp5;
1951 B(1,0) = - A(1,0)*tmp5;
1952 B(2,0) = A(1,0)*tmp6 - A(1,1)*A(2,0)*tmp1;
1953 B(3,0) = A(1,1)*( A(2,0)*tmp2 - A(2,2)*A(3,0)*A(4,4) ) - A(1,0)*tmp7;
1954 B(4,0) = A(1,0)*tmp8 - A(1,1)*( A(2,0)*tmp3 - A(2,2)*( A(3,0)*A(4,3) - A(3,3)*A(4,0) ) );
1955 B(1,1) = A(0,0)*tmp5;
1956 B(2,1) = - A(0,0)*tmp6;
1957 B(3,1) = A(0,0)*tmp7;
1958 B(4,1) = - A(0,0)*tmp8;
1959 B(2,2) = A(0,0)*A(1,1)*tmp1;
1960 B(3,2) = - A(4,4)*tmp9;
1961 B(4,2) = A(4,3)*tmp9 - A(4,2)*A(3,3)*tmp4;
1962 B(3,3) = A(4,4)*tmp10;
1963 B(4,3) = - A(4,3)*tmp10;
1964 B(4,4) = A(3,3)*tmp10;
1966 const ET
det( B(4,4) * A(4,4) );
1992template<
typename MT
1994inline void invertUniLower5x5( DenseMatrix<MT,SO>& dm )
2001 using ET = ElementType_t<MT>;
2003 const StaticMatrix<ET,5UL,5UL,SO> A( *dm );
2006 const ET tmp1( A(3,2)*A(4,3) - A(4,2) );
2007 const ET tmp2( A(2,1)*A(3,2) - A(3,1) );
2008 const ET tmp3( A(2,1)*tmp1 - A(3,1)*A(4,3) + A(4,1) );
2011 B(2,0) = A(1,0)*A(2,1) - A(2,0);
2012 B(3,0) = - A(1,0)*tmp2 + A(2,0)*A(3,2) - A(3,0);
2013 B(4,0) = A(1,0)*tmp3 - A(2,0)*tmp1 + A(3,0)*A(4,3) - A(4,0);
2018 B(4,2) = A(4,3)*A(3,2) - A(4,2);
2041template<
typename MT
2043inline void invertUpper5x5( DenseMatrix<MT,SO>& dm )
2050 using ET = ElementType_t<MT>;
2052 const StaticMatrix<ET,5UL,5UL,SO> A( *dm );
2055 const ET tmp1( A(3,3)*A(4,4) );
2056 const ET tmp2( A(0,1)*A(1,2) - A(0,2)*A(1,1) );
2057 const ET tmp3( A(0,0)*A(1,2) );
2058 const ET tmp4( A(0,0)*A(1,1) );
2060 const ET tmp5 ( A(2,2)*tmp1 );
2061 const ET tmp6 ( A(1,2)*tmp1 );
2062 const ET tmp7 ( A(1,1)*tmp1 );
2063 const ET tmp8 ( A(2,3)*tmp2 - A(2,2)*( A(0,1)*A(1,3) - A(0,3)*A(1,1) ) );
2064 const ET tmp9 ( A(2,3)*tmp3 - A(2,2)*A(0,0)*A(1,3) );
2065 const ET tmp10( A(2,3)*tmp4 );
2066 const ET tmp11( A(2,2)*tmp4 );
2068 B(0,0) = A(1,1)*tmp5;
2069 B(0,1) = - A(0,1)*tmp5;
2070 B(1,1) = A(0,0)*tmp5;
2071 B(0,2) = A(0,1)*tmp6 - A(0,2)*tmp7;
2072 B(1,2) = - A(0,0)*tmp6;
2073 B(2,2) = A(0,0)*tmp7;
2074 B(0,3) = - A(4,4)*tmp8;
2075 B(1,3) = A(4,4)*tmp9;
2076 B(2,3) = - A(4,4)*tmp10;
2077 B(3,3) = A(4,4)*tmp11;
2078 B(0,4) = A(3,4)*tmp8 - A(3,3)*( A(2,4)*tmp2 - A(2,2)*( A(0,1)*A(1,4) - A(0,4)*A(1,1) ) );
2079 B(1,4) = A(3,3)*( A(2,4)*tmp3 - A(2,2)*A(0,0)*A(1,4) ) - A(3,4)*tmp9;
2080 B(2,4) = A(3,4)*tmp10 - A(3,3)*A(2,4)*tmp4;
2081 B(3,4) = - A(3,4)*tmp11;
2082 B(4,4) = A(3,3)*tmp11;
2084 const ET
det( A(0,0) * B(0,0) );
2110template<
typename MT
2112inline void invertUniUpper5x5( DenseMatrix<MT,SO>& dm )
2119 using ET = ElementType_t<MT>;
2121 const StaticMatrix<ET,5UL,5UL,SO> A( *dm );
2124 const ET tmp1( A(0,1)*A(1,2) - A(0,2) );
2125 const ET tmp2( A(2,3)*A(1,2) - A(1,3) );
2126 const ET tmp3( A(2,3)*tmp1 - A(0,1)*A(1,3) + A(0,3) );
2129 B(0,2) = A(0,1)*A(1,2) - A(0,2);
2134 B(0,4) = A(3,4)*tmp3 - A(2,4)*tmp1 + A(0,1)*A(1,4) - A(0,4);
2135 B(1,4) = A(2,4)*A(1,2) - A(1,4) - A(3,4)*tmp2;
2136 B(2,4) = A(3,4)*A(2,3) - A(2,4);
2159template<
typename MT
2161inline void invertDiagonal5x5( DenseMatrix<MT,SO>& dm )
2168 using ET = ElementType_t<MT>;
2172 const ET tmp1( A(0,0)*A(1,1) );
2173 const ET tmp2( A(3,3)*A(4,4) );
2174 const ET tmp3( A(0,0)*tmp2 );
2175 const ET tmp4( tmp1*A(2,2) );
2176 const ET tmp5( tmp4*A(3,3) );
2178 const ET
det( tmp2*tmp4 );
2184 const ET idet( ET(1) /
det );
2186 A(0,0) = A(1,1)*A(2,2)*tmp2*idet;
2187 A(1,1) = A(2,2)*tmp3*idet;
2188 A(2,2) = tmp1*tmp2*idet;
2189 A(3,3) = tmp4*A(4,4)*idet;
2222inline void invert5x5( DenseMatrix<MT,SO>& dm )
2230 case byLU : invertGeneral5x5 ( *dm );
break;
2231 case byLDLT : invertSymmetric5x5( *dm );
break;
2232 case byLDLH : invertHermitian5x5( *dm );
break;
2233 case byLLH : invertHermitian5x5( *dm );
break;
2234 case asGeneral : invertGeneral5x5 ( *dm );
break;
2235 case asSymmetric: invertSymmetric5x5( *dm );
break;
2236 case asHermitian: invertHermitian5x5( *dm );
break;
2237 case asLower : invertLower5x5 ( *dm );
break;
2238 case asUniLower : invertUniLower5x5 ( *dm );
break;
2239 case asUpper : invertUpper5x5 ( *dm );
break;
2240 case asUniUpper : invertUniUpper5x5 ( *dm );
break;
2241 case asDiagonal : invertDiagonal5x5 ( *dm );
break;
2276template<
typename MT
2278inline void invertGeneral6x6( DenseMatrix<MT,SO>& dm )
2286 using ET = ElementType_t<MT>;
2288 const StaticMatrix<ET,6UL,6UL,SO> A( *dm );
2291 ET tmp1 ( A(4,4)*A(5,5) - A(4,5)*A(5,4) );
2292 ET tmp2 ( A(4,3)*A(5,5) - A(4,5)*A(5,3) );
2293 ET tmp3 ( A(4,3)*A(5,4) - A(4,4)*A(5,3) );
2294 ET tmp4 ( A(4,2)*A(5,5) - A(4,5)*A(5,2) );
2295 ET tmp5 ( A(4,2)*A(5,4) - A(4,4)*A(5,2) );
2296 ET tmp6 ( A(4,2)*A(5,3) - A(4,3)*A(5,2) );
2297 ET tmp7 ( A(4,1)*A(5,5) - A(4,5)*A(5,1) );
2298 ET tmp8 ( A(4,1)*A(5,4) - A(4,4)*A(5,1) );
2299 ET tmp9 ( A(4,1)*A(5,3) - A(4,3)*A(5,1) );
2300 ET tmp10( A(4,1)*A(5,2) - A(4,2)*A(5,1) );
2301 ET tmp11( A(4,0)*A(5,5) - A(4,5)*A(5,0) );
2302 ET tmp12( A(4,0)*A(5,4) - A(4,4)*A(5,0) );
2303 ET tmp13( A(4,0)*A(5,3) - A(4,3)*A(5,0) );
2304 ET tmp14( A(4,0)*A(5,2) - A(4,2)*A(5,0) );
2305 ET tmp15( A(4,0)*A(5,1) - A(4,1)*A(5,0) );
2307 ET tmp16( A(3,3)*tmp1 - A(3,4)*tmp2 + A(3,5)*tmp3 );
2308 ET tmp17( A(3,2)*tmp1 - A(3,4)*tmp4 + A(3,5)*tmp5 );
2309 ET tmp18( A(3,2)*tmp2 - A(3,3)*tmp4 + A(3,5)*tmp6 );
2310 ET tmp19( A(3,2)*tmp3 - A(3,3)*tmp5 + A(3,4)*tmp6 );
2311 ET tmp20( A(3,1)*tmp1 - A(3,4)*tmp7 + A(3,5)*tmp8 );
2312 ET tmp21( A(3,1)*tmp2 - A(3,3)*tmp7 + A(3,5)*tmp9 );
2313 ET tmp22( A(3,1)*tmp3 - A(3,3)*tmp8 + A(3,4)*tmp9 );
2314 ET tmp23( A(3,1)*tmp4 - A(3,2)*tmp7 + A(3,5)*tmp10 );
2315 ET tmp24( A(3,1)*tmp5 - A(3,2)*tmp8 + A(3,4)*tmp10 );
2316 ET tmp25( A(3,1)*tmp6 - A(3,2)*tmp9 + A(3,3)*tmp10 );
2317 ET tmp26( A(3,0)*tmp1 - A(3,4)*tmp11 + A(3,5)*tmp12 );
2318 ET tmp27( A(3,0)*tmp2 - A(3,3)*tmp11 + A(3,5)*tmp13 );
2319 ET tmp28( A(3,0)*tmp3 - A(3,3)*tmp12 + A(3,4)*tmp13 );
2320 ET tmp29( A(3,0)*tmp4 - A(3,2)*tmp11 + A(3,5)*tmp14 );
2321 ET tmp30( A(3,0)*tmp5 - A(3,2)*tmp12 + A(3,4)*tmp14 );
2322 ET tmp31( A(3,0)*tmp6 - A(3,2)*tmp13 + A(3,3)*tmp14 );
2323 ET tmp32( A(3,0)*tmp7 - A(3,1)*tmp11 + A(3,5)*tmp15 );
2324 ET tmp33( A(3,0)*tmp8 - A(3,1)*tmp12 + A(3,4)*tmp15 );
2325 ET tmp34( A(3,0)*tmp9 - A(3,1)*tmp13 + A(3,3)*tmp15 );
2326 ET tmp35( A(3,0)*tmp10 - A(3,1)*tmp14 + A(3,2)*tmp15 );
2328 ET tmp36( A(2,2)*tmp16 - A(2,3)*tmp17 + A(2,4)*tmp18 - A(2,5)*tmp19 );
2329 ET tmp37( A(2,1)*tmp16 - A(2,3)*tmp20 + A(2,4)*tmp21 - A(2,5)*tmp22 );
2330 ET tmp38( A(2,1)*tmp17 - A(2,2)*tmp20 + A(2,4)*tmp23 - A(2,5)*tmp24 );
2331 ET tmp39( A(2,1)*tmp18 - A(2,2)*tmp21 + A(2,3)*tmp23 - A(2,5)*tmp25 );
2332 ET tmp40( A(2,1)*tmp19 - A(2,2)*tmp22 + A(2,3)*tmp24 - A(2,4)*tmp25 );
2333 ET tmp41( A(2,0)*tmp16 - A(2,3)*tmp26 + A(2,4)*tmp27 - A(2,5)*tmp28 );
2334 ET tmp42( A(2,0)*tmp17 - A(2,2)*tmp26 + A(2,4)*tmp29 - A(2,5)*tmp30 );
2335 ET tmp43( A(2,0)*tmp18 - A(2,2)*tmp27 + A(2,3)*tmp29 - A(2,5)*tmp31 );
2336 ET tmp44( A(2,0)*tmp19 - A(2,2)*tmp28 + A(2,3)*tmp30 - A(2,4)*tmp31 );
2338 B(0,0) = A(1,1)*tmp36 - A(1,2)*tmp37 + A(1,3)*tmp38 - A(1,4)*tmp39 + A(1,5)*tmp40;
2339 B(0,1) = - A(0,1)*tmp36 + A(0,2)*tmp37 - A(0,3)*tmp38 + A(0,4)*tmp39 - A(0,5)*tmp40;
2340 B(1,0) = - A(1,0)*tmp36 + A(1,2)*tmp41 - A(1,3)*tmp42 + A(1,4)*tmp43 - A(1,5)*tmp44;
2341 B(1,1) = A(0,0)*tmp36 - A(0,2)*tmp41 + A(0,3)*tmp42 - A(0,4)*tmp43 + A(0,5)*tmp44;
2343 ET tmp45( A(2,0)*tmp20 - A(2,1)*tmp26 + A(2,4)*tmp32 - A(2,5)*tmp33 );
2344 ET tmp46( A(2,0)*tmp21 - A(2,1)*tmp27 + A(2,3)*tmp32 - A(2,5)*tmp34 );
2345 ET tmp47( A(2,0)*tmp22 - A(2,1)*tmp28 + A(2,3)*tmp33 - A(2,4)*tmp34 );
2346 ET tmp48( A(2,0)*tmp23 - A(2,1)*tmp29 + A(2,2)*tmp32 - A(2,5)*tmp35 );
2347 ET tmp49( A(2,0)*tmp24 - A(2,1)*tmp30 + A(2,2)*tmp33 - A(2,4)*tmp35 );
2349 B(2,0) = A(1,0)*tmp37 - A(1,1)*tmp41 + A(1,3)*tmp45 - A(1,4)*tmp46 + A(1,5)*tmp47;
2350 B(2,1) = - A(0,0)*tmp37 + A(0,1)*tmp41 - A(0,3)*tmp45 + A(0,4)*tmp46 - A(0,5)*tmp47;
2351 B(3,0) = - A(1,0)*tmp38 + A(1,1)*tmp42 - A(1,2)*tmp45 + A(1,4)*tmp48 - A(1,5)*tmp49;
2352 B(3,1) = A(0,0)*tmp38 - A(0,1)*tmp42 + A(0,2)*tmp45 - A(0,4)*tmp48 + A(0,5)*tmp49;
2354 ET tmp50( A(2,0)*tmp25 - A(2,1)*tmp31 + A(2,2)*tmp34 - A(2,3)*tmp35 );
2356 B(4,0) = A(1,0)*tmp39 - A(1,1)*tmp43 + A(1,2)*tmp46 - A(1,3)*tmp48 + A(1,5)*tmp50;
2357 B(4,1) = - A(0,0)*tmp39 + A(0,1)*tmp43 - A(0,2)*tmp46 + A(0,3)*tmp48 - A(0,5)*tmp50;
2358 B(5,0) = - A(1,0)*tmp40 + A(1,1)*tmp44 - A(1,2)*tmp47 + A(1,3)*tmp49 - A(1,4)*tmp50;
2359 B(5,1) = A(0,0)*tmp40 - A(0,1)*tmp44 + A(0,2)*tmp47 - A(0,3)*tmp49 + A(0,4)*tmp50;
2361 tmp36 = A(1,2)*tmp16 - A(1,3)*tmp17 + A(1,4)*tmp18 - A(1,5)*tmp19;
2362 tmp37 = A(1,1)*tmp16 - A(1,3)*tmp20 + A(1,4)*tmp21 - A(1,5)*tmp22;
2363 tmp38 = A(1,1)*tmp17 - A(1,2)*tmp20 + A(1,4)*tmp23 - A(1,5)*tmp24;
2364 tmp39 = A(1,1)*tmp18 - A(1,2)*tmp21 + A(1,3)*tmp23 - A(1,5)*tmp25;
2365 tmp40 = A(1,1)*tmp19 - A(1,2)*tmp22 + A(1,3)*tmp24 - A(1,4)*tmp25;
2366 tmp41 = A(1,0)*tmp16 - A(1,3)*tmp26 + A(1,4)*tmp27 - A(1,5)*tmp28;
2367 tmp42 = A(1,0)*tmp17 - A(1,2)*tmp26 + A(1,4)*tmp29 - A(1,5)*tmp30;
2368 tmp43 = A(1,0)*tmp18 - A(1,2)*tmp27 + A(1,3)*tmp29 - A(1,5)*tmp31;
2369 tmp44 = A(1,0)*tmp19 - A(1,2)*tmp28 + A(1,3)*tmp30 - A(1,4)*tmp31;
2370 tmp45 = A(1,0)*tmp20 - A(1,1)*tmp26 + A(1,4)*tmp32 - A(1,5)*tmp33;
2371 tmp46 = A(1,0)*tmp21 - A(1,1)*tmp27 + A(1,3)*tmp32 - A(1,5)*tmp34;
2372 tmp47 = A(1,0)*tmp22 - A(1,1)*tmp28 + A(1,3)*tmp33 - A(1,4)*tmp34;
2373 tmp48 = A(1,0)*tmp23 - A(1,1)*tmp29 + A(1,2)*tmp32 - A(1,5)*tmp35;
2374 tmp49 = A(1,0)*tmp24 - A(1,1)*tmp30 + A(1,2)*tmp33 - A(1,4)*tmp35;
2375 tmp50 = A(1,0)*tmp25 - A(1,1)*tmp31 + A(1,2)*tmp34 - A(1,3)*tmp35;
2377 B(0,2) = A(0,1)*tmp36 - A(0,2)*tmp37 + A(0,3)*tmp38 - A(0,4)*tmp39 + A(0,5)*tmp40;
2378 B(1,2) = - A(0,0)*tmp36 + A(0,2)*tmp41 - A(0,3)*tmp42 + A(0,4)*tmp43 - A(0,5)*tmp44;
2379 B(2,2) = A(0,0)*tmp37 - A(0,1)*tmp41 + A(0,3)*tmp45 - A(0,4)*tmp46 + A(0,5)*tmp47;
2380 B(3,2) = - A(0,0)*tmp38 + A(0,1)*tmp42 - A(0,2)*tmp45 + A(0,4)*tmp48 - A(0,5)*tmp49;
2381 B(4,2) = A(0,0)*tmp39 - A(0,1)*tmp43 + A(0,2)*tmp46 - A(0,3)*tmp48 + A(0,5)*tmp50;
2382 B(5,2) = - A(0,0)*tmp40 + A(0,1)*tmp44 - A(0,2)*tmp47 + A(0,3)*tmp49 - A(0,4)*tmp50;
2384 tmp1 = A(0,3)*A(1,4) - A(0,4)*A(1,3);
2385 tmp2 = A(0,2)*A(1,4) - A(0,4)*A(1,2);
2386 tmp3 = A(0,2)*A(1,3) - A(0,3)*A(1,2);
2387 tmp4 = A(0,1)*A(1,4) - A(0,4)*A(1,1);
2388 tmp5 = A(0,1)*A(1,3) - A(0,3)*A(1,1);
2389 tmp6 = A(0,1)*A(1,2) - A(0,2)*A(1,1);
2390 tmp7 = A(0,0)*A(1,4) - A(0,4)*A(1,0);
2391 tmp8 = A(0,0)*A(1,3) - A(0,3)*A(1,0);
2392 tmp9 = A(0,0)*A(1,2) - A(0,2)*A(1,0);
2393 tmp10 = A(0,0)*A(1,1) - A(0,1)*A(1,0);
2394 tmp11 = A(0,3)*A(1,5) - A(0,5)*A(1,3);
2395 tmp12 = A(0,2)*A(1,5) - A(0,5)*A(1,2);
2396 tmp13 = A(0,1)*A(1,5) - A(0,5)*A(1,1);
2397 tmp14 = A(0,0)*A(1,5) - A(0,5)*A(1,0);
2398 tmp15 = A(0,4)*A(1,5) - A(0,5)*A(1,4);
2400 tmp16 = A(2,3)*tmp15 - A(2,4)*tmp11 + A(2,5)*tmp1;
2401 tmp17 = A(2,2)*tmp15 - A(2,4)*tmp12 + A(2,5)*tmp2;
2402 tmp18 = A(2,2)*tmp11 - A(2,3)*tmp12 + A(2,5)*tmp3;
2403 tmp19 = A(2,2)*tmp1 - A(2,3)*tmp2 + A(2,4)*tmp3;
2404 tmp20 = A(2,1)*tmp15 - A(2,4)*tmp13 + A(2,5)*tmp4;
2405 tmp21 = A(2,1)*tmp11 - A(2,3)*tmp13 + A(2,5)*tmp5;
2406 tmp22 = A(2,1)*tmp1 - A(2,3)*tmp4 + A(2,4)*tmp5;
2407 tmp23 = A(2,1)*tmp12 - A(2,2)*tmp13 + A(2,5)*tmp6;
2408 tmp24 = A(2,1)*tmp2 - A(2,2)*tmp4 + A(2,4)*tmp6;
2409 tmp25 = A(2,1)*tmp3 - A(2,2)*tmp5 + A(2,3)*tmp6;
2410 tmp26 = A(2,0)*tmp15 - A(2,4)*tmp14 + A(2,5)*tmp7;
2411 tmp27 = A(2,0)*tmp11 - A(2,3)*tmp14 + A(2,5)*tmp8;
2412 tmp28 = A(2,0)*tmp1 - A(2,3)*tmp7 + A(2,4)*tmp8;
2413 tmp29 = A(2,0)*tmp12 - A(2,2)*tmp14 + A(2,5)*tmp9;
2414 tmp30 = A(2,0)*tmp2 - A(2,2)*tmp7 + A(2,4)*tmp9;
2415 tmp31 = A(2,0)*tmp3 - A(2,2)*tmp8 + A(2,3)*tmp9;
2416 tmp32 = A(2,0)*tmp13 - A(2,1)*tmp14 + A(2,5)*tmp10;
2417 tmp33 = A(2,0)*tmp4 - A(2,1)*tmp7 + A(2,4)*tmp10;
2418 tmp34 = A(2,0)*tmp5 - A(2,1)*tmp8 + A(2,3)*tmp10;
2419 tmp35 = A(2,0)*tmp6 - A(2,1)*tmp9 + A(2,2)*tmp10;
2421 tmp36 = A(3,2)*tmp16 - A(3,3)*tmp17 + A(3,4)*tmp18 - A(3,5)*tmp19;
2422 tmp37 = A(3,1)*tmp16 - A(3,3)*tmp20 + A(3,4)*tmp21 - A(3,5)*tmp22;
2423 tmp38 = A(3,1)*tmp17 - A(3,2)*tmp20 + A(3,4)*tmp23 - A(3,5)*tmp24;
2424 tmp39 = A(3,1)*tmp18 - A(3,2)*tmp21 + A(3,3)*tmp23 - A(3,5)*tmp25;
2425 tmp40 = A(3,1)*tmp19 - A(3,2)*tmp22 + A(3,3)*tmp24 - A(3,4)*tmp25;
2426 tmp41 = A(3,0)*tmp16 - A(3,3)*tmp26 + A(3,4)*tmp27 - A(3,5)*tmp28;
2427 tmp42 = A(3,0)*tmp17 - A(3,2)*tmp26 + A(3,4)*tmp29 - A(3,5)*tmp30;
2428 tmp43 = A(3,0)*tmp18 - A(3,2)*tmp27 + A(3,3)*tmp29 - A(3,5)*tmp31;
2429 tmp44 = A(3,0)*tmp19 - A(3,2)*tmp28 + A(3,3)*tmp30 - A(3,4)*tmp31;
2431 B(0,4) = - A(5,1)*tmp36 + A(5,2)*tmp37 - A(5,3)*tmp38 + A(5,4)*tmp39 - A(5,5)*tmp40;
2432 B(0,5) = A(4,1)*tmp36 - A(4,2)*tmp37 + A(4,3)*tmp38 - A(4,4)*tmp39 + A(4,5)*tmp40;
2433 B(1,4) = A(5,0)*tmp36 - A(5,2)*tmp41 + A(5,3)*tmp42 - A(5,4)*tmp43 + A(5,5)*tmp44;
2434 B(1,5) = - A(4,0)*tmp36 + A(4,2)*tmp41 - A(4,3)*tmp42 + A(4,4)*tmp43 - A(4,5)*tmp44;
2436 tmp45 = A(3,0)*tmp20 - A(3,1)*tmp26 + A(3,4)*tmp32 - A(3,5)*tmp33;
2437 tmp46 = A(3,0)*tmp21 - A(3,1)*tmp27 + A(3,3)*tmp32 - A(3,5)*tmp34;
2438 tmp47 = A(3,0)*tmp22 - A(3,1)*tmp28 + A(3,3)*tmp33 - A(3,4)*tmp34;
2439 tmp48 = A(3,0)*tmp23 - A(3,1)*tmp29 + A(3,2)*tmp32 - A(3,5)*tmp35;
2440 tmp49 = A(3,0)*tmp24 - A(3,1)*tmp30 + A(3,2)*tmp33 - A(3,4)*tmp35;
2442 B(2,4) = - A(5,0)*tmp37 + A(5,1)*tmp41 - A(5,3)*tmp45 + A(5,4)*tmp46 - A(5,5)*tmp47;
2443 B(2,5) = A(4,0)*tmp37 - A(4,1)*tmp41 + A(4,3)*tmp45 - A(4,4)*tmp46 + A(4,5)*tmp47;
2444 B(3,4) = A(5,0)*tmp38 - A(5,1)*tmp42 + A(5,2)*tmp45 - A(5,4)*tmp48 + A(5,5)*tmp49;
2445 B(3,5) = - A(4,0)*tmp38 + A(4,1)*tmp42 - A(4,2)*tmp45 + A(4,4)*tmp48 - A(4,5)*tmp49;
2447 tmp50 = A(3,0)*tmp25 - A(3,1)*tmp31 + A(3,2)*tmp34 - A(3,3)*tmp35;
2449 B(4,4) = - A(5,0)*tmp39 + A(5,1)*tmp43 - A(5,2)*tmp46 + A(5,3)*tmp48 - A(5,5)*tmp50;
2450 B(4,5) = A(4,0)*tmp39 - A(4,1)*tmp43 + A(4,2)*tmp46 - A(4,3)*tmp48 + A(4,5)*tmp50;
2451 B(5,4) = A(5,0)*tmp40 - A(5,1)*tmp44 + A(5,2)*tmp47 - A(5,3)*tmp49 + A(5,4)*tmp50;
2452 B(5,5) = - A(4,0)*tmp40 + A(4,1)*tmp44 - A(4,2)*tmp47 + A(4,3)*tmp49 - A(4,4)*tmp50;
2454 tmp36 = A(4,2)*tmp16 - A(4,3)*tmp17 + A(4,4)*tmp18 - A(4,5)*tmp19;
2455 tmp37 = A(4,1)*tmp16 - A(4,3)*tmp20 + A(4,4)*tmp21 - A(4,5)*tmp22;
2456 tmp38 = A(4,1)*tmp17 - A(4,2)*tmp20 + A(4,4)*tmp23 - A(4,5)*tmp24;
2457 tmp39 = A(4,1)*tmp18 - A(4,2)*tmp21 + A(4,3)*tmp23 - A(4,5)*tmp25;
2458 tmp40 = A(4,1)*tmp19 - A(4,2)*tmp22 + A(4,3)*tmp24 - A(4,4)*tmp25;
2459 tmp41 = A(4,0)*tmp16 - A(4,3)*tmp26 + A(4,4)*tmp27 - A(4,5)*tmp28;
2460 tmp42 = A(4,0)*tmp17 - A(4,2)*tmp26 + A(4,4)*tmp29 - A(4,5)*tmp30;
2461 tmp43 = A(4,0)*tmp18 - A(4,2)*tmp27 + A(4,3)*tmp29 - A(4,5)*tmp31;
2462 tmp44 = A(4,0)*tmp19 - A(4,2)*tmp28 + A(4,3)*tmp30 - A(4,4)*tmp31;
2463 tmp45 = A(4,0)*tmp20 - A(4,1)*tmp26 + A(4,4)*tmp32 - A(4,5)*tmp33;
2464 tmp46 = A(4,0)*tmp21 - A(4,1)*tmp27 + A(4,3)*tmp32 - A(4,5)*tmp34;
2465 tmp47 = A(4,0)*tmp22 - A(4,1)*tmp28 + A(4,3)*tmp33 - A(4,4)*tmp34;
2466 tmp48 = A(4,0)*tmp23 - A(4,1)*tmp29 + A(4,2)*tmp32 - A(4,5)*tmp35;
2467 tmp49 = A(4,0)*tmp24 - A(4,1)*tmp30 + A(4,2)*tmp33 - A(4,4)*tmp35;
2468 tmp50 = A(4,0)*tmp25 - A(4,1)*tmp31 + A(4,2)*tmp34 - A(4,3)*tmp35;
2470 B(0,3) = A(5,1)*tmp36 - A(5,2)*tmp37 + A(5,3)*tmp38 - A(5,4)*tmp39 + A(5,5)*tmp40;
2471 B(1,3) = - A(5,0)*tmp36 + A(5,2)*tmp41 - A(5,3)*tmp42 + A(5,4)*tmp43 - A(5,5)*tmp44;
2472 B(2,3) = A(5,0)*tmp37 - A(5,1)*tmp41 + A(5,3)*tmp45 - A(5,4)*tmp46 + A(5,5)*tmp47;
2473 B(3,3) = - A(5,0)*tmp38 + A(5,1)*tmp42 - A(5,2)*tmp45 + A(5,4)*tmp48 - A(5,5)*tmp49;
2474 B(4,3) = A(5,0)*tmp39 - A(5,1)*tmp43 + A(5,2)*tmp46 - A(5,3)*tmp48 + A(5,5)*tmp50;
2475 B(5,3) = - A(5,0)*tmp40 + A(5,1)*tmp44 - A(5,2)*tmp47 + A(5,3)*tmp49 - A(5,4)*tmp50;
2477 const ET
det( A(0,0)*B(0,0) + A(0,1)*B(1,0) + A(0,2)*B(2,0) +
2478 A(0,3)*B(3,0) + A(0,4)*B(4,0) + A(0,5)*B(5,0) );
2507template<
typename MT
2509inline void invertSymmetric6x6( DenseMatrix<MT,SO>& dm )
2516 using ET = ElementType_t<MT>;
2518 const StaticMatrix<ET,6UL,6UL,SO> A( *dm );
2521 ET tmp1 ( A(4,4)*A(5,5) - A(4,5)*A(5,4) );
2522 ET tmp2 ( A(4,3)*A(5,5) - A(4,5)*A(5,3) );
2523 ET tmp3 ( A(4,3)*A(5,4) - A(4,4)*A(5,3) );
2524 ET tmp4 ( A(4,2)*A(5,5) - A(4,5)*A(5,2) );
2525 ET tmp5 ( A(4,2)*A(5,4) - A(4,4)*A(5,2) );
2526 ET tmp6 ( A(4,2)*A(5,3) - A(4,3)*A(5,2) );
2527 ET tmp7 ( A(4,1)*A(5,5) - A(4,5)*A(5,1) );
2528 ET tmp8 ( A(4,1)*A(5,4) - A(4,4)*A(5,1) );
2529 ET tmp9 ( A(4,1)*A(5,3) - A(4,3)*A(5,1) );
2530 ET tmp10( A(4,1)*A(5,2) - A(4,2)*A(5,1) );
2531 ET tmp11( A(4,0)*A(5,5) - A(4,5)*A(5,0) );
2532 ET tmp12( A(4,0)*A(5,4) - A(4,4)*A(5,0) );
2533 ET tmp13( A(4,0)*A(5,3) - A(4,3)*A(5,0) );
2534 ET tmp14( A(4,0)*A(5,2) - A(4,2)*A(5,0) );
2535 ET tmp15( A(4,0)*A(5,1) - A(4,1)*A(5,0) );
2537 ET tmp16( A(3,3)*tmp1 - A(3,4)*tmp2 + A(3,5)*tmp3 );
2538 ET tmp17( A(3,2)*tmp1 - A(3,4)*tmp4 + A(3,5)*tmp5 );
2539 ET tmp18( A(3,2)*tmp2 - A(3,3)*tmp4 + A(3,5)*tmp6 );
2540 ET tmp19( A(3,2)*tmp3 - A(3,3)*tmp5 + A(3,4)*tmp6 );
2541 ET tmp20( A(3,1)*tmp1 - A(3,4)*tmp7 + A(3,5)*tmp8 );
2542 ET tmp21( A(3,1)*tmp2 - A(3,3)*tmp7 + A(3,5)*tmp9 );
2543 ET tmp22( A(3,1)*tmp3 - A(3,3)*tmp8 + A(3,4)*tmp9 );
2544 ET tmp23( A(3,1)*tmp4 - A(3,2)*tmp7 + A(3,5)*tmp10 );
2545 ET tmp24( A(3,1)*tmp5 - A(3,2)*tmp8 + A(3,4)*tmp10 );
2546 ET tmp25( A(3,1)*tmp6 - A(3,2)*tmp9 + A(3,3)*tmp10 );
2547 ET tmp26( A(3,0)*tmp1 - A(3,4)*tmp11 + A(3,5)*tmp12 );
2548 ET tmp27( A(3,0)*tmp2 - A(3,3)*tmp11 + A(3,5)*tmp13 );
2549 ET tmp28( A(3,0)*tmp3 - A(3,3)*tmp12 + A(3,4)*tmp13 );
2550 ET tmp29( A(3,0)*tmp4 - A(3,2)*tmp11 + A(3,5)*tmp14 );
2551 ET tmp30( A(3,0)*tmp5 - A(3,2)*tmp12 + A(3,4)*tmp14 );
2552 ET tmp31( A(3,0)*tmp6 - A(3,2)*tmp13 + A(3,3)*tmp14 );
2553 ET tmp32( A(3,0)*tmp7 - A(3,1)*tmp11 + A(3,5)*tmp15 );
2554 ET tmp33( A(3,0)*tmp8 - A(3,1)*tmp12 + A(3,4)*tmp15 );
2555 ET tmp34( A(3,0)*tmp9 - A(3,1)*tmp13 + A(3,3)*tmp15 );
2556 ET tmp35( A(3,0)*tmp10 - A(3,1)*tmp14 + A(3,2)*tmp15 );
2558 ET tmp36( A(2,2)*tmp16 - A(2,3)*tmp17 + A(2,4)*tmp18 - A(2,5)*tmp19 );
2559 ET tmp37( A(2,1)*tmp16 - A(2,3)*tmp20 + A(2,4)*tmp21 - A(2,5)*tmp22 );
2560 ET tmp38( A(2,1)*tmp17 - A(2,2)*tmp20 + A(2,4)*tmp23 - A(2,5)*tmp24 );
2561 ET tmp39( A(2,1)*tmp18 - A(2,2)*tmp21 + A(2,3)*tmp23 - A(2,5)*tmp25 );
2562 ET tmp40( A(2,1)*tmp19 - A(2,2)*tmp22 + A(2,3)*tmp24 - A(2,4)*tmp25 );
2563 ET tmp41( A(2,0)*tmp16 - A(2,3)*tmp26 + A(2,4)*tmp27 - A(2,5)*tmp28 );
2564 ET tmp42( A(2,0)*tmp17 - A(2,2)*tmp26 + A(2,4)*tmp29 - A(2,5)*tmp30 );
2565 ET tmp43( A(2,0)*tmp18 - A(2,2)*tmp27 + A(2,3)*tmp29 - A(2,5)*tmp31 );
2566 ET tmp44( A(2,0)*tmp19 - A(2,2)*tmp28 + A(2,3)*tmp30 - A(2,4)*tmp31 );
2568 B(0,0) = A(1,1)*tmp36 - A(1,2)*tmp37 + A(1,3)*tmp38 - A(1,4)*tmp39 + A(1,5)*tmp40;
2569 B(0,1) = - A(0,1)*tmp36 + A(0,2)*tmp37 - A(0,3)*tmp38 + A(0,4)*tmp39 - A(0,5)*tmp40;
2570 B(1,1) = A(0,0)*tmp36 - A(0,2)*tmp41 + A(0,3)*tmp42 - A(0,4)*tmp43 + A(0,5)*tmp44;
2572 ET tmp45( A(2,0)*tmp20 - A(2,1)*tmp26 + A(2,4)*tmp32 - A(2,5)*tmp33 );
2573 ET tmp46( A(2,0)*tmp21 - A(2,1)*tmp27 + A(2,3)*tmp32 - A(2,5)*tmp34 );
2574 ET tmp47( A(2,0)*tmp22 - A(2,1)*tmp28 + A(2,3)*tmp33 - A(2,4)*tmp34 );
2575 ET tmp48( A(2,0)*tmp23 - A(2,1)*tmp29 + A(2,2)*tmp32 - A(2,5)*tmp35 );
2576 ET tmp49( A(2,0)*tmp24 - A(2,1)*tmp30 + A(2,2)*tmp33 - A(2,4)*tmp35 );
2578 B(2,0) = A(1,0)*tmp37 - A(1,1)*tmp41 + A(1,3)*tmp45 - A(1,4)*tmp46 + A(1,5)*tmp47;
2579 B(2,1) = - A(0,0)*tmp37 + A(0,1)*tmp41 - A(0,3)*tmp45 + A(0,4)*tmp46 - A(0,5)*tmp47;
2580 B(3,0) = - A(1,0)*tmp38 + A(1,1)*tmp42 - A(1,2)*tmp45 + A(1,4)*tmp48 - A(1,5)*tmp49;
2581 B(3,1) = A(0,0)*tmp38 - A(0,1)*tmp42 + A(0,2)*tmp45 - A(0,4)*tmp48 + A(0,5)*tmp49;
2583 ET tmp50( A(2,0)*tmp25 - A(2,1)*tmp31 + A(2,2)*tmp34 - A(2,3)*tmp35 );
2585 B(4,0) = A(1,0)*tmp39 - A(1,1)*tmp43 + A(1,2)*tmp46 - A(1,3)*tmp48 + A(1,5)*tmp50;
2586 B(4,1) = - A(0,0)*tmp39 + A(0,1)*tmp43 - A(0,2)*tmp46 + A(0,3)*tmp48 - A(0,5)*tmp50;
2587 B(5,0) = - A(1,0)*tmp40 + A(1,1)*tmp44 - A(1,2)*tmp47 + A(1,3)*tmp49 - A(1,4)*tmp50;
2588 B(5,1) = A(0,0)*tmp40 - A(0,1)*tmp44 + A(0,2)*tmp47 - A(0,3)*tmp49 + A(0,4)*tmp50;
2590 tmp36 = A(1,1)*tmp16 - A(1,3)*tmp20 + A(1,4)*tmp21 - A(1,5)*tmp22;
2591 tmp37 = A(1,1)*tmp17 - A(1,2)*tmp20 + A(1,4)*tmp23 - A(1,5)*tmp24;
2592 tmp38 = A(1,0)*tmp16 - A(1,3)*tmp26 + A(1,4)*tmp27 - A(1,5)*tmp28;
2593 tmp39 = A(1,0)*tmp17 - A(1,2)*tmp26 + A(1,4)*tmp29 - A(1,5)*tmp30;
2594 tmp40 = A(1,0)*tmp20 - A(1,1)*tmp26 + A(1,4)*tmp32 - A(1,5)*tmp33;
2595 tmp41 = A(1,0)*tmp21 - A(1,1)*tmp27 + A(1,3)*tmp32 - A(1,5)*tmp34;
2596 tmp42 = A(1,0)*tmp22 - A(1,1)*tmp28 + A(1,3)*tmp33 - A(1,4)*tmp34;
2597 tmp43 = A(1,0)*tmp23 - A(1,1)*tmp29 + A(1,2)*tmp32 - A(1,5)*tmp35;
2598 tmp44 = A(1,0)*tmp24 - A(1,1)*tmp30 + A(1,2)*tmp33 - A(1,4)*tmp35;
2600 B(2,2) = A(0,0)*tmp36 - A(0,1)*tmp38 + A(0,3)*tmp40 - A(0,4)*tmp41 + A(0,5)*tmp42;
2601 B(3,2) = - A(0,0)*tmp37 + A(0,1)*tmp39 - A(0,2)*tmp40 + A(0,4)*tmp43 - A(0,5)*tmp44;
2603 tmp1 = A(0,3)*A(1,4) - A(0,4)*A(1,3);
2604 tmp2 = A(0,2)*A(1,4) - A(0,4)*A(1,2);
2605 tmp3 = A(0,2)*A(1,3) - A(0,3)*A(1,2);
2606 tmp4 = A(0,1)*A(1,4) - A(0,4)*A(1,1);
2607 tmp5 = A(0,1)*A(1,3) - A(0,3)*A(1,1);
2608 tmp6 = A(0,1)*A(1,2) - A(0,2)*A(1,1);
2609 tmp7 = A(0,0)*A(1,4) - A(0,4)*A(1,0);
2610 tmp8 = A(0,0)*A(1,3) - A(0,3)*A(1,0);
2611 tmp9 = A(0,0)*A(1,2) - A(0,2)*A(1,0);
2612 tmp10 = A(0,0)*A(1,1) - A(0,1)*A(1,0);
2613 tmp11 = A(0,3)*A(1,5) - A(0,5)*A(1,3);
2614 tmp12 = A(0,2)*A(1,5) - A(0,5)*A(1,2);
2615 tmp13 = A(0,1)*A(1,5) - A(0,5)*A(1,1);
2616 tmp14 = A(0,0)*A(1,5) - A(0,5)*A(1,0);
2617 tmp15 = A(0,4)*A(1,5) - A(0,5)*A(1,4);
2619 tmp16 = A(2,3)*tmp15 - A(2,4)*tmp11 + A(2,5)*tmp1;
2620 tmp17 = A(2,2)*tmp15 - A(2,4)*tmp12 + A(2,5)*tmp2;
2621 tmp18 = A(2,2)*tmp11 - A(2,3)*tmp12 + A(2,5)*tmp3;
2622 tmp19 = A(2,2)*tmp1 - A(2,3)*tmp2 + A(2,4)*tmp3;
2623 tmp20 = A(2,1)*tmp15 - A(2,4)*tmp13 + A(2,5)*tmp4;
2624 tmp21 = A(2,1)*tmp11 - A(2,3)*tmp13 + A(2,5)*tmp5;
2625 tmp22 = A(2,1)*tmp1 - A(2,3)*tmp4 + A(2,4)*tmp5;
2626 tmp23 = A(2,1)*tmp12 - A(2,2)*tmp13 + A(2,5)*tmp6;
2627 tmp24 = A(2,1)*tmp2 - A(2,2)*tmp4 + A(2,4)*tmp6;
2628 tmp25 = A(2,1)*tmp3 - A(2,2)*tmp5 + A(2,3)*tmp6;
2629 tmp26 = A(2,0)*tmp15 - A(2,4)*tmp14 + A(2,5)*tmp7;
2630 tmp27 = A(2,0)*tmp11 - A(2,3)*tmp14 + A(2,5)*tmp8;
2631 tmp28 = A(2,0)*tmp1 - A(2,3)*tmp7 + A(2,4)*tmp8;
2632 tmp29 = A(2,0)*tmp12 - A(2,2)*tmp14 + A(2,5)*tmp9;
2633 tmp30 = A(2,0)*tmp2 - A(2,2)*tmp7 + A(2,4)*tmp9;
2634 tmp31 = A(2,0)*tmp3 - A(2,2)*tmp8 + A(2,3)*tmp9;
2635 tmp32 = A(2,0)*tmp13 - A(2,1)*tmp14 + A(2,5)*tmp10;
2636 tmp33 = A(2,0)*tmp4 - A(2,1)*tmp7 + A(2,4)*tmp10;
2637 tmp34 = A(2,0)*tmp5 - A(2,1)*tmp8 + A(2,3)*tmp10;
2638 tmp35 = A(2,0)*tmp6 - A(2,1)*tmp9 + A(2,2)*tmp10;
2640 tmp36 = A(3,1)*tmp16 - A(3,3)*tmp20 + A(3,4)*tmp21 - A(3,5)*tmp22;
2641 tmp37 = A(3,1)*tmp17 - A(3,2)*tmp20 + A(3,4)*tmp23 - A(3,5)*tmp24;
2642 tmp38 = A(3,0)*tmp16 - A(3,3)*tmp26 + A(3,4)*tmp27 - A(3,5)*tmp28;
2643 tmp39 = A(3,0)*tmp17 - A(3,2)*tmp26 + A(3,4)*tmp29 - A(3,5)*tmp30;
2644 tmp40 = A(3,0)*tmp20 - A(3,1)*tmp26 + A(3,4)*tmp32 - A(3,5)*tmp33;
2645 tmp41 = A(3,0)*tmp21 - A(3,1)*tmp27 + A(3,3)*tmp32 - A(3,5)*tmp34;
2646 tmp42 = A(3,0)*tmp22 - A(3,1)*tmp28 + A(3,3)*tmp33 - A(3,4)*tmp34;
2647 tmp43 = A(3,0)*tmp23 - A(3,1)*tmp29 + A(3,2)*tmp32 - A(3,5)*tmp35;
2648 tmp44 = A(3,0)*tmp24 - A(3,1)*tmp30 + A(3,2)*tmp33 - A(3,4)*tmp35;
2650 B(2,4) = - A(5,0)*tmp36 + A(5,1)*tmp38 - A(5,3)*tmp40 + A(5,4)*tmp41 - A(5,5)*tmp42;
2651 B(2,5) = A(4,0)*tmp36 - A(4,1)*tmp38 + A(4,3)*tmp40 - A(4,4)*tmp41 + A(4,5)*tmp42;
2652 B(3,4) = A(5,0)*tmp37 - A(5,1)*tmp39 + A(5,2)*tmp40 - A(5,4)*tmp43 + A(5,5)*tmp44;
2653 B(3,5) = - A(4,0)*tmp37 + A(4,1)*tmp39 - A(4,2)*tmp40 + A(4,4)*tmp43 - A(4,5)*tmp44;
2655 tmp36 = A(3,1)*tmp18 - A(3,2)*tmp21 + A(3,3)*tmp23 - A(3,5)*tmp25;
2656 tmp37 = A(3,1)*tmp19 - A(3,2)*tmp22 + A(3,3)*tmp24 - A(3,4)*tmp25;
2657 tmp38 = A(3,0)*tmp18 - A(3,2)*tmp27 + A(3,3)*tmp29 - A(3,5)*tmp31;
2658 tmp39 = A(3,0)*tmp19 - A(3,2)*tmp28 + A(3,3)*tmp30 - A(3,4)*tmp31;
2659 tmp40 = A(3,0)*tmp25 - A(3,1)*tmp31 + A(3,2)*tmp34 - A(3,3)*tmp35;
2661 B(4,4) = - A(5,0)*tmp36 + A(5,1)*tmp38 - A(5,2)*tmp41 + A(5,3)*tmp43 - A(5,5)*tmp40;
2662 B(4,5) = A(4,0)*tmp36 - A(4,1)*tmp38 + A(4,2)*tmp41 - A(4,3)*tmp43 + A(4,5)*tmp40;
2663 B(5,5) = - A(4,0)*tmp37 + A(4,1)*tmp39 - A(4,2)*tmp42 + A(4,3)*tmp44 - A(4,4)*tmp40;
2665 tmp36 = A(4,1)*tmp17 - A(4,2)*tmp20 + A(4,4)*tmp23 - A(4,5)*tmp24;
2666 tmp37 = A(4,0)*tmp17 - A(4,2)*tmp26 + A(4,4)*tmp29 - A(4,5)*tmp30;
2667 tmp38 = A(4,0)*tmp20 - A(4,1)*tmp26 + A(4,4)*tmp32 - A(4,5)*tmp33;
2668 tmp39 = A(4,0)*tmp23 - A(4,1)*tmp29 + A(4,2)*tmp32 - A(4,5)*tmp35;
2669 tmp40 = A(4,0)*tmp24 - A(4,1)*tmp30 + A(4,2)*tmp33 - A(4,4)*tmp35;
2671 B(3,3) = - A(5,0)*tmp36 + A(5,1)*tmp37 - A(5,2)*tmp38 + A(5,4)*tmp39 - A(5,5)*tmp40;
2689 const ET
det( A(0,0)*B(0,0) + A(0,1)*B(1,0) + A(0,2)*B(2,0) +
2690 A(0,3)*B(3,0) + A(0,4)*B(4,0) + A(0,5)*B(5,0) );
2718template<
typename MT
2720inline void invertHermitian6x6( DenseMatrix<MT,SO>& dm )
2727 using ET = ElementType_t<MT>;
2729 const StaticMatrix<ET,6UL,6UL,SO> A( *dm );
2732 ET tmp1 ( A(4,4)*A(5,5) - A(4,5)*A(5,4) );
2733 ET tmp2 ( A(4,3)*A(5,5) - A(4,5)*A(5,3) );
2734 ET tmp3 ( A(4,3)*A(5,4) - A(4,4)*A(5,3) );
2735 ET tmp4 ( A(4,2)*A(5,5) - A(4,5)*A(5,2) );
2736 ET tmp5 ( A(4,2)*A(5,4) - A(4,4)*A(5,2) );
2737 ET tmp6 ( A(4,2)*A(5,3) - A(4,3)*A(5,2) );
2738 ET tmp7 ( A(4,1)*A(5,5) - A(4,5)*A(5,1) );
2739 ET tmp8 ( A(4,1)*A(5,4) - A(4,4)*A(5,1) );
2740 ET tmp9 ( A(4,1)*A(5,3) - A(4,3)*A(5,1) );
2741 ET tmp10( A(4,1)*A(5,2) - A(4,2)*A(5,1) );
2742 ET tmp11( A(4,0)*A(5,5) - A(4,5)*A(5,0) );
2743 ET tmp12( A(4,0)*A(5,4) - A(4,4)*A(5,0) );
2744 ET tmp13( A(4,0)*A(5,3) - A(4,3)*A(5,0) );
2745 ET tmp14( A(4,0)*A(5,2) - A(4,2)*A(5,0) );
2746 ET tmp15( A(4,0)*A(5,1) - A(4,1)*A(5,0) );
2748 ET tmp16( A(3,3)*tmp1 - A(3,4)*tmp2 + A(3,5)*tmp3 );
2749 ET tmp17( A(3,2)*tmp1 - A(3,4)*tmp4 + A(3,5)*tmp5 );
2750 ET tmp18( A(3,2)*tmp2 - A(3,3)*tmp4 + A(3,5)*tmp6 );
2751 ET tmp19( A(3,2)*tmp3 - A(3,3)*tmp5 + A(3,4)*tmp6 );
2752 ET tmp20( A(3,1)*tmp1 - A(3,4)*tmp7 + A(3,5)*tmp8 );
2753 ET tmp21( A(3,1)*tmp2 - A(3,3)*tmp7 + A(3,5)*tmp9 );
2754 ET tmp22( A(3,1)*tmp3 - A(3,3)*tmp8 + A(3,4)*tmp9 );
2755 ET tmp23( A(3,1)*tmp4 - A(3,2)*tmp7 + A(3,5)*tmp10 );
2756 ET tmp24( A(3,1)*tmp5 - A(3,2)*tmp8 + A(3,4)*tmp10 );
2757 ET tmp25( A(3,1)*tmp6 - A(3,2)*tmp9 + A(3,3)*tmp10 );
2758 ET tmp26( A(3,0)*tmp1 - A(3,4)*tmp11 + A(3,5)*tmp12 );
2759 ET tmp27( A(3,0)*tmp2 - A(3,3)*tmp11 + A(3,5)*tmp13 );
2760 ET tmp28( A(3,0)*tmp3 - A(3,3)*tmp12 + A(3,4)*tmp13 );
2761 ET tmp29( A(3,0)*tmp4 - A(3,2)*tmp11 + A(3,5)*tmp14 );
2762 ET tmp30( A(3,0)*tmp5 - A(3,2)*tmp12 + A(3,4)*tmp14 );
2763 ET tmp31( A(3,0)*tmp6 - A(3,2)*tmp13 + A(3,3)*tmp14 );
2764 ET tmp32( A(3,0)*tmp7 - A(3,1)*tmp11 + A(3,5)*tmp15 );
2765 ET tmp33( A(3,0)*tmp8 - A(3,1)*tmp12 + A(3,4)*tmp15 );
2766 ET tmp34( A(3,0)*tmp9 - A(3,1)*tmp13 + A(3,3)*tmp15 );
2767 ET tmp35( A(3,0)*tmp10 - A(3,1)*tmp14 + A(3,2)*tmp15 );
2769 ET tmp36( A(2,2)*tmp16 - A(2,3)*tmp17 + A(2,4)*tmp18 - A(2,5)*tmp19 );
2770 ET tmp37( A(2,1)*tmp16 - A(2,3)*tmp20 + A(2,4)*tmp21 - A(2,5)*tmp22 );
2771 ET tmp38( A(2,1)*tmp17 - A(2,2)*tmp20 + A(2,4)*tmp23 - A(2,5)*tmp24 );
2772 ET tmp39( A(2,1)*tmp18 - A(2,2)*tmp21 + A(2,3)*tmp23 - A(2,5)*tmp25 );
2773 ET tmp40( A(2,1)*tmp19 - A(2,2)*tmp22 + A(2,3)*tmp24 - A(2,4)*tmp25 );
2774 ET tmp41( A(2,0)*tmp16 - A(2,3)*tmp26 + A(2,4)*tmp27 - A(2,5)*tmp28 );
2775 ET tmp42( A(2,0)*tmp17 - A(2,2)*tmp26 + A(2,4)*tmp29 - A(2,5)*tmp30 );
2776 ET tmp43( A(2,0)*tmp18 - A(2,2)*tmp27 + A(2,3)*tmp29 - A(2,5)*tmp31 );
2777 ET tmp44( A(2,0)*tmp19 - A(2,2)*tmp28 + A(2,3)*tmp30 - A(2,4)*tmp31 );
2779 B(0,0) = ET(
real( A(1,1)*tmp36 - A(1,2)*tmp37 + A(1,3)*tmp38 - A(1,4)*tmp39 + A(1,5)*tmp40 ) );
2780 B(0,1) = - A(0,1)*tmp36 + A(0,2)*tmp37 - A(0,3)*tmp38 + A(0,4)*tmp39 - A(0,5)*tmp40;
2781 B(1,1) = ET(
real( A(0,0)*tmp36 - A(0,2)*tmp41 + A(0,3)*tmp42 - A(0,4)*tmp43 + A(0,5)*tmp44 ) );
2783 ET tmp45( A(2,0)*tmp20 - A(2,1)*tmp26 + A(2,4)*tmp32 - A(2,5)*tmp33 );
2784 ET tmp46( A(2,0)*tmp21 - A(2,1)*tmp27 + A(2,3)*tmp32 - A(2,5)*tmp34 );
2785 ET tmp47( A(2,0)*tmp22 - A(2,1)*tmp28 + A(2,3)*tmp33 - A(2,4)*tmp34 );
2786 ET tmp48( A(2,0)*tmp23 - A(2,1)*tmp29 + A(2,2)*tmp32 - A(2,5)*tmp35 );
2787 ET tmp49( A(2,0)*tmp24 - A(2,1)*tmp30 + A(2,2)*tmp33 - A(2,4)*tmp35 );
2789 B(2,0) = A(1,0)*tmp37 - A(1,1)*tmp41 + A(1,3)*tmp45 - A(1,4)*tmp46 + A(1,5)*tmp47;
2790 B(2,1) = - A(0,0)*tmp37 + A(0,1)*tmp41 - A(0,3)*tmp45 + A(0,4)*tmp46 - A(0,5)*tmp47;
2791 B(3,0) = - A(1,0)*tmp38 + A(1,1)*tmp42 - A(1,2)*tmp45 + A(1,4)*tmp48 - A(1,5)*tmp49;
2792 B(3,1) = A(0,0)*tmp38 - A(0,1)*tmp42 + A(0,2)*tmp45 - A(0,4)*tmp48 + A(0,5)*tmp49;
2794 ET tmp50( A(2,0)*tmp25 - A(2,1)*tmp31 + A(2,2)*tmp34 - A(2,3)*tmp35 );
2796 B(4,0) = A(1,0)*tmp39 - A(1,1)*tmp43 + A(1,2)*tmp46 - A(1,3)*tmp48 + A(1,5)*tmp50;
2797 B(4,1) = - A(0,0)*tmp39 + A(0,1)*tmp43 - A(0,2)*tmp46 + A(0,3)*tmp48 - A(0,5)*tmp50;
2798 B(5,0) = - A(1,0)*tmp40 + A(1,1)*tmp44 - A(1,2)*tmp47 + A(1,3)*tmp49 - A(1,4)*tmp50;
2799 B(5,1) = A(0,0)*tmp40 - A(0,1)*tmp44 + A(0,2)*tmp47 - A(0,3)*tmp49 + A(0,4)*tmp50;
2801 tmp36 = A(1,1)*tmp16 - A(1,3)*tmp20 + A(1,4)*tmp21 - A(1,5)*tmp22;
2802 tmp37 = A(1,1)*tmp17 - A(1,2)*tmp20 + A(1,4)*tmp23 - A(1,5)*tmp24;
2803 tmp38 = A(1,0)*tmp16 - A(1,3)*tmp26 + A(1,4)*tmp27 - A(1,5)*tmp28;
2804 tmp39 = A(1,0)*tmp17 - A(1,2)*tmp26 + A(1,4)*tmp29 - A(1,5)*tmp30;
2805 tmp40 = A(1,0)*tmp20 - A(1,1)*tmp26 + A(1,4)*tmp32 - A(1,5)*tmp33;
2806 tmp41 = A(1,0)*tmp21 - A(1,1)*tmp27 + A(1,3)*tmp32 - A(1,5)*tmp34;
2807 tmp42 = A(1,0)*tmp22 - A(1,1)*tmp28 + A(1,3)*tmp33 - A(1,4)*tmp34;
2808 tmp43 = A(1,0)*tmp23 - A(1,1)*tmp29 + A(1,2)*tmp32 - A(1,5)*tmp35;
2809 tmp44 = A(1,0)*tmp24 - A(1,1)*tmp30 + A(1,2)*tmp33 - A(1,4)*tmp35;
2811 B(2,2) = ET(
real( A(0,0)*tmp36 - A(0,1)*tmp38 + A(0,3)*tmp40 - A(0,4)*tmp41 + A(0,5)*tmp42 ) );
2812 B(3,2) = - A(0,0)*tmp37 + A(0,1)*tmp39 - A(0,2)*tmp40 + A(0,4)*tmp43 - A(0,5)*tmp44;
2814 tmp1 = A(0,3)*A(1,4) - A(0,4)*A(1,3);
2815 tmp2 = A(0,2)*A(1,4) - A(0,4)*A(1,2);
2816 tmp3 = A(0,2)*A(1,3) - A(0,3)*A(1,2);
2817 tmp4 = A(0,1)*A(1,4) - A(0,4)*A(1,1);
2818 tmp5 = A(0,1)*A(1,3) - A(0,3)*A(1,1);
2819 tmp6 = A(0,1)*A(1,2) - A(0,2)*A(1,1);
2820 tmp7 = A(0,0)*A(1,4) - A(0,4)*A(1,0);
2821 tmp8 = A(0,0)*A(1,3) - A(0,3)*A(1,0);
2822 tmp9 = A(0,0)*A(1,2) - A(0,2)*A(1,0);
2823 tmp10 = A(0,0)*A(1,1) - A(0,1)*A(1,0);
2824 tmp11 = A(0,3)*A(1,5) - A(0,5)*A(1,3);
2825 tmp12 = A(0,2)*A(1,5) - A(0,5)*A(1,2);
2826 tmp13 = A(0,1)*A(1,5) - A(0,5)*A(1,1);
2827 tmp14 = A(0,0)*A(1,5) - A(0,5)*A(1,0);
2828 tmp15 = A(0,4)*A(1,5) - A(0,5)*A(1,4);
2830 tmp16 = A(2,3)*tmp15 - A(2,4)*tmp11 + A(2,5)*tmp1;
2831 tmp17 = A(2,2)*tmp15 - A(2,4)*tmp12 + A(2,5)*tmp2;
2832 tmp18 = A(2,2)*tmp11 - A(2,3)*tmp12 + A(2,5)*tmp3;
2833 tmp19 = A(2,2)*tmp1 - A(2,3)*tmp2 + A(2,4)*tmp3;
2834 tmp20 = A(2,1)*tmp15 - A(2,4)*tmp13 + A(2,5)*tmp4;
2835 tmp21 = A(2,1)*tmp11 - A(2,3)*tmp13 + A(2,5)*tmp5;
2836 tmp22 = A(2,1)*tmp1 - A(2,3)*tmp4 + A(2,4)*tmp5;
2837 tmp23 = A(2,1)*tmp12 - A(2,2)*tmp13 + A(2,5)*tmp6;
2838 tmp24 = A(2,1)*tmp2 - A(2,2)*tmp4 + A(2,4)*tmp6;
2839 tmp25 = A(2,1)*tmp3 - A(2,2)*tmp5 + A(2,3)*tmp6;
2840 tmp26 = A(2,0)*tmp15 - A(2,4)*tmp14 + A(2,5)*tmp7;
2841 tmp27 = A(2,0)*tmp11 - A(2,3)*tmp14 + A(2,5)*tmp8;
2842 tmp28 = A(2,0)*tmp1 - A(2,3)*tmp7 + A(2,4)*tmp8;
2843 tmp29 = A(2,0)*tmp12 - A(2,2)*tmp14 + A(2,5)*tmp9;
2844 tmp30 = A(2,0)*tmp2 - A(2,2)*tmp7 + A(2,4)*tmp9;
2845 tmp31 = A(2,0)*tmp3 - A(2,2)*tmp8 + A(2,3)*tmp9;
2846 tmp32 = A(2,0)*tmp13 - A(2,1)*tmp14 + A(2,5)*tmp10;
2847 tmp33 = A(2,0)*tmp4 - A(2,1)*tmp7 + A(2,4)*tmp10;
2848 tmp34 = A(2,0)*tmp5 - A(2,1)*tmp8 + A(2,3)*tmp10;
2849 tmp35 = A(2,0)*tmp6 - A(2,1)*tmp9 + A(2,2)*tmp10;
2851 tmp36 = A(3,1)*tmp16 - A(3,3)*tmp20 + A(3,4)*tmp21 - A(3,5)*tmp22;
2852 tmp37 = A(3,1)*tmp17 - A(3,2)*tmp20 + A(3,4)*tmp23 - A(3,5)*tmp24;
2853 tmp38 = A(3,0)*tmp16 - A(3,3)*tmp26 + A(3,4)*tmp27 - A(3,5)*tmp28;
2854 tmp39 = A(3,0)*tmp17 - A(3,2)*tmp26 + A(3,4)*tmp29 - A(3,5)*tmp30;
2855 tmp40 = A(3,0)*tmp20 - A(3,1)*tmp26 + A(3,4)*tmp32 - A(3,5)*tmp33;
2856 tmp41 = A(3,0)*tmp21 - A(3,1)*tmp27 + A(3,3)*tmp32 - A(3,5)*tmp34;
2857 tmp42 = A(3,0)*tmp22 - A(3,1)*tmp28 + A(3,3)*tmp33 - A(3,4)*tmp34;
2858 tmp43 = A(3,0)*tmp23 - A(3,1)*tmp29 + A(3,2)*tmp32 - A(3,5)*tmp35;
2859 tmp44 = A(3,0)*tmp24 - A(3,1)*tmp30 + A(3,2)*tmp33 - A(3,4)*tmp35;
2861 B(2,4) = - A(5,0)*tmp36 + A(5,1)*tmp38 - A(5,3)*tmp40 + A(5,4)*tmp41 - A(5,5)*tmp42;
2862 B(2,5) = A(4,0)*tmp36 - A(4,1)*tmp38 + A(4,3)*tmp40 - A(4,4)*tmp41 + A(4,5)*tmp42;
2863 B(3,4) = A(5,0)*tmp37 - A(5,1)*tmp39 + A(5,2)*tmp40 - A(5,4)*tmp43 + A(5,5)*tmp44;
2864 B(3,5) = - A(4,0)*tmp37 + A(4,1)*tmp39 - A(4,2)*tmp40 + A(4,4)*tmp43 - A(4,5)*tmp44;
2866 tmp36 = A(3,1)*tmp18 - A(3,2)*tmp21 + A(3,3)*tmp23 - A(3,5)*tmp25;
2867 tmp37 = A(3,1)*tmp19 - A(3,2)*tmp22 + A(3,3)*tmp24 - A(3,4)*tmp25;
2868 tmp38 = A(3,0)*tmp18 - A(3,2)*tmp27 + A(3,3)*tmp29 - A(3,5)*tmp31;
2869 tmp39 = A(3,0)*tmp19 - A(3,2)*tmp28 + A(3,3)*tmp30 - A(3,4)*tmp31;
2870 tmp40 = A(3,0)*tmp25 - A(3,1)*tmp31 + A(3,2)*tmp34 - A(3,3)*tmp35;
2872 B(4,4) = - ET(
real( A(5,0)*tmp36 - A(5,1)*tmp38 + A(5,2)*tmp41 - A(5,3)*tmp43 + A(5,5)*tmp40 ) );
2873 B(4,5) = A(4,0)*tmp36 - A(4,1)*tmp38 + A(4,2)*tmp41 - A(4,3)*tmp43 + A(4,5)*tmp40;
2874 B(5,5) = - ET(
real( A(4,0)*tmp37 - A(4,1)*tmp39 + A(4,2)*tmp42 - A(4,3)*tmp44 + A(4,4)*tmp40 ) );
2876 tmp36 = A(4,1)*tmp17 - A(4,2)*tmp20 + A(4,4)*tmp23 - A(4,5)*tmp24;
2877 tmp37 = A(4,0)*tmp17 - A(4,2)*tmp26 + A(4,4)*tmp29 - A(4,5)*tmp30;
2878 tmp38 = A(4,0)*tmp20 - A(4,1)*tmp26 + A(4,4)*tmp32 - A(4,5)*tmp33;
2879 tmp39 = A(4,0)*tmp23 - A(4,1)*tmp29 + A(4,2)*tmp32 - A(4,5)*tmp35;
2880 tmp40 = A(4,0)*tmp24 - A(4,1)*tmp30 + A(4,2)*tmp33 - A(4,4)*tmp35;
2882 B(3,3) = - ET(
real( A(5,0)*tmp36 - A(5,1)*tmp37 + A(5,2)*tmp38 - A(5,4)*tmp39 + A(5,5)*tmp40 ) );
2884 B(0,2) =
conj( B(2,0) );
2885 B(0,3) =
conj( B(3,0) );
2886 B(0,4) =
conj( B(4,0) );
2887 B(0,5) =
conj( B(5,0) );
2888 B(1,0) =
conj( B(0,1) );
2889 B(1,2) =
conj( B(2,1) );
2890 B(1,3) =
conj( B(3,1) );
2891 B(1,4) =
conj( B(4,1) );
2892 B(1,5) =
conj( B(5,1) );
2893 B(2,3) =
conj( B(3,2) );
2894 B(4,2) =
conj( B(2,4) );
2895 B(4,3) =
conj( B(3,4) );
2896 B(5,2) =
conj( B(2,5) );
2897 B(5,3) =
conj( B(3,5) );
2898 B(5,4) =
conj( B(4,5) );
2900 const ET
det(
real( A(0,0)*B(0,0) + A(0,1)*B(1,0) + A(0,2)*B(2,0) +
2901 A(0,3)*B(3,0) + A(0,4)*B(4,0) + A(0,5)*B(5,0) ) );
2929template<
typename MT
2931inline void invertLower6x6( DenseMatrix<MT,SO>& dm )
2938 using ET = ElementType_t<MT>;
2940 const StaticMatrix<ET,6UL,6UL,SO> A( *dm );
2943 const ET tmp1( A(4,4)*A(5,5) );
2944 const ET tmp2( A(4,3)*A(5,5) );
2945 const ET tmp3( A(4,3)*A(5,4) - A(4,4)*A(5,3) );
2947 const ET tmp4( A(3,3)*tmp1 );
2948 const ET tmp5( A(3,2)*tmp1 );
2949 const ET tmp6( A(3,2)*tmp2 - A(3,3)*A(4,2)*A(5,5) );
2950 const ET tmp7( A(3,2)*tmp3 - A(3,3)*( A(4,2)*A(5,4) - A(4,4)*A(5,2) ) );
2951 const ET tmp8( A(0,0)*A(1,1)*A(2,2) );
2953 const ET tmp9 ( A(2,2)*tmp4 );
2954 const ET tmp10( A(2,1)*tmp4 );
2955 const ET tmp11( A(2,1)*tmp5 - A(2,2)*A(3,1)*tmp1 );
2956 const ET tmp12( A(2,1)*tmp6 - A(2,2)*( A(3,1)*tmp2 - A(3,3)*A(4,1)*A(5,5) ) );
2957 const ET tmp13( A(2,1)*tmp7 - A(2,2)*( A(3,1)*tmp3 - A(3,3)*( A(4,1)*A(5,4) - A(4,4)*A(5,1) ) ) );
2958 const ET tmp14( A(4,4)*tmp8 );
2959 const ET tmp15( A(4,3)*tmp8 );
2960 const ET tmp16( A(3,3)*tmp8 );
2962 B(0,0) = A(1,1)*tmp9;
2963 B(1,0) = - A(1,0)*tmp9;
2964 B(2,0) = A(1,0)*tmp10 - A(1,1)*A(2,0)*tmp4;
2965 B(3,0) = - A(1,0)*tmp11 + A(1,1)*( A(2,0)*tmp5 - A(2,2)*A(3,0)*tmp1 );
2966 B(4,0) = A(1,0)*tmp12 - A(1,1)*( A(2,0)*tmp6 - A(2,2)*( A(3,0)*tmp2 - A(3,3)*A(4,0)*A(5,5) ) );
2967 B(5,0) = - A(1,0)*tmp13 + A(1,1)*( A(2,0)*tmp7 - A(2,2)*( A(3,0)*tmp3 - A(3,3)*( A(4,0)*A(5,4) - A(4,4)*A(5,0) ) ) );
2968 B(1,1) = A(0,0)*tmp9;
2969 B(2,1) = - A(0,0)*tmp10;
2970 B(3,1) = A(0,0)*tmp11;
2971 B(4,1) = - A(0,0)*tmp12;
2972 B(5,1) = A(0,0)*tmp13;
2973 B(2,2) = A(0,0)*A(1,1)*tmp4;
2974 B(3,2) = - A(0,0)*A(1,1)*tmp5;
2975 B(4,2) = A(0,0)*A(1,1)*tmp6;
2976 B(5,2) = - A(0,0)*A(1,1)*tmp7;
2977 B(3,3) = A(5,5)*tmp14;
2978 B(4,3) = - A(5,5)*tmp15;
2979 B(5,3) = A(5,4)*tmp15 - A(5,3)*tmp14;
2980 B(4,4) = A(5,5)*tmp16 - A(5,3)*A(3,5)*tmp8;
2981 B(5,4) = - A(5,4)*tmp16;
2982 B(5,5) = A(4,4)*tmp16;
2984 const ET
det( B(5,5)*A(5,5) );
3010template<
typename MT
3012inline void invertUniLower6x6( DenseMatrix<MT,SO>& dm )
3019 using ET = ElementType_t<MT>;
3021 const StaticMatrix<ET,6UL,6UL,SO> A( *dm );
3024 const ET tmp1( A(4,3)*A(5,4) - A(5,3) );
3025 const ET tmp2( A(3,2)*A(4,3) - A(4,2) );
3026 const ET tmp3( A(3,2)*tmp1 - A(4,2)*A(5,4) + A(5,2) );
3027 const ET tmp4( A(2,1)*A(3,2) - A(3,1) );
3028 const ET tmp5( A(2,1)*tmp2 - A(3,1)*A(4,3) + A(4,1) );
3029 const ET tmp6( A(2,1)*tmp3 - A(3,1)*tmp1 + A(4,1)*A(5,4) - A(5,1) );
3032 B(2,0) = A(1,0)*A(2,1) - A(2,0);
3033 B(3,0) = - A(1,0)*tmp4 + A(2,0)*A(3,2) - A(3,0);
3034 B(4,0) = A(1,0)*tmp5 - A(2,0)*tmp2 + A(3,0)*A(4,3) - A(4,0);
3035 B(5,0) = - A(1,0)*tmp6 + A(2,0)*tmp3 - A(3,0)*tmp1 + A(4,0)*A(5,4) - A(5,0);
3043 B(4,3) = A(5,3)*A(4,5) - A(4,3);
3044 B(5,3) = A(5,4)*A(4,3) - A(5,3);
3045 B(5,4) = A(5,3)*A(3,4) - A(5,4);
3067template<
typename MT
3069inline void invertUpper6x6( DenseMatrix<MT,SO>& dm )
3076 using ET = ElementType_t<MT>;
3078 const StaticMatrix<ET,6UL,6UL,SO> A( *dm );
3081 const ET tmp1( A(0,1)*A(1,2) - A(0,2)*A(1,1) );
3082 const ET tmp2( A(0,0)*A(1,2) );
3083 const ET tmp3( A(0,0)*A(1,1) );
3085 const ET tmp4( A(3,3)*A(4,4)*A(5,5) );
3086 const ET tmp5( A(2,3)*tmp1 - A(2,2)*( A(0,1)*A(1,3) - A(0,3)*A(1,1) ) );
3087 const ET tmp6( A(2,3)*tmp2 - A(0,0)*A(1,3)*A(2,2) );
3088 const ET tmp7( A(2,3)*tmp3 );
3089 const ET tmp8( A(2,2)*tmp3 );
3091 const ET tmp9 ( A(2,2)*tmp4 );
3092 const ET tmp10( A(1,2)*tmp4 );
3093 const ET tmp11( A(1,1)*tmp4 );
3094 const ET tmp12( A(3,3)*( A(2,4)*tmp1 - A(2,2)*( A(0,1)*A(1,4) - A(0,4)*A(1,1) ) ) - A(3,4)*tmp5 );
3095 const ET tmp13( A(3,3)*( A(2,4)*tmp2 - A(0,0)*A(1,4)*A(2,2) ) - A(3,4)*tmp6 );
3096 const ET tmp14( A(3,3)*A(2,4)*tmp3 - A(3,4)*tmp7 );
3097 const ET tmp15( - A(3,4)*tmp8 );
3098 const ET tmp16( - A(3,3)*tmp8 );
3100 B(0,0) = A(1,1)*tmp9;
3101 B(0,1) = - A(0,1)*tmp9;
3102 B(1,1) = A(0,0)*tmp9;
3103 B(0,2) = A(0,1)*tmp10 - A(0,2)*tmp11;
3104 B(1,2) = - A(0,0)*tmp10;
3105 B(2,2) = A(0,0)*tmp11;
3106 B(0,3) = - A(5,5)*A(4,4)*tmp5;
3107 B(1,3) = A(5,5)*A(4,4)*tmp6;
3108 B(2,3) = - A(5,5)*A(4,4)*tmp7;
3109 B(3,3) = A(5,5)*A(4,4)*tmp8;
3110 B(0,4) = - A(5,5)*tmp12;
3111 B(1,4) = A(5,5)*tmp13;
3112 B(2,4) = - A(5,5)*tmp14;
3113 B(3,4) = A(5,5)*tmp15;
3114 B(4,4) = - A(5,5)*tmp16;
3115 B(0,5) = - A(4,4)*( A(3,3)*( A(2,5)*tmp1 - A(2,2)*( A(0,1)*A(1,5) - A(0,5)*A(1,1) ) ) - A(3,5)*tmp5 ) + A(4,5)*tmp12;
3116 B(1,5) = A(4,4)*( A(3,3)*( A(2,5)*tmp2 - A(0,0)*A(1,5)*A(2,2) ) - A(3,5)*tmp6 ) - A(4,5)*tmp13;
3117 B(2,5) = - A(4,4)*( A(3,3)*A(2,5)*tmp3 - A(3,5)*tmp7 ) + A(4,5)*tmp14;
3118 B(3,5) = - A(4,4)*A(3,5)*tmp8 - A(4,5)*tmp15;
3119 B(4,5) = A(4,5)*tmp16;
3120 B(5,5) = - A(4,4)*tmp16;
3122 const ET
det( A(0,0)*B(0,0) );
3148template<
typename MT
3150inline void invertUniUpper6x6( DenseMatrix<MT,SO>& dm )
3157 using ET = ElementType_t<MT>;
3159 const StaticMatrix<ET,6UL,6UL,SO> A( *dm );
3162 const ET tmp1( A(0,1)*A(1,2) - A(0,2) );
3163 const ET tmp2( A(2,3)*tmp1 - A(0,1)*A(1,3) + A(0,3) );
3164 const ET tmp3( A(2,3)*A(1,2) - A(1,3) );
3165 const ET tmp4( A(2,4)*tmp1 - A(0,1)*A(1,4) + A(0,4) - A(3,4)*tmp2 );
3166 const ET tmp5( A(2,4)*A(1,2) - A(1,4) - A(3,4)*tmp3 );
3167 const ET tmp6( A(2,4) - A(3,4)*A(2,3) );
3170 B(0,2) = A(0,1)*A(1,2) - A(0,2);
3179 B(0,5) = - A(2,5)*tmp1 + A(0,1)*A(1,5) - A(0,5) + A(3,5)*tmp2 + A(4,5)*tmp4;
3180 B(1,5) = A(2,5)*A(1,2) - A(1,5) - A(3,5)*tmp3 - A(4,5)*tmp5;
3181 B(2,5) = - A(2,5) + A(3,5)*A(2,3) + A(4,5)*tmp6;
3182 B(3,5) = - A(3,5) + A(4,5)*A(3,4);
3205template<
typename MT
3207inline void invertDiagonal6x6( DenseMatrix<MT,SO>& dm )
3214 using ET = ElementType_t<MT>;
3218 const ET tmp1( A(0,0)*A(1,1) );
3219 const ET tmp2( A(3,3)*A(4,4) );
3220 const ET tmp3( tmp1*A(2,2) );
3221 const ET tmp4( tmp2*A(5,5) );
3222 const ET tmp5( A(0,0)*tmp4 );
3223 const ET tmp6( tmp3*A(3,3) );
3225 const ET
det( tmp3*tmp4 );
3231 const ET idet( ET(1) /
det );
3233 A(0,0) = A(1,1)*A(2,2)*tmp4*idet;
3234 A(1,1) = tmp5*A(2,2)*idet;
3235 A(2,2) = tmp1*tmp4*idet;
3236 A(3,3) = tmp3*A(4,4)*A(5,5)*idet;
3237 A(4,4) = tmp6*A(5,5)*idet;
3238 A(5,5) = tmp2*tmp3*idet;
3270inline void invert6x6( DenseMatrix<MT,SO>& dm )
3278 case byLU : invertGeneral6x6 ( *dm );
break;
3279 case byLDLT : invertSymmetric6x6( *dm );
break;
3280 case byLDLH : invertHermitian6x6( *dm );
break;
3281 case byLLH : invertHermitian6x6( *dm );
break;
3282 case asGeneral : invertGeneral6x6 ( *dm );
break;
3283 case asSymmetric: invertSymmetric6x6( *dm );
break;
3284 case asHermitian: invertHermitian6x6( *dm );
break;
3285 case asLower : invertLower6x6 ( *dm );
break;
3286 case asUniLower : invertUniLower6x6 ( *dm );
break;
3287 case asUpper : invertUpper6x6 ( *dm );
break;
3288 case asUniUpper : invertUniUpper6x6 ( *dm );
break;
3289 case asDiagonal : invertDiagonal6x6 ( *dm );
break;
3335template<
typename MT
3337inline void invertByLU( DenseMatrix<MT,SO>& dm )
3342 const size_t n(
min( (*dm).rows(), (*dm).columns() ) );
3343 const std::unique_ptr<blas_int_t[]> ipiv(
new blas_int_t[n] );
3345 getrf( *dm, ipiv.get() );
3346 getri( *dm, ipiv.get() );
3380template<
typename MT
3382inline void invertByLDLT( DenseMatrix<MT,SO>& dm )
3389 const char uplo( ( SO )?(
'L' ):(
'U' ) );
3390 const std::unique_ptr<blas_int_t[]> ipiv(
new blas_int_t[(*dm).rows()] );
3392 sytrf( *dm, uplo, ipiv.get() );
3393 sytri( *dm, uplo, ipiv.get() );
3396 for(
size_t i=1UL; i<(*dm).rows(); ++i ) {
3397 for(
size_t j=0UL; j<i; ++j ) {
3398 (*dm)(j,i) = (*dm)(i,j);
3403 for(
size_t j=1UL; j<(*dm).columns(); ++j ) {
3404 for(
size_t i=0UL; i<j; ++i ) {
3405 (*dm)(j,i) = (*dm)(i,j);
3442template<
typename MT
3444inline auto invertByLDLH( DenseMatrix<MT,SO>& dm )
3445 -> EnableIf_t< IsBuiltin_v< ElementType_t<MT> > >
3447 invertByLDLT( *dm );
3481template<
typename MT
3483inline auto invertByLDLH( DenseMatrix<MT,SO>& dm )
3484 -> EnableIf_t< IsComplex_v< ElementType_t<MT> > >
3491 const char uplo( ( SO )?(
'L' ):(
'U' ) );
3492 const std::unique_ptr<blas_int_t[]> ipiv(
new blas_int_t[(*dm).rows()] );
3494 hetrf( *dm, uplo, ipiv.get() );
3495 hetri( *dm, uplo, ipiv.get() );
3498 for(
size_t i=1UL; i<(*dm).rows(); ++i ) {
3499 for(
size_t j=0UL; j<i; ++j ) {
3500 (*dm)(j,i) =
conj( (*dm)(i,j) );
3505 for(
size_t j=1UL; j<(*dm).columns(); ++j ) {
3506 for(
size_t i=0UL; i<j; ++i ) {
3507 (*dm)(j,i) =
conj( (*dm)(i,j) );
3544template<
typename MT
3546inline void invertByLLH( DenseMatrix<MT,SO>& dm )
3553 const char uplo( ( SO )?(
'L' ):(
'U' ) );
3559 for(
size_t i=1UL; i<(*dm).rows(); ++i ) {
3560 for(
size_t j=0UL; j<i; ++j ) {
3561 (*dm)(j,i) =
conj( (*dm)(i,j) );
3566 for(
size_t j=1UL; j<(*dm).columns(); ++j ) {
3567 for(
size_t i=0UL; i<j; ++i ) {
3568 (*dm)(j,i) =
conj( (*dm)(i,j) );
3605template<
typename MT
3607inline void invertLowerNxN( DenseMatrix<MT,SO>& dm )
3612 trtri( *dm,
'L',
'N' );
3646template<
typename MT
3648inline void invertUniLowerNxN( DenseMatrix<MT,SO>& dm )
3653 trtri( *dm,
'L',
'U' );
3687template<
typename MT
3689inline void invertUpperNxN( DenseMatrix<MT,SO>& dm )
3694 trtri( *dm,
'U',
'N' );
3728template<
typename MT
3730inline void invertUniUpperNxN( DenseMatrix<MT,SO>& dm )
3735 trtri( *dm,
'U',
'U' );
3769template<
typename MT
3771inline void invertDiagonalNxN( DenseMatrix<MT,SO>& dm )
3776 for(
size_t i=0UL; i<(*dm).rows(); ++i )
3815inline void invertNxN( DenseMatrix<MT,SO>& dm )
3823 case byLU : invertByLU ( *dm );
break;
3824 case byLDLT : invertByLDLT ( *dm );
break;
3825 case byLDLH : invertByLDLH ( *dm );
break;
3826 case byLLH : invertByLLH ( *dm );
break;
3827 case asGeneral : invertByLU ( *dm );
break;
3830 case asLower : invertLowerNxN ( *dm );
break;
3831 case asUniLower : invertUniLowerNxN( *dm );
break;
3832 case asUpper : invertUpperNxN ( *dm );
break;
3833 case asUniUpper : invertUniUpperNxN( *dm );
break;
3834 case asDiagonal : invertDiagonalNxN( *dm );
break;
3855template<
typename MT,
bool SO >
3856inline void invert( DenseMatrix<MT,SO>& dm );
3858template< InversionFlag IF,
typename MT,
bool SO >
3859inline void invert( DenseMatrix<MT,SO>& dm );
3890template<
typename MT
3894 invert< getInversionFlag<MT>() >( *dm );
3950 switch( (*dm).rows() ) {
3952 case 1UL:
invert( (*dm)(0,0) );
break;
3953 case 2UL: invert2x2<IF>( *dm );
break;
3954 case 3UL: invert3x3<IF>( *dm );
break;
3955 case 4UL: invert4x4<IF>( *dm );
break;
3956 case 5UL: invert5x5<IF>( *dm );
break;
3957 case 6UL: invert6x6<IF>( *dm );
break;
3958 default : invertNxN<IF>( *dm );
break;
Constraint on the data type.
Header file for auxiliary alias declarations.
typename T::ElementType ElementType_t
Alias declaration for nested ElementType type definitions.
Definition: Aliases.h:190
Header file for run time assertion macros.
Constraint on the data type.
Header file for the conjugate shim.
Header file for the EnableIf class template.
Header file for the dense matrix inversion flags.
Header file for the invert shim.
Header file for the IsBuiltin type trait.
Header file for the IsComplex type trait.
Header file for the IsDiagonal type trait.
Header file for the isDivisor shim.
Header file for the IsHermitian type trait.
Header file for the IsLower type trait.
Header file for the IsSymmetric type trait.
Header file for the IsUniLower type trait.
Header file for the IsUniUpper type trait.
Header file for the IsUpper type trait.
Constraint on the data type.
Base class for dense matrices.
Definition: DenseMatrix.h:82
Header file for the implementation of a fixed-size matrix.
Header file for the DenseMatrix base class.
Header file for the LAPACK LU decomposition functions (getrf)
Header file for the LAPACK LU-based matrix inversion functionality (getri)
decltype(auto) min(const DenseMatrix< MT1, SO1 > &lhs, const DenseMatrix< MT2, SO2 > &rhs)
Computes the componentwise minimum of the dense matrices lhs and rhs.
Definition: DMatDMatMapExpr.h:1339
decltype(auto) real(const DenseMatrix< MT, SO > &dm)
Returns a matrix containing the real part of each single element of dm.
Definition: DMatMapExpr.h:1529
void invert(DenseMatrix< MT, SO > &dm)
In-place inversion of the given dense matrix.
Definition: Inversion.h:3940
decltype(auto) conj(const DenseMatrix< MT, SO > &dm)
Returns a matrix containing the complex conjugate of each single element of dm.
Definition: DMatMapExpr.h:1464
bool isHermitian(const DenseMatrix< MT, SO > &dm)
Checks if the given dense matrix is Hermitian.
Definition: DenseMatrix.h:1534
ElementType_t< MT > det(const DenseMatrix< MT, SO > &dm)
Computation of the determinant of the given dense square matrix.
Definition: DMatDetExpr.h:384
bool isSymmetric(const DenseMatrix< MT, SO > &dm)
Checks if the given dense matrix is symmetric.
Definition: DenseMatrix.h:1456
bool isDivisor(const DenseVector< VT, TF > &dv)
Returns whether the given dense vector is a valid divisor.
Definition: DenseVector.h:1261
bool isIntact(const DiagonalMatrix< MT, SO, DF > &m)
Returns whether the invariants of the given diagonal matrix are intact.
Definition: DiagonalMatrix.h:207
void sytrf(char uplo, blas_int_t n, float *A, blas_int_t lda, blas_int_t *ipiv, float *work, blas_int_t lwork, blas_int_t *info)
LAPACK kernel for the decomposition of the given dense symmetric indefinite single precision column-m...
Definition: sytrf.h:155
void hetrf(char uplo, blas_int_t n, complex< float > *A, blas_int_t lda, blas_int_t *ipiv, complex< float > *work, blas_int_t lwork, blas_int_t *info)
LAPACK kernel for the decomposition of the given dense Hermitian indefinite single precision complex ...
Definition: hetrf.h:143
void potrf(char uplo, blas_int_t n, float *A, blas_int_t lda, blas_int_t *info)
LAPACK kernel for the Cholesky decomposition of the given dense positive definite single precision co...
Definition: potrf.h:137
void getrf(blas_int_t m, blas_int_t n, float *A, blas_int_t lda, blas_int_t *ipiv, blas_int_t *info)
LAPACK kernel for the LU decomposition of the given dense general single precision column-major matri...
Definition: getrf.h:140
void trtri(char uplo, char diag, blas_int_t n, float *A, blas_int_t lda, blas_int_t *info)
LAPACK kernel for the inversion of the given dense triangular single precision column-major matrix.
Definition: trtri.h:134
void hetri(char uplo, blas_int_t n, complex< float > *A, blas_int_t lda, const blas_int_t *ipiv, complex< float > *work, blas_int_t *info)
LAPACK kernel for the inversion of the given dense Hermitian indefinite single precision complex colu...
Definition: hetri.h:127
void potri(char uplo, blas_int_t n, float *A, blas_int_t lda, blas_int_t *info)
LAPACK kernel for the inversion of the given dense positive definite single precision column-major sq...
Definition: potri.h:130
void sytri(char uplo, blas_int_t n, float *A, blas_int_t lda, const blas_int_t *ipiv, float *work, blas_int_t *info)
LAPACK kernel for the inversion of the given dense symmetric indefinite single precision column-major...
Definition: sytri.h:139
void getri(blas_int_t n, float *A, blas_int_t lda, const blas_int_t *ipiv, float *work, blas_int_t lwork, blas_int_t *info)
LAPACK kernel for the inversion of the given dense general single precision column-major square matri...
Definition: getri.h:138
#define BLAZE_CONSTRAINT_MUST_BE_BLAS_COMPATIBLE_TYPE(T)
Constraint on the data type.
Definition: BLASCompatible.h:61
#define BLAZE_CONSTRAINT_MUST_NOT_BE_STRICTLY_TRIANGULAR_MATRIX_TYPE(T)
Constraint on the data type.
Definition: StrictlyTriangular.h:81
#define BLAZE_CONSTRAINT_MUST_NOT_BE_UNIFORM_TYPE(T)
Constraint on the data type.
Definition: Uniform.h:81
#define BLAZE_CONSTRAINT_MUST_NOT_BE_ADAPTOR_TYPE(T)
Constraint on the data type.
Definition: Adaptor.h:81
#define BLAZE_THROW_DIVISION_BY_ZERO(MESSAGE)
Macro for the emission of an exception on detection of a division by zero.
Definition: Exception.h:97
InversionFlag
Inversion flag.
Definition: InversionFlag.h:102
int32_t blas_int_t
Signed integer type used in the BLAS/LAPACK wrapper functions.
Definition: Types.h:64
@ byLDLT
Flag for the Bunch-Kaufman-based inversion for symmetric matrices.
Definition: InversionFlag.h:104
@ asHermitian
Flag for the inversion of a Hermitian matrix (same as byLDLH).
Definition: InversionFlag.h:110
@ asUniLower
Flag for the inversion of a lower unitriangular matrix.
Definition: InversionFlag.h:112
@ asLower
Flag for the inversion of a lower triangular matrix.
Definition: InversionFlag.h:111
@ asUpper
Flag for the inversion of a upper triangular matrix.
Definition: InversionFlag.h:113
@ asSymmetric
Flag for the inversion of a symmetric matrix (same as byLDLT).
Definition: InversionFlag.h:109
@ asUniUpper
Flag for the inversion of a upper unitriangular matrix.
Definition: InversionFlag.h:114
@ byLDLH
Flag for the Bunch-Kaufman-based inversion for Hermitian matrices.
Definition: InversionFlag.h:105
@ asDiagonal
Flag for the inversion of a diagonal matrix.
Definition: InversionFlag.h:115
@ byLU
Flag for the LU-based matrix inversion.
Definition: InversionFlag.h:103
@ byLLH
Flag for the Cholesky-based inversion for positive-definite matrices.
Definition: InversionFlag.h:106
@ asGeneral
Flag for the inversion of a general matrix (same as byLU).
Definition: InversionFlag.h:108
bool isSquare(const Matrix< MT, SO > &matrix) noexcept
Checks if the given matrix is a square matrix.
Definition: Matrix.h:1383
#define BLAZE_INTERNAL_ASSERT(expr, msg)
Run time assertion macro for internal checks.
Definition: Assert.h:101
#define BLAZE_USER_ASSERT(expr, msg)
Run time assertion macro for user checks.
Definition: Assert.h:117
#define BLAZE_THROW_INVALID_ARGUMENT(MESSAGE)
Macro for the emission of a std::invalid_argument exception.
Definition: Exception.h:235
Header file for the LAPACK Hermitian matrix decomposition functionality (hetrf)
Header file for the LAPACK Hermitian matrix inversion functionality (hetri)
Header file for the exception macros of the math module.
Header file for the LAPACK Cholesky decomposition functions (potrf)
Header file for the LAPACK matrix Cholesky-based inversion functions (potri)
Header file for the real shim.
Header file for the LAPACK symmetric matrix decomposition functions (sytrf)
Header file for the LAPACK symmetric matrix inversion functions (sytri)
Header file for the LAPACK triangular matrix inversion functions (trtri)
Header file for basic type definitions.
Header file for the generic min algorithm.