10 #ifndef MI_MATH_MATRIX_H
11 #define MI_MATH_MATRIX_H
88 template <
typename T, Size ROW, Size COL>
278 template <
typename T,
class Matrix,
bool specialized>
279 struct Matrix_struct_get_base_pointer
282 static inline const T* get_base_ptr(
const Matrix& m) {
return m.elements; }
287 template <
typename T,
class Matrix>
288 struct Matrix_struct_get_base_pointer<T,Matrix,true>
290 static inline T* get_base_ptr( Matrix& m) {
return &m.xx; }
291 static inline const T* get_base_ptr(
const Matrix& m) {
return &m.xx; }
296 template <
typename T, Size ROW, Size COL>
299 return Matrix_struct_get_base_pointer<T,Matrix_struct<T,ROW,COL>,
300 (ROW<=4 && COL<=4)>::get_base_ptr( mat);
304 template <
typename T, Size ROW, Size COL>
307 return Matrix_struct_get_base_pointer<T,Matrix_struct<T,ROW,COL>,
308 (ROW<=4 && COL<=4)>::get_base_ptr( mat);
365 template <
typename T, Size ROW, Size COL>
451 const Size MIN_DIM = (ROW < COL) ? ROW : COL;
452 for(
Size k(0u); k < MIN_DIM; ++k)
453 begin()[k * COL + k] = diag;
469 template <
typename Iterator>
472 for(
Size i(0u); i <
SIZE; ++i, ++p)
488 template <
typename T2>
492 begin()[i] = array[i];
497 template <
typename T2>
506 template <
typename T2>
518 for(
Size i(0u); i < ROW; ++i)
519 for(
Size j(0u); j < COL; ++j)
520 begin()[i * COL + j] = other.
begin()[j * ROW + i];
526 template <
typename T2>
531 for(
Size i(0u); i < ROW; ++i)
532 for(
Size j(0u); j < COL; ++j)
533 begin()[i * COL + j] = T(other.
begin()[j * ROW + i]);
622 inline Matrix( T m0, T m1, T m2, T m3, T m4, T m5)
632 inline Matrix( T m0, T m1, T m2, T m3, T m4, T m5, T m6, T m7)
642 inline Matrix( T m0, T m1, T m2, T m3, T m4, T m5, T m6, T m7, T m8)
654 T m0, T m1, T m2, T m3,
655 T m4, T m5, T m6, T m7,
656 T m8, T m9, T m10, T m11)
668 T m0, T m1, T m2, T m3,
669 T m4, T m5, T m6, T m7,
670 T m8, T m9, T m10, T m11,
671 T m12, T m13, T m14, T m15)
694 return begin()[row * COL + col];
704 return begin()[row * COL + col];
723 return begin()[row * COL + col];
744 begin()[row * COL + col] = value;
753 return this->xx * this->yy * this->zz
754 + this->xy * this->yz * this->zx
755 + this->xz * this->yx * this->zy
756 - this->xx * this->zy * this->yz
757 - this->xy * this->zz * this->yx
758 - this->xz * this->zx * this->yy;
774 for(
Size i=0; i < ROW-1; ++i) {
775 for(
Size j=i+1; j < COL; ++j) {
798 this->wx += T( vector.x);
799 this->wy += T( vector.y);
800 this->wz += T( vector.z);
808 this->wx += T( vector.x);
809 this->wy += T( vector.y);
810 this->wz += T( vector.z);
828 this->wx = T( vector.x);
829 this->wy = T( vector.y);
830 this->wz = T( vector.z);
838 this->wx = T( vector.x);
839 this->wy = T( vector.y);
840 this->wz = T( vector.z);
847 inline void rotate( T xangle, T yangle, T zangle)
861 tmp.
set_rotation( T( angles.x), T( angles.y), T( angles.z));
872 tmp.
set_rotation( T( angles.x), T( angles.y), T( angles.z));
881 inline void set_rotation( T x_angle, T y_angle, T z_angle);
890 set_rotation( T( angles.x), T( angles.y), T( angles.z));
900 set_rotation( T( angles.x), T( angles.y), T( angles.z));
943 template <
typename T, Size ROW, Size COL>
952 template <
typename T, Size ROW, Size COL>
963 template <
typename T, Size ROW, Size COL>
974 template <
typename T, Size ROW, Size COL>
985 template <
typename T, Size ROW, Size COL>
996 template <
typename T, Size ROW, Size COL>
1007 template <
typename T, Size ROW, Size COL>
1008 Matrix<T,ROW,COL>&
operator+=( Matrix<T,ROW,COL>& lhs,
const Matrix<T,ROW,COL>& rhs);
1011 template <
typename T, Size ROW, Size COL>
1012 Matrix<T,ROW,COL>&
operator-=( Matrix<T,ROW,COL>& lhs,
const Matrix<T,ROW,COL>& rhs);
1015 template <
typename T, Size ROW, Size COL>
1026 template <
typename T, Size ROW, Size COL>
1037 template <
typename T, Size ROW, Size COL>
1042 for(
Size i(0u); i < ROW*COL; ++i)
1053 template <
typename T, Size ROW, Size COL>
1061 for(
Size rrow = 0; rrow < ROW; ++rrow) {
1062 for(
Size rcol = 0; rcol < COL; ++rcol) {
1063 lhs( rrow, rcol) = T(0);
1064 for(
Size k = 0; k < COL; ++k)
1065 lhs( rrow, rcol) += old( rrow, k) * rhs( k, rcol);
1076 template <
typename T, Size ROW1, Size COL1, Size ROW2, Size COL2>
1085 for(
Size rrow = 0; rrow < ROW1; ++rrow) {
1086 for(
Size rcol = 0; rcol < COL2; ++rcol) {
1087 result( rrow, rcol) = T(0);
1088 for(
Size k = 0; k < COL1; ++k)
1089 result( rrow, rcol) += lhs( rrow, k) * rhs( k, rcol);
1102 template <
typename T, Size ROW, Size COL, Size DIM>
1109 for(
Size row = 0; row < ROW; ++row) {
1111 for(
Size col = 0; col < COL; ++col)
1112 result[row] += mat( row, col) * vec[col];
1123 template <Size DIM,
typename T, Size ROW, Size COL>
1130 for(
Size col = 0; col < COL; ++col) {
1132 for(
Size row = 0; row < ROW; ++row)
1133 result[col] += mat( row, col) * vec[row];
1140 template <
typename T, Size ROW, Size COL>
1143 for(
Size i=0; i < ROW*COL; ++i)
1144 mat.
begin()[i] *= factor;
1149 template <
typename T, Size ROW, Size COL>
1158 template <
typename T, Size ROW, Size COL>
1172 template <Size NEW_ROW, Size NEW_COL,
typename T, Size ROW, Size COL>
1179 for(
Size i=0; i < NEW_ROW; ++i)
1180 for(
Size j=0; j < NEW_COL; ++j)
1181 result( i, j) = mat( i, j);
1188 template <
typename T, Size ROW, Size COL>
1193 for(
Size i=0; i < ROW; ++i)
1194 for(
Size j=0; j < COL; ++j)
1195 result( j, i) = mat( i, j);
1204 template <
typename T,
typename U>
1209 const T w = T(mat.xw * point + mat.ww);
1210 if( w == T(0) || w == T(1))
1211 return U(mat.xx * point + mat.wx);
1213 return U((mat.xx * point + mat.wx) / w);
1221 template <
typename T,
typename U>
1226 T w = T(mat.xw * point.x + mat.yw * point.y + mat.ww);
1227 if( w == T(0) || w == T(1))
1229 U(mat.xx * point.x + mat.yx * point.y + mat.wx),
1230 U(mat.xy * point.x + mat.yy * point.y + mat.wy));
1234 U((mat.xx * point.x + mat.yx * point.y + mat.wx) * w),
1235 U((mat.xy * point.x + mat.yy * point.y + mat.wy) * w));
1244 template <
typename T,
typename U>
1249 T w = T(mat.xw * point.x + mat.yw * point.y + mat.zw * point.z + mat.ww);
1250 if( w == T(0) || w == T(1))
1252 U(mat.xx * point.x + mat.yx * point.y + mat.zx * point.z + mat.wx),
1253 U(mat.xy * point.x + mat.yy * point.y + mat.zy * point.z + mat.wy),
1254 U(mat.xz * point.x + mat.yz * point.y + mat.zz * point.z + mat.wz));
1258 U((mat.xx * point.x + mat.yx * point.y + mat.zx * point.z + mat.wx) * w),
1259 U((mat.xy * point.x + mat.yy * point.y + mat.zy * point.z + mat.wy) * w),
1260 U((mat.xz * point.x + mat.yz * point.y + mat.zz * point.z + mat.wz) * w));
1269 template <
typename T,
typename U>
1275 U(mat.xx * point.x + mat.yx * point.y + mat.zx * point.z + mat.wx * point.w),
1276 U(mat.xy * point.x + mat.yy * point.y + mat.zy * point.z + mat.wy * point.w),
1277 U(mat.xz * point.x + mat.yz * point.y + mat.zz * point.z + mat.wz * point.w),
1278 U(mat.xw * point.x + mat.yw * point.y + mat.zw * point.z + mat.ww * point.w));
1286 template <
typename T,
typename U>
1291 return U(mat.xx * vector);
1299 template <
typename T,
typename U>
1305 U(mat.xx * vector.x + mat.yx * vector.y),
1306 U(mat.xy * vector.x + mat.yy * vector.y));
1314 template <
typename T,
typename U>
1320 U(mat.xx * vector.x + mat.yx * vector.y + mat.zx * vector.z),
1321 U(mat.xy * vector.x + mat.yy * vector.y + mat.zy * vector.z),
1322 U(mat.xz * vector.x + mat.yz * vector.y + mat.zz * vector.z));
1336 template <
typename T,
typename U>
1342 U(inv_mat.xx * normal.x + inv_mat.xy * normal.y + inv_mat.xz * normal.z),
1343 U(inv_mat.yx * normal.x + inv_mat.yy * normal.y + inv_mat.yz * normal.z),
1344 U(inv_mat.zx * normal.x + inv_mat.zy * normal.y + inv_mat.zz * normal.z));
1360 template <
typename T,
typename U>
1366 mat.yx, mat.yy, mat.yz,
1367 mat.zx, mat.zy, mat.zz);
1368 bool inverted = sub_mat.
invert();
1372 U(sub_mat.xx * normal.x + sub_mat.xy * normal.y + sub_mat.xz * normal.z),
1373 U(sub_mat.yx * normal.x + sub_mat.yy * normal.y + sub_mat.yz * normal.z),
1374 U(sub_mat.zx * normal.x + sub_mat.zy * normal.y + sub_mat.zz * normal.z));
1380 template <
typename T, Size ROW, Size COL>
1385 for(
Size i=0; i < ROW*COL; ++i)
1390 template <
typename T, Size ROW, Size COL>
1395 for(
Size i=0; i < ROW*COL; ++i)
1400 #ifndef MI_FOR_DOXYGEN_ONLY
1402 template <
typename T, Size ROW, Size COL>
1409 const T min_angle = T(0.00024f);
1411 if(
abs( xangle) > min_angle) {
1418 if(
abs( yangle) > min_angle) {
1425 if(
abs(zangle) > min_angle) {
1432 this->xx = tcy * tcz;
1433 this->xy = tcy * tsz;
1437 this->yx = tmp * tcz - tcx * tsz;
1438 this->yy = tmp * tsz + tcx * tcz;
1439 this->yz = tsx * tcy;
1442 this->zx = tmp * tcz + tsx * tsz;
1443 this->zy = tmp * tsz - tsx * tcz;
1444 this->zz = tcx * tcy;
1447 template <
typename T, Size ROW, Size COL>
1451 Vector<T,3> axis( axis_v);
1452 const T min_angle = T(0.00024f);
1454 if(
abs( T(angle)) < min_angle) {
1455 T xa = axis.x * T(angle);
1456 T ya = axis.y * T(angle);
1457 T za = axis.z * T(angle);
1474 T s =
sin( T(angle));
1475 T c =
cos( T(angle));
1479 tmp = t * T(axis.x);
1480 this->xx = tmp * T(axis.x) + c;
1481 this->xy = tmp * T(axis.y) + s * T(axis.z);
1482 this->xz = tmp * T(axis.z) - s * T(axis.y);
1485 tmp = t * T(axis.y);
1486 this->yx = tmp * T(axis.x) - s * T(axis.z);
1487 this->yy = tmp * T(axis.y) + c;
1488 this->yz = tmp * T(axis.z) + s * T(axis.x);
1491 tmp = t * T(axis.z);
1492 this->zx = tmp * T(axis.x) + s * T(axis.y);
1493 this->zy = tmp * T(axis.y) - s * T(axis.x);
1494 this->zz = tmp * T(axis.z) + c;
1497 this->wx = this->wy = this->wz = T(0);
1501 template <
typename T, Size ROW, Size COL>
1505 Vector<T,3> axis( axis_v);
1506 const T min_angle = T(0.00024f);
1508 if(
abs(T(angle)) < min_angle) {
1509 T xa = axis.x * T(angle);
1510 T ya = axis.y * T(angle);
1511 T za = axis.z * T(angle);
1528 T s =
sin( T(angle));
1529 T c =
cos( T(angle));
1533 tmp = t * T(axis.x);
1534 this->xx = tmp * T(axis.x) + c;
1535 this->xy = tmp * T(axis.y) + s * T(axis.z);
1536 this->xz = tmp * T(axis.z) - s * T(axis.y);
1539 tmp = t * T(axis.y);
1540 this->yx = tmp * T(axis.x) - s * T(axis.z);
1541 this->yy = tmp * T(axis.y) + c;
1542 this->yz = tmp * T(axis.z) + s * T(axis.x);
1545 tmp = t * T(axis.z);
1546 this->zx = tmp * T(axis.x) + s * T(axis.y);
1547 this->zy = tmp * T(axis.y) - s * T(axis.x);
1548 this->zz = tmp * T(axis.z) + c;
1551 this->wx = this->wy = this->wz = T(0);
1555 template <
typename T, Size ROW, Size COL>
1557 const Vector<Float32,3>& position,
1558 const Vector<Float32,3>& target,
1559 const Vector<Float32,3>& up)
1562 Vector<Float32,3> xaxis, yaxis, zaxis;
1565 zaxis = position - target;
1569 xaxis =
cross( up, zaxis);
1573 yaxis =
cross( zaxis, xaxis);
1578 T(xaxis.x), T(yaxis.x), T(zaxis.x), T(0),
1579 T(xaxis.y), T(yaxis.y), T(zaxis.y), T(0),
1580 T(xaxis.z), T(yaxis.z), T(zaxis.z), T(0),
1581 T(0), T(0), T(0), T(1));
1584 Matrix<T,4,4> trans(
1585 T(1), T(0), T(0), T(0),
1586 T(0), T(1), T(0), T(0),
1587 T(0), T(0), T(1), T(0),
1588 T(-position.x), T(-position.y), T(-position.z), T(1));
1590 *
this = trans * rot;
1593 template <
typename T, Size ROW, Size COL>
1595 const Vector<Float64,3>& position,
1596 const Vector<Float64,3>& target,
1597 const Vector<Float64,3>& up)
1600 Vector<Float64,3> xaxis, yaxis, zaxis;
1603 zaxis = position - target;
1607 xaxis =
cross( up, zaxis);
1611 yaxis =
cross( zaxis, xaxis);
1616 T(xaxis.x), T(yaxis.x), T(zaxis.x), T(0),
1617 T(xaxis.y), T(yaxis.y), T(zaxis.y), T(0),
1618 T(xaxis.z), T(yaxis.z), T(zaxis.z), T(0),
1619 T(0), T(0), T(0), T(1));
1622 Matrix<T,4,4> trans(
1623 T(1), T(0), T(0), T(0),
1624 T(0), T(1), T(0), T(0),
1625 T(0), T(0), T(1), T(0),
1626 T(-position.x), T(-position.y), T(-position.z), T(1));
1628 *
this = trans * rot;
1635 template <
class T, Size ROW, Size COL>
1636 class Matrix_inverter
1639 typedef math::Matrix<T,ROW,COL> Matrix;
1644 static inline bool invert( Matrix& ) {
return false; }
1648 template <
class T, Size DIM>
1649 class Matrix_inverter<T,DIM,DIM>
1652 typedef math::Matrix<T,DIM,DIM> Matrix;
1653 typedef math::Vector<T,DIM> Vector;
1654 typedef math::Vector<Size,DIM> Index_vector;
1660 static bool lu_decomposition(
1662 Index_vector& indx);
1666 static void lu_backsubstitution(
1668 const Index_vector& indx,
1671 static bool invert( Matrix& mat);
1674 template <
class T, Size DIM>
1675 bool Matrix_inverter<T,DIM,DIM>::lu_decomposition(
1681 for(
Size i = 0; i < DIM; i++) {
1683 for(
Size j = 0; j < DIM; j++) {
1684 T temp =
abs(lu.get(i,j));
1696 for(
Size j = 0; j < DIM; j++) {
1697 for(
Size i = 0; i < j; i++) {
1698 T sum = lu.get(i,j);
1699 for(
Size k = 0; k < i; k++)
1700 sum -= lu.get(i,k) * lu.get(k,j);
1704 for(
Size i = j; i < DIM; i++) {
1705 T sum = lu.get(i,j);
1706 for(
Size k = 0; k < j; k++)
1707 sum -= lu.get(i,k) * lu.get(k,j);
1709 T dum = vv[i] *
abs(sum);
1716 for(
Size k = 0; k < DIM; k++) {
1717 T dum = lu.get(imax,k);
1718 lu.set(imax, k, lu.get(j,k));
1724 if( lu.get(j,j) == 0)
1727 T dum = T(1) / lu.get(j,j);
1728 for(
Size i = j + 1; i < DIM; i++)
1729 lu.set(i, j, lu.get(i,j) * dum);
1735 template <
class T, Size DIM>
1736 void Matrix_inverter<T,DIM,DIM>::lu_backsubstitution(
1738 const Index_vector& indx,
1743 for(
Size i = 0; i < DIM; i++) {
1748 for(
Size j = ii; j < i; j++) {
1749 sum -= lu.get(i,j) * b[j];
1758 for(
Size i2 = DIM; i2 > 0;) {
1761 for(
Size j = i2+1; j < DIM; j++)
1762 sum -= lu.get(i2,j) * b[j];
1763 b[i2] = sum / lu.get(i2,i2);
1767 template <
class T, Size DIM>
1768 bool Matrix_inverter<T,DIM,DIM>::invert( Matrix& mat)
1774 if( !lu_decomposition(lu, indx))
1778 for(
Size j = 0; j < DIM; ++j) {
1781 lu_backsubstitution( lu, indx, col);
1782 for(
Size i = 0; i < DIM; ++i) {
1783 mat.set( i, j, col[i]);
1791 class Matrix_inverter<T,1,1>
1794 typedef math::Matrix<T,1,1> Matrix;
1796 static inline bool invert( Matrix& mat)
1798 T s = mat.get( 0, 0);
1801 mat.set( 0, 0, T(1) / s);
1808 class Matrix_inverter<T,2,2>
1811 typedef math::Matrix<T,2,2> Matrix;
1813 static inline bool invert( Matrix& mat)
1815 T a = mat.get( 0, 0);
1816 T b = mat.get( 0, 1);
1817 T c = mat.get( 1, 0);
1818 T d = mat.get( 1, 1);
1822 T rdet = T(1) / det;
1823 mat.set( 0, 0, d * rdet);
1824 mat.set( 0, 1,-b * rdet);
1825 mat.set( 1, 0,-c * rdet);
1826 mat.set( 1, 1, a * rdet);
1831 template <
typename T, Size ROW, Size COL>
1834 return Matrix_inverter<T,ROW,COL>::invert( *
this);
1842 template <
typename T>
1845 const Matrix<T,4,4>& rhs)
1847 Matrix<T,4,4> old( lhs);
1849 lhs.xx = old.xx * rhs.xx + old.xy * rhs.yx + old.xz * rhs.zx + old.xw * rhs.wx;
1850 lhs.xy = old.xx * rhs.xy + old.xy * rhs.yy + old.xz * rhs.zy + old.xw * rhs.wy;
1851 lhs.xz = old.xx * rhs.xz + old.xy * rhs.yz + old.xz * rhs.zz + old.xw * rhs.wz;
1852 lhs.xw = old.xx * rhs.xw + old.xy * rhs.yw + old.xz * rhs.zw + old.xw * rhs.ww;
1854 lhs.yx = old.yx * rhs.xx + old.yy * rhs.yx + old.yz * rhs.zx + old.yw * rhs.wx;
1855 lhs.yy = old.yx * rhs.xy + old.yy * rhs.yy + old.yz * rhs.zy + old.yw * rhs.wy;
1856 lhs.yz = old.yx * rhs.xz + old.yy * rhs.yz + old.yz * rhs.zz + old.yw * rhs.wz;
1857 lhs.yw = old.yx * rhs.xw + old.yy * rhs.yw + old.yz * rhs.zw + old.yw * rhs.ww;
1859 lhs.zx = old.zx * rhs.xx + old.zy * rhs.yx + old.zz * rhs.zx + old.zw * rhs.wx;
1860 lhs.zy = old.zx * rhs.xy + old.zy * rhs.yy + old.zz * rhs.zy + old.zw * rhs.wy;
1861 lhs.zz = old.zx * rhs.xz + old.zy * rhs.yz + old.zz * rhs.zz + old.zw * rhs.wz;
1862 lhs.zw = old.zx * rhs.xw + old.zy * rhs.yw + old.zz * rhs.zw + old.zw * rhs.ww;
1864 lhs.wx = old.wx * rhs.xx + old.wy * rhs.yx + old.wz * rhs.zx + old.ww * rhs.wx;
1865 lhs.wy = old.wx * rhs.xy + old.wy * rhs.yy + old.wz * rhs.zy + old.ww * rhs.wy;
1866 lhs.wz = old.wx * rhs.xz + old.wy * rhs.yz + old.wz * rhs.zz + old.ww * rhs.wz;
1867 lhs.ww = old.wx * rhs.xw + old.wy * rhs.yw + old.wz * rhs.zw + old.ww * rhs.ww;
1873 template <
typename T>
1875 const Matrix<T,4,4>& lhs,
1876 const Matrix<T,4,4>& rhs)
1878 Matrix<T,4,4> temp( lhs);
1883 #endif // MI_FOR_DOXYGEN_ONLY
1891 #endif // MI_MATH_MATRIX_H