lolengine
/
lol
mirror of https://github.com/lolengine/lol
1
0
Fork 0
Code Issues 0 Releases 0 Wiki Activity
3077 Commits
5 Branches
18 MiB
 
 
 
Tree: f579dbc881
lol/legacy/lol/math/matrix.h

639 lines
18 KiB
Raw Blame History 

  1. //

  2. //  Lol Engine

  3. //

  4. //  Copyright © 2010—2019 Sam Hocevar <sam@hocevar.net>

  5. //

  6. //  Lol Engine is free software. It comes without any warranty, to

  7. //  the extent permitted by applicable law. You can redistribute it

  8. //  and/or modify it under the terms of the Do What the Fuck You Want

  9. //  to Public License, Version 2, as published by the WTFPL Task Force.

  10. //  See http://www.wtfpl.net/ for more details.

  11. //

  12. 

  13. #pragma once

  14. 

  15. //

  16. // The matrix classes

  17. // ------------------

  18. //

  19. 

  20. #include <ostream>

  21. 

  22. #include <lol/math/vector.h>

  23. #include <lol/math/transform.h>

  24. 

  25. #if _WIN32

  26. #   pragma push_macro("near")

  27. #   pragma push_macro("far")

  28. #   undef near

  29. #   undef far

  30. #endif

  31. 

  32. namespace lol

  33. {

  34. 

  35. /*

  36.  * The generic “mat_t” type, which is fixed-size

  37.  */

  38. 

  39. template<typename T, int COLS, int ROWS>

  40. struct lol_attr_nodiscard mat_t

  41.   : public linear_ops::base<vec_t<T,ROWS>>

  42. {

  43.     static int const count = COLS;

  44.     typedef T scalar_element;

  45.     typedef vec_t<T,ROWS> element;

  46.     typedef mat_t<T,COLS,ROWS> type;

  47. 

  48.     inline mat_t() {}

  49. 

  50.     explicit inline mat_t(T const &val)

  51.     {

  52.         T const zero = T(0);

  53.         for (int i = 0; i < COLS; ++i)

  54.             for (int j = 0; j < ROWS; ++j)

  55.                 m_data[i][j] = i == j ? val : zero;

  56.     }

  57. 

  58.     /* Explicit constructor for type conversion */

  59.     template<typename U>

  60.     explicit inline mat_t(mat_t<U, COLS, ROWS> const &m)

  61.     {

  62.         for (int i = 0; i < COLS; ++i)

  63.             m_data[i] = (vec_t<T,ROWS>)m[i];

  64.     }

  65. 

  66.     inline vec_t<T,ROWS>& operator[](size_t n) { return m_data[n]; }

  67.     inline vec_t<T,ROWS> const& operator[](size_t n) const { return m_data[n]; }

  68. 

  69. private:

  70.     vec_t<T,ROWS> m_data[COLS];

  71. };

  72. 

  73. /*

  74.  * 2×2-element matrices

  75.  */

  76. 

  77. template <typename T>

  78. struct lol_attr_nodiscard mat_t<T, 2, 2>

  79.   : public linear_ops::base<vec_t<T,2>>

  80. {

  81.     static int const count = 2;

  82.     typedef T scalar_element;

  83.     typedef vec_t<T,2> element;

  84.     typedef mat_t<T,2,2> type;

  85. 

  86.     inline mat_t() {}

  87.     inline mat_t(vec_t<T,2> v0, vec_t<T,2> v1)

  88.       : m_data{ v0, v1 } {}

  89. 

  90.     explicit inline mat_t(T const &val)

  91.       : m_data{ vec_t<T,2>(val, T(0)),

  92.                 vec_t<T,2>(T(0), val) } {}

  93. 

  94.     explicit inline mat_t(mat_t<T,4,4> const &m)

  95.       : m_data{ m[0].xy, m[1].xy } {}

  96. 

  97.     /* Explicit constructor for type conversion */

  98.     template<typename U>

  99.     explicit inline mat_t(mat_t<U,2,2> const &m)

  100.       : m_data{ (element)m[0], (element)m[1] } {}

  101. 

  102.     inline vec_t<T,2>& operator[](size_t n) { return m_data[n]; }

  103.     inline vec_t<T,2> const& operator[](size_t n) const { return m_data[n]; }

  104. 

  105.     /* Helpers for transformation matrices */

  106.     static mat_t<T,2,2> rotate(T radians);

  107.     static inline mat_t<T,2,2> rotate(mat_t<T,2,2> m, T radians)

  108.     {

  109.         return rotate(radians) * m;

  110.     }

  111. 

  112.     void printf() const;

  113.     std::string tostring() const;

  114. 

  115.     static const mat_t<T,2,2> identity;

  116. 

  117. private:

  118.     vec_t<T,2> m_data[2];

  119. };

  120. 

  121. static_assert(sizeof(imat2) == 16, "sizeof(imat2) == 16");

  122. static_assert(sizeof(f16mat2) == 8, "sizeof(f16mat2) == 8");

  123. static_assert(sizeof(mat2) == 16, "sizeof(mat2) == 16");

  124. static_assert(sizeof(dmat2) == 32, "sizeof(dmat2) == 32");

  125. 

  126. /*

  127.  * 3×3-element matrices

  128.  */

  129. 

  130. template <typename T>

  131. struct lol_attr_nodiscard mat_t<T, 3, 3>

  132.   : public linear_ops::base<vec_t<T,3>>

  133. {

  134.     static int const count = 3;

  135.     typedef T scalar_element;

  136.     typedef vec_t<T,3> element;

  137.     typedef mat_t<T,3,3> type;

  138. 

  139.     inline mat_t() {}

  140.     inline mat_t(vec_t<T,3> v0, vec_t<T,3> v1, vec_t<T,3> v2)

  141.       : m_data{ v0, v1, v2 } {}

  142. 

  143.     explicit inline mat_t(T const &val)

  144.       : m_data{ vec_t<T,3>(val, (T)0, (T)0),

  145.                 vec_t<T,3>((T)0, val, (T)0),

  146.                 vec_t<T,3>((T)0, (T)0, val) } {}

  147. 

  148.     explicit inline mat_t(mat_t<T,2,2> m, T const &val = T(1))

  149.       : m_data{ vec_t<T,3>(m[0], (T)0),

  150.                 vec_t<T,3>(m[1], (T)0),

  151.                 vec_t<T,3>((T)0, (T)0, val) } {}

  152. 

  153.     explicit inline mat_t(mat_t<T,4,4> const &m)

  154.       : m_data{ m[0].xyz, m[1].xyz, m[2].xyz } {}

  155. 

  156.     /* Explicit constructor for type conversion */

  157.     template<typename U>

  158.     explicit inline mat_t(mat_t<U,3,3> const &m)

  159.       : m_data{ (element)m[0], (element)m[1], (element)m[2] } {}

  160. 

  161.     explicit mat_t(quat_t<T> const &q);

  162. 

  163.     inline vec_t<T,3>& operator[](size_t n) { return m_data[n]; }

  164.     inline vec_t<T,3> const& operator[](size_t n) const { return m_data[n]; }

  165. 

  166.     /* Helpers for transformation matrices */

  167.     static mat_t<T,3,3> scale(T x);

  168.     static mat_t<T,3,3> scale(T x, T y, T z);

  169.     static mat_t<T,3,3> scale(vec_t<T,3> v);

  170.     static mat_t<T,3,3> rotate(T radians, T x, T y, T z);

  171.     static mat_t<T,3,3> rotate(T radians, vec_t<T,3> v);

  172. 

  173.     static mat_t<T,3,3> fromeuler_xyz(vec_t<T,3> const &v);

  174.     static mat_t<T,3,3> fromeuler_xzy(vec_t<T,3> const &v);

  175.     static mat_t<T,3,3> fromeuler_yxz(vec_t<T,3> const &v);

  176.     static mat_t<T,3,3> fromeuler_yzx(vec_t<T,3> const &v);

  177.     static mat_t<T,3,3> fromeuler_zxy(vec_t<T,3> const &v);

  178.     static mat_t<T,3,3> fromeuler_zyx(vec_t<T,3> const &v);

  179.     static mat_t<T,3,3> fromeuler_xyz(T phi, T theta, T psi);

  180.     static mat_t<T,3,3> fromeuler_xzy(T phi, T theta, T psi);

  181.     static mat_t<T,3,3> fromeuler_yxz(T phi, T theta, T psi);

  182.     static mat_t<T,3,3> fromeuler_yzx(T phi, T theta, T psi);

  183.     static mat_t<T,3,3> fromeuler_zxy(T phi, T theta, T psi);

  184.     static mat_t<T,3,3> fromeuler_zyx(T phi, T theta, T psi);

  185. 

  186.     static mat_t<T,3,3> fromeuler_xyx(vec_t<T,3> const &v);

  187.     static mat_t<T,3,3> fromeuler_xzx(vec_t<T,3> const &v);

  188.     static mat_t<T,3,3> fromeuler_yxy(vec_t<T,3> const &v);

  189.     static mat_t<T,3,3> fromeuler_yzy(vec_t<T,3> const &v);

  190.     static mat_t<T,3,3> fromeuler_zxz(vec_t<T,3> const &v);

  191.     static mat_t<T,3,3> fromeuler_zyz(vec_t<T,3> const &v);

  192.     static mat_t<T,3,3> fromeuler_xyx(T phi, T theta, T psi);

  193.     static mat_t<T,3,3> fromeuler_xzx(T phi, T theta, T psi);

  194.     static mat_t<T,3,3> fromeuler_yxy(T phi, T theta, T psi);

  195.     static mat_t<T,3,3> fromeuler_yzy(T phi, T theta, T psi);

  196.     static mat_t<T,3,3> fromeuler_zxz(T phi, T theta, T psi);

  197.     static mat_t<T,3,3> fromeuler_zyz(T phi, T theta, T psi);

  198. 

  199.     static inline mat_t<T,3,3> rotate(mat_t<T,3,3> m, T radians, vec_t<T,3> v)

  200.     {

  201.         return rotate(radians, v) * m;

  202.     }

  203. 

  204.     void printf() const;

  205.     std::string tostring() const;

  206. 

  207.     static const mat_t<T,3,3> identity;

  208. 

  209. private:

  210.     vec_t<T,3> m_data[3];

  211. };

  212. 

  213. static_assert(sizeof(imat3) == 36, "sizeof(imat3) == 36");

  214. static_assert(sizeof(f16mat3) == 18, "sizeof(f16mat3) == 18");

  215. static_assert(sizeof(mat3) == 36, "sizeof(mat3) == 36");

  216. static_assert(sizeof(dmat3) == 72, "sizeof(dmat3) == 72");

  217. 

  218. /*

  219.  * 4×4-element matrices

  220.  */

  221. 

  222. template <typename T>

  223. struct lol_attr_nodiscard mat_t<T, 4, 4>

  224.   : public linear_ops::base<vec_t<T,4>>

  225. {

  226.     static int const count = 4;

  227.     typedef T scalar_element;

  228.     typedef vec_t<T,4> element;

  229.     typedef mat_t<T,4,4> type;

  230. 

  231.     inline mat_t() {}

  232.     inline mat_t(vec_t<T,4> v0, vec_t<T,4> v1, vec_t<T,4> v2, vec_t<T,4> v3)

  233.       : m_data{ v0, v1, v2, v3 } {}

  234. 

  235.     explicit inline mat_t(T const &val)

  236.       : m_data{ vec_t<T,4>(val, (T)0, (T)0, (T)0),

  237.                 vec_t<T,4>((T)0, val, (T)0, (T)0),

  238.                 vec_t<T,4>((T)0, (T)0, val, (T)0),

  239.                 vec_t<T,4>((T)0, (T)0, (T)0, val) } {}

  240. 

  241.     explicit inline mat_t(mat_t<T,2,2> m, T const &val = T(1))

  242.       : m_data{ vec_t<T,4>(m[0], (T)0, (T)0),

  243.                 vec_t<T,4>(m[1], (T)0, (T)0),

  244.                 vec_t<T,4>((T)0, (T)0, val, (T)0),

  245.                 vec_t<T,4>((T)0, (T)0, (T)0, val) } {}

  246. 

  247.     explicit inline mat_t(mat_t<T,3,3> m, T const &val = T(1))

  248.       : m_data{ vec_t<T,4>(m[0], (T)0),

  249.                 vec_t<T,4>(m[1], (T)0),

  250.                 vec_t<T,4>(m[2], (T)0),

  251.                 vec_t<T,4>((T)0, (T)0, (T)0, val) } {}

  252. 

  253.     /* Explicit constructor for type conversion */

  254.     template<typename U>

  255.     explicit inline mat_t(mat_t<U,4,4> const &m)

  256.       : m_data{ (element)m[0], (element)m[1],

  257.                 (element)m[2], (element)m[3] } {}

  258. 

  259.     explicit mat_t(quat_t<T> const &q);

  260. 

  261.     inline vec_t<T,4>& operator[](size_t n) { return m_data[n]; }

  262.     inline vec_t<T,4> const& operator[](size_t n) const { return m_data[n]; }

  263. 

  264.     /* Helpers for transformation matrices */

  265.     static mat_t<T,4,4> translate(T x, T y, T z);

  266.     static mat_t<T,4,4> translate(vec_t<T,3> v);

  267. 

  268.     static inline mat_t<T,4,4> scale(T x)

  269.     {

  270.         return mat_t<T,4,4>(mat_t<T,3,3>::scale(x), (T)1);

  271.     }

  272. 

  273.     static inline mat_t<T,4,4> scale(T x, T y, T z)

  274.     {

  275.         return mat_t<T,4,4>(mat_t<T,3,3>::scale(x, y, z), (T)1);

  276.     }

  277. 

  278.     static inline mat_t<T,4,4> scale(vec_t<T,3> v)

  279.     {

  280.         return mat_t<T,4,4>(mat_t<T,3,3>::scale(v), (T)1);

  281.     }

  282. 

  283.     static inline mat_t<T,4,4> translate(mat_t<T,4,4> const &m, vec_t<T,3> v)

  284.     {

  285.         return translate(v) * m;

  286.     }

  287. 

  288.     static inline mat_t<T,4,4> rotate(T radians, T x, T y, T z)

  289.     {

  290.         return mat_t<T,4,4>(mat_t<T,3,3>::rotate(radians, x, y, z), (T)1);

  291.     }

  292. 

  293.     static inline mat_t<T,4,4> rotate(T radians, vec_t<T,3> v)

  294.     {

  295.         return mat_t<T,4,4>(mat_t<T,3,3>::rotate(radians, v), (T)1);

  296.     }

  297. 

  298.     static inline mat_t<T,4,4> rotate(mat_t<T,4,4> &m, T radians, vec_t<T,3> v)

  299.     {

  300.         return rotate(radians, v) * m;

  301.     }

  302. 

  303.     static mat_t<T,4,4> fromeuler_xyz(vec_t<T,3> const &v);

  304.     static mat_t<T,4,4> fromeuler_xzy(vec_t<T,3> const &v);

  305.     static mat_t<T,4,4> fromeuler_yxz(vec_t<T,3> const &v);

  306.     static mat_t<T,4,4> fromeuler_yzx(vec_t<T,3> const &v);

  307.     static mat_t<T,4,4> fromeuler_zxy(vec_t<T,3> const &v);

  308.     static mat_t<T,4,4> fromeuler_zyx(vec_t<T,3> const &v);

  309.     static mat_t<T,4,4> fromeuler_xyz(T phi, T theta, T psi);

  310.     static mat_t<T,4,4> fromeuler_xzy(T phi, T theta, T psi);

  311.     static mat_t<T,4,4> fromeuler_yxz(T phi, T theta, T psi);

  312.     static mat_t<T,4,4> fromeuler_yzx(T phi, T theta, T psi);

  313.     static mat_t<T,4,4> fromeuler_zxy(T phi, T theta, T psi);

  314.     static mat_t<T,4,4> fromeuler_zyx(T phi, T theta, T psi);

  315. 

  316.     static mat_t<T,4,4> fromeuler_xyx(vec_t<T,3> const &v);

  317.     static mat_t<T,4,4> fromeuler_xzx(vec_t<T,3> const &v);

  318.     static mat_t<T,4,4> fromeuler_yxy(vec_t<T,3> const &v);

  319.     static mat_t<T,4,4> fromeuler_yzy(vec_t<T,3> const &v);

  320.     static mat_t<T,4,4> fromeuler_zxz(vec_t<T,3> const &v);

  321.     static mat_t<T,4,4> fromeuler_zyz(vec_t<T,3> const &v);

  322.     static mat_t<T,4,4> fromeuler_xyx(T phi, T theta, T psi);

  323.     static mat_t<T,4,4> fromeuler_xzx(T phi, T theta, T psi);

  324.     static mat_t<T,4,4> fromeuler_yxy(T phi, T theta, T psi);

  325.     static mat_t<T,4,4> fromeuler_yzy(T phi, T theta, T psi);

  326.     static mat_t<T,4,4> fromeuler_zxz(T phi, T theta, T psi);

  327.     static mat_t<T,4,4> fromeuler_zyz(T phi, T theta, T psi);

  328. 

  329.     /* Helpers for view matrices */

  330.     static mat_t<T,4,4> lookat(vec_t<T,3> eye, vec_t<T,3> center, vec_t<T,3> up);

  331. 

  332.     /* Helpers for projection matrices; FOV values are in radians */

  333.     static mat_t<T,4,4> ortho(T left, T right, T bottom, T top, T near, T far);

  334.     static mat_t<T,4,4> ortho(T width, T height, T near, T far);

  335.     static mat_t<T,4,4> frustum(T left, T right, T bottom, T top, T near, T far);

  336.     static mat_t<T,4,4> perspective(T fov_y, T width, T height, T near, T far);

  337.     static mat_t<T,4,4> shifted_perspective(T fov_y, T screen_size, T screen_ratio_yx, T near, T far);

  338. 

  339.     void printf() const;

  340.     std::string tostring() const;

  341. 

  342.     static const mat_t<T,4,4> identity;

  343. 

  344. private:

  345.     vec_t<T,4> m_data[4];

  346. };

  347. 

  348. static_assert(sizeof(imat4) == 64, "sizeof(imat4) == 64");

  349. static_assert(sizeof(f16mat4) == 32, "sizeof(f16mat4) == 32");

  350. static_assert(sizeof(mat4) == 64, "sizeof(mat4) == 64");

  351. static_assert(sizeof(dmat4) == 128, "sizeof(dmat4) == 128");

  352. 

  353. /*

  354.  * stdstream method implementations

  355.  */

  356. 

  357. template<class U, int COLS, int ROWS>

  358. static std::ostream &operator<<(std::ostream &stream,

  359.                                 mat_t<U,COLS,ROWS> const &m)

  360. {

  361.     for (int y = 0; y < ROWS; ++y)

  362.     {

  363.         stream << (y == 0 ? "(" : ", ");

  364.         for (int x = 0; x < COLS; ++x)

  365.             stream << (x == 0 ? "(" : ", ") << m[x][y];

  366.         stream << ")";

  367.     }

  368.     return stream << ")";

  369. }

  370. 

  371. /*

  372.  * Transpose any matrix

  373.  */

  374. 

  375. template<typename T, int COLS, int ROWS>

  376. static inline mat_t<T, ROWS, COLS> transpose(mat_t<T, COLS, ROWS> const &m)

  377. {

  378.     mat_t<T, ROWS, COLS> ret;

  379.     for (int i = 0; i < COLS; ++i)

  380.         for (int j = 0; j < ROWS; ++j)

  381.             ret[j][i] = m[i][j];

  382.     return ret;

  383. }

  384. 

  385. /*

  386.  * Compute a square submatrix, useful for minors and cofactor matrices

  387.  */

  388. 

  389. template<typename T, int N>

  390. mat_t<T, N - 1, N - 1> submatrix(mat_t<T, N, N> const &m, int i, int j)

  391. {

  392.     ASSERT(i >= 0); ASSERT(j >= 0); ASSERT(i < N); ASSERT(j < N);

  393. 

  394.     mat_t<T, N - 1, N - 1> ret;

  395.     for (int i2 = 0; i2 < N - 1; ++i2)

  396.         for (int j2 = 0; j2 < N - 1; ++j2)

  397.             ret[i2][j2] = m[i2 + (i2 >= i)][j2 + (j2 >= j)];

  398. 

  399.     return ret;

  400. }

  401. 

  402. /*

  403.  * Compute square matrix cofactor

  404.  */

  405. 

  406. template<typename T, int N> lol_attr_nodiscard

  407. T cofactor(mat_t<T, N, N> const &m, int i, int j)

  408. {

  409.     ASSERT(i >= 0); ASSERT(j >= 0); ASSERT(i < N); ASSERT(j < N);

  410.     T tmp = determinant(submatrix(m, i, j));

  411.     return ((i + j) & 1) ? -tmp : tmp;

  412. }

  413. 

  414. template<typename T> lol_attr_nodiscard

  415. T cofactor(mat_t<T, 2, 2> const &m, int i, int j)

  416. {

  417.     /* This specialisation shouldn't be needed, but Visual Studio. */

  418.     ASSERT(i >= 0); ASSERT(j >= 0); ASSERT(i < 2); ASSERT(j < 2);

  419.     T tmp = m[1 - i][1 - j];

  420.     return (i ^ j) ? -tmp : tmp;

  421. }

  422. 

  423. // Lu decomposition with partial pivoting

  424. template<typename T, int N> lol_attr_nodiscard

  425. std::tuple<mat_t<T, N, N>, vec_t<int, N>, int> lu_decomposition(mat_t<T, N, N> const &m)

  426. {

  427.     mat_t<T, N, N> lu = m;

  428.     vec_t<int, N> perm;

  429.     int sign = 1;

  430. 

  431.     for (int i = 0; i < N; ++i)

  432.         perm[i] = i;

  433. 

  434.     for (int k = 0; k < N; ++k)

  435.     {

  436.         // Find row with the largest absolute value

  437.         int best_j = k;

  438.         for (int j = k + 1; j < N; ++j)

  439.             if (abs(lu[k][j]) > lol::abs(lu[k][best_j]))

  440.                 best_j = j;

  441. 

  442.         // Swap rows in result

  443.         if (best_j != k)

  444.         {

  445.             std::swap(perm[k], perm[best_j]);

  446.             sign = -sign;

  447.             for (int i = 0; i < N; ++i)

  448.                 std::swap(lu[i][k], lu[i][best_j]);

  449.         }

  450. 

  451.         // Compute the Schur complement in the lower triangular part

  452.         for (int j = k + 1; j < N; ++j)

  453.         {

  454.             lu[k][j] /= lu[k][k];

  455.             for (int i = k + 1; i < N; ++i)

  456.                 lu[i][j] -= lu[i][k] * lu[k][j];

  457.         }

  458.     }

  459. 

  460.     return std::make_tuple(lu, perm, sign);

  461. }

  462. 

  463. /*

  464.  * Compute square matrix determinant, with a specialisation for 1×1 matrices

  465.  */

  466. 

  467. template<typename T, int N> lol_attr_nodiscard

  468. T determinant(mat_t<T, N, N> const &m)

  469. {

  470.     auto lup = lu_decomposition(m);

  471. 

  472.     T det(T(std::get<2>(lup)));

  473.     for (int i = 0; i < N; ++i)

  474.         det *= std::get<0>(lup)[i][i];

  475. 

  476.     return det;

  477. }

  478. 

  479. template<typename T> lol_attr_nodiscard

  480. T const & determinant(mat_t<T, 1, 1> const &m)

  481. {

  482.     return m[0][0];

  483. }

  484. 

  485. // Compute inverse of the L matrix of an LU decomposition

  486. template<typename T, int N>

  487. mat_t<T, N, N> l_inverse(mat_t<T, N, N> const & lu)

  488. {

  489.     mat_t<T, N, N> ret { 0 };

  490. 

  491.     for (int j = 0; j < N; ++j)

  492.     {

  493.         for (int i = j; i >= 0; --i)

  494.         {

  495.             T sum = 0;

  496.             for (int k = i + 1; k <= j; ++k)

  497.                 sum += ret[k][j] * lu[i][k];

  498.             ret[i][j] = T(j == i ? 1 : 0) - sum;

  499.         }

  500.     }

  501. 

  502.     return ret;

  503. }

  504. 

  505. // Compute inverse of the U matrix of an LU decomposition

  506. template<typename T, int N>

  507. mat_t<T, N, N> u_inverse(mat_t<T, N, N> const & lu)

  508. {

  509.     mat_t<T, N, N> ret { 0 };

  510. 

  511.     for (int i = 0; i < N; ++i)

  512.     {

  513.         for (int j = i; j < N; ++j)

  514.         {

  515.             T sum = 0;

  516.             for (int k = i; k < j; ++k)

  517.                 sum += ret[k][i] * lu[j][k];

  518.             ret[j][i] = ((i == j ? 1 : 0) - sum) / lu[j][j];

  519.         }

  520.     }

  521. 

  522.     return ret;

  523. }

  524. 

  525. /*

  526.  * Compute square matrix inverse

  527.  */

  528. 

  529. template<typename T, int N>

  530. mat_t<T, N, N> inverse(mat_t<T, N, N> const &m)

  531. {

  532.     auto lup = lu_decomposition(m);

  533.     auto lu = std::get<0>(lup);

  534.     auto p = std::get<1>(lup);

  535.     auto invlu = u_inverse(lu) * l_inverse(lu);

  536. 

  537.     // Rearrange columns according to the original permutation vector

  538.     mat_t<T, N, N> ret;

  539.     for (int i = 0; i < N; ++i)

  540.         ret[p[i]] = invlu[i];

  541.     return ret;

  542. }

  543. 

  544. /*

  545.  * Matrix-vector and vector-matrix multiplication

  546.  */

  547. 

  548. template<typename T, int COLS, int ROWS, int SWIZZLE>

  549. static inline vec_t<T, ROWS> operator *(mat_t<T, COLS, ROWS> const &m,

  550.                                         vec_t<T, COLS, SWIZZLE> const &v)

  551. {

  552.     vec_t<T, ROWS> ret(T(0));

  553.     for (int i = 0; i < COLS; ++i)

  554.         ret += m[i] * v[i];

  555.     return ret;

  556. }

  557. 

  558. template<typename T, int COLS, int ROWS, int SWIZZLE>

  559. static inline vec_t<T, COLS> operator *(vec_t<T, ROWS, SWIZZLE> const &v,

  560.                                         mat_t<T, COLS, ROWS> const &m)

  561. {

  562.     vec_t<T, COLS> ret(T(0));

  563.     for (int i = 0; i < COLS; ++i)

  564.         ret[i] = dot(v, m[i]);

  565.     return ret;

  566. }

  567. 

  568. /*

  569.  * Matrix-matrix multiplication

  570.  */

  571. 

  572. template<typename T, int COLS, int N, int ROWS>

  573. static inline mat_t<T, COLS, ROWS> operator *(mat_t<T, N, ROWS> const &a,

  574.                                               mat_t<T, COLS, N> const &b)

  575. {

  576.     mat_t<T, COLS, ROWS> ret;

  577.     for (int i = 0; i < COLS; ++i)

  578.         ret[i] = a * b[i];

  579.     return ret;

  580. }

  581. 

  582. template<typename T, int N>

  583. static inline mat_t<T, N, N> &operator *=(mat_t<T, N, N> &a,

  584.                                           mat_t<T, N, N> const &b)

  585. {

  586.     return a = a * b;

  587. }

  588. 

  589. /*

  590.  * Vector-vector outer product

  591.  */

  592. 

  593. template<typename T, int COLS, int ROWS>

  594. static inline mat_t<T, COLS, ROWS> outer(vec_t<T, ROWS> const &a,

  595.                                          vec_t<T, COLS> const &b)

  596. {

  597.     /* Valid cast because mat_t and vec_t have similar layouts */

  598.     return *reinterpret_cast<mat_t<T, 1, ROWS> const *>(&a)

  599.          * *reinterpret_cast<mat_t<T, COLS, 1> const *>(&b);

  600. }

  601. 

  602. /*

  603.  * Matrix-matrix outer product (Kronecker product)

  604.  */

  605. 

  606. template<typename T, int COLS1, int COLS2, int ROWS1, int ROWS2>

  607. static inline mat_t<T, COLS1 * COLS2, ROWS1 * ROWS2>

  608. outer(mat_t<T, COLS1, ROWS1> const &a, mat_t<T, COLS2, ROWS2> const &b)

  609. {

  610.     mat_t<T, COLS1 * COLS2, ROWS1 * ROWS2> ret;

  611.     for (int i1 = 0; i1 < COLS1; ++i1)

  612.         for (int i2 = 0; i2 < COLS2; ++i2)

  613.         {

  614.             /* Valid cast because mat_t and vec_t have similar layouts */

  615.             *reinterpret_cast<mat_t<T, ROWS1, ROWS2> *>(&ret[i1 * COLS2 + i2])

  616.                 = outer(b[i2], a[i1]);

  617.         }

  618.     return ret;

  619. }

  620. 

  621. /*

  622.  * Constants

  623.  */

  624. 

  625. template<typename T>

  626. mat_t<T,2,2> const mat_t<T,2,2>::identity = mat_t<T,2,2>((T)1);

  627. template<typename T>

  628. mat_t<T,3,3> const mat_t<T,3,3>::identity = mat_t<T,3,3>((T)1);

  629. template<typename T>

  630. mat_t<T,4,4> const mat_t<T,4,4>::identity = mat_t<T,4,4>((T)1);

  631. 

  632. } /* namespace lol */

  633. 

  634. #if _WIN32

  635. #   pragma pop_macro("near")

  636. #   pragma pop_macro("far")

  637. #endif