143 lines
5.7 KiB
C++
143 lines
5.7 KiB
C++
// =====================================================================================================================
|
|
// fennec, a free and open source game engine
|
|
// Copyright © 2025 - 2026 Medusa Slockbower
|
|
//
|
|
// This program is free software: you can redistribute it and/or modify
|
|
// it under the terms of the GNU General Public License as published by
|
|
// the Free Software Foundation, either version 3 of the License, or
|
|
// (at your option) any later version.
|
|
//
|
|
// This program is distributed in the hope that it will be useful,
|
|
// but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
// GNU General Public License for more details.
|
|
//
|
|
// You should have received a copy of the GNU General Public License
|
|
// along with this program. If not, see <https://www.gnu.org/licenses/>.
|
|
// =====================================================================================================================
|
|
|
|
#ifndef FENNEC_MATH_DETAIL_MATRIX_H
|
|
#define FENNEC_MATH_DETAIL_MATRIX_H
|
|
|
|
#include <fennec/math/detail/_fwd.h>
|
|
|
|
namespace fennec
|
|
{
|
|
|
|
// specialization for mat2
|
|
template<typename scalar, size_t rows, size_t...cols> requires(rows == 2 && sizeof...(cols) == 2)
|
|
constexpr scalar determinant(const matrix<scalar, rows, cols...>& m) {
|
|
return m[0][0] * m[1][1] - m[1][0] * m[0][1];
|
|
}
|
|
|
|
// specialization for mat3
|
|
template<typename scalar, size_t rows, size_t...cols> requires(rows == 3 && sizeof...(cols) == 3)
|
|
constexpr scalar determinant(const matrix<scalar, rows, cols...>& m) {
|
|
// Cofactor expansion over first column
|
|
return m[0][0] * (m[1][1] * m[2][2] - m[1][2] * m[2][1])
|
|
- m[0][1] * (m[1][0] * m[2][2] - m[1][2] * m[2][0])
|
|
+ m[0][2] * (m[1][0] * m[2][1] - m[1][1] * m[2][0]);
|
|
}
|
|
|
|
// specialization for mat4
|
|
template<typename scalar, size_t rows, size_t...cols> requires(rows == 4 && sizeof...(cols) == 4)
|
|
constexpr scalar determinant(const matrix<scalar, rows, cols...>& m) {
|
|
// Cofactor expansion over the first column
|
|
scalar d00 = m[2][2] * m[3][3] - m[3][2] * m[2][3];
|
|
scalar d01 = m[2][1] * m[3][3] - m[3][1] * m[2][3];
|
|
scalar d02 = m[2][1] * m[3][2] - m[3][1] * m[2][2];
|
|
scalar d03 = m[2][0] * m[3][3] - m[3][0] * m[2][3];
|
|
scalar d04 = m[2][0] * m[3][2] - m[3][0] * m[2][2];
|
|
scalar d05 = m[2][0] * m[3][1] - m[3][0] * m[2][1];
|
|
|
|
scalar f0 = +(m[1][1]*d00 - m[1][2]*d01 + m[1][3]*d02);
|
|
scalar f1 = -(m[1][0]*d00 - m[1][2]*d03 + m[1][3]*d04);
|
|
scalar f2 = +(m[1][0]*d01 - m[1][1]*d03 + m[1][3]*d05);
|
|
scalar f3 = -(m[1][0]*d02 - m[1][1]*d04 + m[1][2]*d05);
|
|
|
|
return m[0][0]*f0 + m[0][1]*f1 + m[0][2]*f2 + m[0][3]*f3;
|
|
}
|
|
|
|
|
|
// specialization for mat2
|
|
template<typename scalar, size_t rows, size_t...cols> requires(rows == 2 && sizeof...(cols) == 2)
|
|
constexpr matrix<scalar, rows, cols...> inverse(const matrix<scalar, rows, cols...>& m) {
|
|
scalar di = scalar(1) / fennec::determinant(m);
|
|
return matrix<scalar, rows, cols...>(
|
|
m[1][1] * di, -m[0][1] * di, -m[1][0] * di, m[0][0] * di
|
|
);
|
|
}
|
|
|
|
// specialization for mat3
|
|
template<typename scalar, size_t rows, size_t...cols> requires(rows == 3 && sizeof...(cols) == 3)
|
|
constexpr matrix<scalar, rows, cols...> inverse(const matrix<scalar, rows, cols...>& m) {
|
|
scalar di = scalar(1) / fennec::determinant(m);
|
|
|
|
matrix<scalar, rows, cols...> i(
|
|
(m[1][1] * m[2][2] - m[2][1] * m[1][2]) * di
|
|
, -(m[0][1] * m[2][2] - m[2][1] * m[0][2]) * di
|
|
, (m[0][1] * m[1][2] - m[1][1] * m[0][2]) * di
|
|
|
|
, -(m[1][0] * m[2][2] - m[2][0] * m[1][2]) * di
|
|
, (m[0][0] * m[2][2] - m[2][0] * m[0][2]) * di
|
|
, -(m[0][0] * m[1][2] - m[1][0] * m[0][2]) * di
|
|
|
|
, (m[1][0] * m[2][1] - m[2][0] * m[1][1]) * di
|
|
, -(m[0][0] * m[2][1] - m[2][0] * m[0][1]) * di
|
|
, (m[0][0] * m[1][1] - m[1][0] * m[0][1]) * di
|
|
);
|
|
|
|
return i;
|
|
}
|
|
|
|
template<typename scalar, size_t rows, size_t...cols> requires(rows == 4 && sizeof...(cols) == 4)
|
|
constexpr matrix<scalar, rows, cols...> inverse(const matrix<scalar, rows, cols...>& m) {
|
|
scalar s0 = m[0][0] * m[1][1] - m[1][0] * m[0][1];
|
|
scalar s1 = m[0][0] * m[1][2] - m[1][0] * m[0][2];
|
|
scalar s2 = m[0][0] * m[1][3] - m[1][0] * m[0][3];
|
|
scalar s3 = m[0][1] * m[1][2] - m[1][1] * m[0][2];
|
|
scalar s4 = m[0][1] * m[1][3] - m[1][1] * m[0][3];
|
|
scalar s5 = m[0][2] * m[1][3] - m[1][2] * m[0][3];
|
|
|
|
scalar c5 = m[2][2] * m[3][3] - m[3][2] * m[2][3];
|
|
scalar c4 = m[2][1] * m[3][3] - m[3][1] * m[2][3];
|
|
scalar c3 = m[2][1] * m[3][2] - m[3][1] * m[2][2];
|
|
scalar c2 = m[2][0] * m[3][3] - m[3][0] * m[2][3];
|
|
scalar c1 = m[2][0] * m[3][2] - m[3][0] * m[2][2];
|
|
scalar c0 = m[2][0] * m[3][1] - m[3][0] * m[2][1];
|
|
|
|
// Should check for 0 determinant
|
|
scalar invdet = (s0 * c5 - s1 * c4 + s2 * c3 + s3 * c2 - s4 * c1 + s5 * c0);
|
|
invdet = invdet ? scalar(1.0) / invdet : scalar(0); // this will get reduced to cmov, div still happens
|
|
|
|
matrix<scalar, rows, cols...> i(
|
|
( m[1][1] * c5 - m[1][2] * c4 + m[1][3] * c3) * invdet
|
|
, (-m[0][1] * c5 + m[0][2] * c4 - m[0][3] * c3) * invdet
|
|
, ( m[3][1] * s5 - m[3][2] * s4 + m[3][3] * s3) * invdet
|
|
, (-m[2][1] * s5 + m[2][2] * s4 - m[2][3] * s3) * invdet
|
|
|
|
, (-m[1][0] * c5 + m[1][2] * c2 - m[1][3] * c1) * invdet
|
|
, ( m[0][0] * c5 - m[0][2] * c2 + m[0][3] * c1) * invdet
|
|
, (-m[3][0] * s5 + m[3][2] * s2 - m[3][3] * s1) * invdet
|
|
, ( m[2][0] * s5 - m[2][2] * s2 + m[2][3] * s1) * invdet
|
|
|
|
, ( m[1][0] * c4 - m[1][1] * c2 + m[1][3] * c0) * invdet
|
|
, (-m[0][0] * c4 + m[0][1] * c2 - m[0][3] * c0) * invdet
|
|
, ( m[3][0] * s4 - m[3][1] * s2 + m[3][3] * s0) * invdet
|
|
, (-m[2][0] * s4 + m[2][1] * s2 - m[2][3] * s0) * invdet
|
|
|
|
, (-m[1][0] * c3 + m[1][1] * c1 - m[1][2] * c0) * invdet
|
|
, ( m[0][0] * c3 - m[0][1] * c1 + m[0][2] * c0) * invdet
|
|
, (-m[3][0] * s3 + m[3][1] * s1 - m[3][2] * s0) * invdet
|
|
, ( m[2][0] * s3 - m[2][1] * s1 + m[2][2] * s0) * invdet
|
|
);
|
|
|
|
return i;
|
|
}
|
|
|
|
|
|
}
|
|
|
|
|
|
#endif // FENNEC_MATH_DETAIL_MATRIX_H
|