diff --git a/include/misc/Maths.h b/include/misc/Maths.h index 885d6e8..7004d5c 100644 --- a/include/misc/Maths.h +++ b/include/misc/Maths.h @@ -46,7 +46,10 @@ Vec3 Cross(const Vec3& vect, const Vec3& other) { }; } - +template +T Dot(const Vec4& vect, const Vec4& other) { + return vect.x * other.x + vect.y * other.y + vect.z * other.z + vect.w * other.w; +} ////////////////////////////////////////////////////////////////// // Matricies // @@ -54,6 +57,8 @@ Vec3 Cross(const Vec3& vect, const Vec3& other) { template struct Mat4 { + static const std::size_t MATRIX_SIZE = 4; + T x0, x1, x2, x3; T y0, y1, y2, y3; T z0, z1, z2, z3; @@ -66,16 +71,44 @@ struct Mat4 { T& operator[] (std::size_t offset) { return reinterpret_cast(this)[offset]; } + + T at(std::size_t row, std::size_t column) const { + return operator[](row * MATRIX_SIZE + column); + } + + T& at(std::size_t row, std::size_t column) { + return operator[](row * MATRIX_SIZE + column); + } }; typedef Mat4 Mat4f; typedef Mat4 Mat4i; typedef Mat4 Mat4d; -Mat4f Perspective(float fovY, float aspectRatio, float zNear, float zFar); -Mat4f Look(const Vec3f& eye, const Vec3f& center, const Vec3f& up); +template +Vec4 Dot(const Mat4& mat, const Vec4& vect) { + return { + Dot(*reinterpret_cast*>(&mat), vect), + Dot(*reinterpret_cast*>(&mat[Mat4::MATRIX_SIZE]), vect), + Dot(*reinterpret_cast*>(&mat[2 * Mat4::MATRIX_SIZE]), vect), + Dot(*reinterpret_cast*>(&mat[3 * Mat4::MATRIX_SIZE]), vect), + }; +} -Mat4f Dot(const Mat4f& mat, const Mat4f& other); +template +Mat4 Dot(const Mat4& mat, const Mat4& other) { + Mat4 result {}; + + for (std::size_t i = 0; i < Mat4::MATRIX_SIZE; i++) { + for (std::size_t j = 0; j < Mat4::MATRIX_SIZE; j++) { + for (std::size_t k = 0; k < Mat4::MATRIX_SIZE; k++) { + result.at(i, j) = mat.at(i, k) * other.at(k, j); + } + } + } + + return result; +} template Mat4 Identity() { @@ -109,5 +142,10 @@ Mat4 Transpose(const Mat4& mat) { return result; } +Mat4f Perspective(float fovY, float aspectRatio, float zNear, float zFar); +Mat4f Look(const Vec3f& eye, const Vec3f& center, const Vec3f& up); + +Mat4f Inverse(const Mat4f& mat); + } // namespace maths } // namespace td diff --git a/src/misc/Maths.cpp b/src/misc/Maths.cpp index 2382693..515327e 100644 --- a/src/misc/Maths.cpp +++ b/src/misc/Maths.cpp @@ -38,20 +38,47 @@ Mat4f Look(const Vec3f& eye, const Vec3f& front, const Vec3f& up) { return result; } -Mat4f Dot(const Mat4f& mat, const Mat4f& other) { - Mat4f result {}; +Mat4f Inverse(const Mat4f& mat) { + float s0 = mat.at(0, 0) * mat.at(1, 1) - mat.at(1, 0) * mat.at(0, 1); + float s1 = mat.at(0, 0) * mat.at(1, 2) - mat.at(1, 0) * mat.at(0, 2); + float s2 = mat.at(0, 0) * mat.at(1, 3) - mat.at(1, 0) * mat.at(0, 3); + float s3 = mat.at(0, 1) * mat.at(1, 2) - mat.at(1, 1) * mat.at(0, 2); + float s4 = mat.at(0, 1) * mat.at(1, 3) - mat.at(1, 1) * mat.at(0, 3); + float s5 = mat.at(0, 2) * mat.at(1, 3) - mat.at(1, 2) * mat.at(0, 3); - static const std::size_t MAT_SIZE = 4; + float c5 = mat.at(2, 2) * mat.at(3, 3) - mat.at(3, 2) * mat.at(2, 3); + float c4 = mat.at(2, 1) * mat.at(3, 3) - mat.at(3, 1) * mat.at(2, 3); + float c3 = mat.at(2, 1) * mat.at(3, 2) - mat.at(3, 1) * mat.at(2, 2); + float c2 = mat.at(2, 0) * mat.at(3, 3) - mat.at(3, 0) * mat.at(2, 3); + float c1 = mat.at(2, 0) * mat.at(3, 2) - mat.at(3, 0) * mat.at(2, 2); + float c0 = mat.at(2, 0) * mat.at(3, 1) - mat.at(3, 0) * mat.at(2, 1); - for (std::size_t i = 0; i < MAT_SIZE; i++) { - for (std::size_t j = 0; j < MAT_SIZE; j++) { - for (std::size_t k = 0; k < MAT_SIZE; k++) { - result[i * MAT_SIZE + j] += mat[i * MAT_SIZE + k] * other[k * MAT_SIZE + j]; - } - } - } + // Should check for 0 determinant + float invdet = 1.0 / (s0 * c5 - s1 * c4 + s2 * c3 + s3 * c2 - s4 * c1 + s5 * c0); - return result; + Mat4f result; + + result.at(0, 0) = ( mat.at(1, 1) * c5 - mat.at(1, 2) * c4 + mat.at(1, 3) * c3) * invdet; + result.at(0, 1) = (-mat.at(0, 1) * c5 + mat.at(0, 2) * c4 - mat.at(0, 3) * c3) * invdet; + result.at(0, 2) = ( mat.at(3, 1) * s5 - mat.at(3, 2) * s4 + mat.at(3, 3) * s3) * invdet; + result.at(0, 3) = (-mat.at(2, 1) * s5 + mat.at(2, 2) * s4 - mat.at(2, 3) * s3) * invdet; + + result.at(1, 0) = (-mat.at(1, 0) * c5 + mat.at(1, 2) * c2 - mat.at(1, 3) * c1) * invdet; + result.at(1, 1) = ( mat.at(0, 0) * c5 - mat.at(0, 2) * c2 + mat.at(0, 3) * c1) * invdet; + result.at(1, 2) = (-mat.at(3, 0) * s5 + mat.at(3, 2) * s2 - mat.at(3, 3) * s1) * invdet; + result.at(1, 3) = ( mat.at(2, 0) * s5 - mat.at(2, 2) * s2 + mat.at(2, 3) * s1) * invdet; + + result.at(2, 0) = ( mat.at(1, 0) * c4 - mat.at(1, 1) * c2 + mat.at(1, 3) * c0) * invdet; + result.at(2, 1) = (-mat.at(0, 0) * c4 + mat.at(0, 1) * c2 - mat.at(0, 3) * c0) * invdet; + result.at(2, 2) = ( mat.at(3, 0) * s4 - mat.at(3, 1) * s2 + mat.at(3, 3) * s0) * invdet; + result.at(2, 3) = (-mat.at(2, 0) * s4 + mat.at(2, 1) * s2 - mat.at(2, 3) * s0) * invdet; + + result.at(3, 0) = (-mat.at(1, 0) * c3 + mat.at(1, 1) * c1 - mat.at(1, 2) * c0) * invdet; + result.at(3, 1) = ( mat.at(0, 0) * c3 - mat.at(0, 1) * c1 + mat.at(0, 2) * c0) * invdet; + result.at(3, 2) = (-mat.at(3, 0) * s3 + mat.at(3, 1) * s1 - mat.at(3, 2) * s0) * invdet; + result.at(3, 3) = ( mat.at(2, 0) * s3 - mat.at(2, 1) * s1 + mat.at(2, 2) * s0) * invdet; + + return result; } } // namespace maths