Skip to content

Commit 69b2c6c

Browse files
jnyfahjnyfah
committed
added inverse and identity
1 parent 81f5e50 commit 69b2c6c

File tree

2 files changed

+208
-9
lines changed

2 files changed

+208
-9
lines changed

‎ZEngine/ZEngine/Core/Maths/Matrix.h

Lines changed: 165 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -34,7 +34,7 @@ namespace ZEngine::Core::Maths
3434
const RowProxy operator[](size_t row) const
3535
{
3636
ZENGINE_VALIDATE_ASSERT(row < R, "Row index out of range");
37-
return RowProxy{&m_data[row * C]};
37+
return RowProxy{const_cast<T*>(&m_data[row * C])};
3838
}
3939

4040
Vec<T, C> getRow(size_t row) const
@@ -59,11 +59,29 @@ namespace ZEngine::Core::Maths
5959
return result;
6060
}
6161

62-
Matrix<T, R, C> identity()
62+
Matrix<T, R, C> operator+(Matrix<T, R, C>& other)
6363
{
6464
Matrix<T, R, C> result{};
65-
for (size_t i = 0; i < C; ++i)
66-
result[i][i] = T(1);
65+
for (size_t i = 0; i < R; ++i)
66+
{
67+
for (size_t j = 0; j < C; j++)
68+
{
69+
result[i][j] = other[i][j] + (*this)[i][j];
70+
}
71+
}
72+
return result;
73+
}
74+
75+
Matrix<T, R, C> operator-(Matrix<T, R, C>& other)
76+
{
77+
Matrix<T, R, C> result{};
78+
for (size_t i = 0; i < R; ++i)
79+
{
80+
for (size_t j = 0; j < C; j++)
81+
{
82+
result[i][j] = (*this)[i][j] - other[i][j];
83+
}
84+
}
6785
return result;
6886
}
6987
};
@@ -86,6 +104,12 @@ namespace ZEngine::Core::Maths
86104
this->m_data[3] = m11;
87105
}
88106

107+
Mat2(const Matrix<T, 2, 2>& other)
108+
{
109+
for (size_t i = 0; i < 4; ++i)
110+
this->m_data[i] = other.m_data[i];
111+
}
112+
89113
T determinant() const
90114
{
91115
return this->m_data[0] * this->m_data[3] - this->m_data[1] * this->m_data[2];
@@ -157,6 +181,15 @@ namespace ZEngine::Core::Maths
157181
}
158182
return *this;
159183
}
184+
Mat2 inverse()
185+
{
186+
T det = this->determinant();
187+
ZENGINE_VALIDATE_ASSERT(det != 0, "Matrix is singular and cannot be inverted");
188+
189+
T invDet = 1 / det;
190+
191+
return Mat2<T>(this->m_data[3] * invDet, -this->m_data[1] * invDet, -this->m_data[2] * invDet, this->m_data[0] * invDet);
192+
}
160193
};
161194

162195
template <typename T>
@@ -165,7 +198,7 @@ namespace ZEngine::Core::Maths
165198

166199
Mat3()
167200
{
168-
for (size_t i = 0; i < 6; ++i)
201+
for (size_t i = 0; i < 9; ++i)
169202
this->m_data[i] = 0;
170203
}
171204

@@ -182,6 +215,12 @@ namespace ZEngine::Core::Maths
182215
this->m_data[8] = m22;
183216
}
184217

218+
Mat3(const Matrix<T, 3, 3>& other)
219+
{
220+
for (size_t i = 0; i < 9; ++i)
221+
this->m_data[i] = other.m_data[i];
222+
}
223+
185224
T determinant() const
186225
{
187226
return this->m_data[0] * (this->m_data[4] * this->m_data[8] - this->m_data[5] * this->m_data[7]) - this->m_data[1] * (this->m_data[3] * this->m_data[8] - this->m_data[5] * this->m_data[6]) + this->m_data[2] * (this->m_data[3] * this->m_data[7] - this->m_data[4] * this->m_data[6]);
@@ -252,6 +291,25 @@ namespace ZEngine::Core::Maths
252291
}
253292
return *this;
254293
}
294+
295+
Mat3 inverse()
296+
{
297+
T det = this->determinant();
298+
ZENGINE_VALIDATE_ASSERT(det != 0, "Matrix is singular and cannot be inverted");
299+
300+
T invDet = 1 / det;
301+
302+
return Mat3<T>(
303+
(this->m_data[4] * this->m_data[8] - this->m_data[5] * this->m_data[7]) * invDet,
304+
-(this->m_data[1] * this->m_data[8] - this->m_data[2] * this->m_data[7]) * invDet,
305+
(this->m_data[1] * this->m_data[5] - this->m_data[2] * this->m_data[4]) * invDet,
306+
-(this->m_data[3] * this->m_data[8] - this->m_data[5] * this->m_data[6]) * invDet,
307+
(this->m_data[0] * this->m_data[8] - this->m_data[2] * this->m_data[6]) * invDet,
308+
-(this->m_data[0] * this->m_data[5] - this->m_data[2] * this->m_data[3]) * invDet,
309+
(this->m_data[3] * this->m_data[7] - this->m_data[4] * this->m_data[6]) * invDet,
310+
-(this->m_data[0] * this->m_data[7] - this->m_data[1] * this->m_data[6]) * invDet,
311+
(this->m_data[0] * this->m_data[4] - this->m_data[1] * this->m_data[3]) * invDet);
312+
}
255313
};
256314

257315
template <typename T>
@@ -284,17 +342,23 @@ namespace ZEngine::Core::Maths
284342
this->m_data[15] = m33;
285343
}
286344

345+
Mat4(const Matrix<T, 4, 4>& other)
346+
{
347+
for (size_t i = 0; i < 16; ++i)
348+
this->m_data[i] = other.m_data[i];
349+
}
350+
287351
Mat4& operator+=(const Mat4& other)
288352
{
289-
for (size_t i = 0; i < 8; i++)
353+
for (size_t i = 0; i < 16; i++)
290354
{
291355
this->m_data[i] += other.m_data[i];
292356
}
293357
return *this;
294358
}
295359
Mat4& operator-=(const Mat4& other)
296360
{
297-
for (size_t i = 0; i < 8; i++)
361+
for (size_t i = 0; i < 16; i++)
298362
{
299363
this->m_data[i] -= other.m_data[i];
300364
}
@@ -352,12 +416,64 @@ namespace ZEngine::Core::Maths
352416
}
353417

354418
T determinant() const
419+
{
420+
return (
421+
this->m_data[0] * (this->m_data[5] * (this->m_data[10] * this->m_data[15] - this->m_data[11] * this->m_data[14]) - this->m_data[6] * (this->m_data[9] * this->m_data[15] - this->m_data[11] * this->m_data[13]) + this->m_data[7] * (this->m_data[9] * this->m_data[14] - this->m_data[10] * this->m_data[13])) -
422+
this->m_data[1] * (this->m_data[4] * (this->m_data[10] * this->m_data[15] - this->m_data[11] * this->m_data[14]) - this->m_data[6] * (this->m_data[8] * this->m_data[15] - this->m_data[11] * this->m_data[12]) + this->m_data[7] * (this->m_data[8] * this->m_data[14] - this->m_data[10] * this->m_data[12])) +
423+
this->m_data[2] * (this->m_data[4] * (this->m_data[9] * this->m_data[15] - this->m_data[11] * this->m_data[13]) - this->m_data[5] * (this->m_data[8] * this->m_data[15] - this->m_data[11] * this->m_data[12]) + this->m_data[7] * (this->m_data[8] * this->m_data[13] - this->m_data[9] * this->m_data[12])) -
424+
this->m_data[3] * (this->m_data[4] * (this->m_data[9] * this->m_data[14] - this->m_data[10] * this->m_data[13]) - this->m_data[5] * (this->m_data[8] * this->m_data[14] - this->m_data[10] * this->m_data[12]) + this->m_data[6] * (this->m_data[8] * this->m_data[13] - this->m_data[9] * this->m_data[12])));
425+
}
426+
Mat4<T> inverse() const
355427
{
356428
const T* m = this->m_data;
357429

358-
T det = m[0] * (m[5] * (m[10] * m[15] - m[11] * m[14]) - m[6] * (m[9] * m[15] - m[11] * m[13]) + m[7] * (m[9] * m[14] - m[10] * m[13])) - m[1] * (m[4] * (m[10] * m[15] - m[11] * m[14]) - m[6] * (m[8] * m[15] - m[11] * m[12]) + m[7] * (m[8] * m[14] - m[10] * m[12])) + m[2] * (m[4] * (m[9] * m[15] - m[11] * m[13]) - m[5] * (m[8] * m[15] - m[11] * m[12]) + m[7] * (m[8] * m[13] - m[9] * m[12])) - m[3] * (m[4] * (m[9] * m[14] - m[10] * m[13]) - m[5] * (m[8] * m[14] - m[10] * m[12]) + m[6] * (m[8] * m[13] - m[9] * m[12]));
430+
T c00 = m[5] * (m[10] * m[15] - m[11] * m[14]) - m[6] * (m[9] * m[15] - m[11] * m[13]) + m[7] * (m[9] * m[14] - m[10] * m[13]);
431+
T c01 = -(m[4] * (m[10] * m[15] - m[11] * m[14]) - m[6] * (m[8] * m[15] - m[11] * m[12]) + m[7] * (m[8] * m[14] - m[10] * m[12]));
432+
T c02 = m[4] * (m[9] * m[15] - m[11] * m[13]) - m[5] * (m[8] * m[15] - m[11] * m[12]) + m[7] * (m[8] * m[13] - m[9] * m[12]);
433+
T c03 = -(m[4] * (m[9] * m[14] - m[10] * m[13]) - m[5] * (m[8] * m[14] - m[10] * m[12]) + m[6] * (m[8] * m[13] - m[9] * m[12]));
434+
435+
T c10 = -(m[1] * (m[10] * m[15] - m[11] * m[14]) - m[2] * (m[9] * m[15] - m[11] * m[13]) + m[3] * (m[9] * m[14] - m[10] * m[13]));
436+
T c11 = m[0] * (m[10] * m[15] - m[11] * m[14]) - m[2] * (m[8] * m[15] - m[11] * m[12]) + m[3] * (m[8] * m[14] - m[10] * m[12]);
437+
T c12 = -(m[0] * (m[9] * m[15] - m[11] * m[13]) - m[1] * (m[8] * m[15] - m[11] * m[12]) + m[3] * (m[8] * m[13] - m[9] * m[12]));
438+
T c13 = m[0] * (m[9] * m[14] - m[10] * m[13]) - m[1] * (m[8] * m[14] - m[10] * m[12]) + m[2] * (m[8] * m[13] - m[9] * m[12]);
439+
440+
T c20 = m[1] * (m[6] * m[15] - m[7] * m[14]) - m[2] * (m[5] * m[15] - m[7] * m[13]) + m[3] * (m[5] * m[14] - m[6] * m[13]);
441+
T c21 = -(m[0] * (m[6] * m[15] - m[7] * m[14]) - m[2] * (m[4] * m[15] - m[7] * m[12]) + m[3] * (m[4] * m[14] - m[6] * m[12]));
442+
T c22 = m[0] * (m[5] * m[15] - m[7] * m[13]) - m[1] * (m[4] * m[15] - m[7] * m[12]) + m[3] * (m[4] * m[13] - m[5] * m[12]);
443+
T c23 = -(m[0] * (m[5] * m[14] - m[6] * m[13]) - m[1] * (m[4] * m[14] - m[6] * m[12]) + m[2] * (m[4] * m[13] - m[5] * m[12]));
444+
445+
T c30 = -(m[1] * (m[6] * m[11] - m[7] * m[10]) - m[2] * (m[5] * m[11] - m[7] * m[9]) + m[3] * (m[5] * m[10] - m[6] * m[9]));
446+
T c31 = m[0] * (m[6] * m[11] - m[7] * m[10]) - m[2] * (m[4] * m[11] - m[7] * m[8]) + m[3] * (m[4] * m[10] - m[6] * m[8]);
447+
T c32 = -(m[0] * (m[5] * m[11] - m[7] * m[9]) - m[1] * (m[4] * m[11] - m[7] * m[8]) + m[3] * (m[4] * m[9] - m[5] * m[8]));
448+
T c33 = m[0] * (m[5] * m[10] - m[6] * m[9]) - m[1] * (m[4] * m[10] - m[6] * m[8]) + m[2] * (m[4] * m[9] - m[5] * m[8]);
449+
450+
T det = m[0] * c00 + m[1] * c01 + m[2] * c02 + m[3] * c03;
451+
ZENGINE_VALIDATE_ASSERT(det != 0, "Matrix is singular and cannot be inverted");
452+
453+
T invDet = static_cast<T>(1) / det;
454+
455+
Mat4<T> result;
456+
result.m_data[0] = c00 * invDet;
457+
result.m_data[1] = c10 * invDet;
458+
result.m_data[2] = c20 * invDet;
459+
result.m_data[3] = c30 * invDet;
460+
461+
result.m_data[4] = c01 * invDet;
462+
result.m_data[5] = c11 * invDet;
463+
result.m_data[6] = c21 * invDet;
464+
result.m_data[7] = c31 * invDet;
465+
466+
result.m_data[8] = c02 * invDet;
467+
result.m_data[9] = c12 * invDet;
468+
result.m_data[10] = c22 * invDet;
469+
result.m_data[11] = c32 * invDet;
470+
471+
result.m_data[12] = c03 * invDet;
472+
result.m_data[13] = c13 * invDet;
473+
result.m_data[14] = c23 * invDet;
474+
result.m_data[15] = c33 * invDet;
359475

360-
return det;
476+
return result;
361477
}
362478
};
363479

@@ -366,4 +482,44 @@ namespace ZEngine::Core::Maths
366482
using Mat3f = Mat3<float>;
367483
using Mat4f = Mat4<float>;
368484

485+
template <typename T>
486+
T identity();
487+
488+
template <>
489+
inline Mat2f identity<Mat2f>()
490+
{
491+
return Mat2f(1, 0, 0, 1);
492+
}
493+
494+
template <>
495+
inline Mat3f identity<Mat3f>()
496+
{
497+
return Mat3f(1, 0, 0, 0, 1, 0, 0, 0, 1);
498+
}
499+
500+
template <>
501+
inline Mat4f identity<Mat4f>()
502+
{
503+
return Mat4f(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1);
504+
}
505+
506+
template <typename T, size_t R, size_t K, size_t C>
507+
Matrix<T, R, C> operator*(const Matrix<T, R, K>& a, const Matrix<T, K, C>& b)
508+
{
509+
Matrix<T, R, C> result{};
510+
for (size_t i = 0; i < R; ++i)
511+
{
512+
for (size_t j = 0; j < C; ++j)
513+
{
514+
T sum = T{};
515+
for (size_t k = 0; k < K; ++k)
516+
{
517+
sum += a[i][k] * b[k][j];
518+
}
519+
result[i][j] = sum;
520+
}
521+
}
522+
return result;
523+
}
524+
369525
} // namespace ZEngine::Core::Maths

‎ZEngine/tests/Maths/Matrix_test.cpp

Lines changed: 43 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -69,3 +69,46 @@ TEST(MatrixTest, RowAndColumnAccessFloat)
6969
EXPECT_NEAR(col[1], 6.0f, EPSILON);
7070
EXPECT_NEAR(col[2], 9.0f, EPSILON);
7171
}
72+
73+
TEST(MatrixTest, Identity)
74+
{
75+
Mat2f I2 = identity<Mat2f>();
76+
Mat3f I3 = identity<Mat3f>();
77+
Mat4f I4 = identity<Mat4f>();
78+
79+
for (int i = 0; i < 2; ++i)
80+
for (int j = 0; j < 2; ++j)
81+
EXPECT_NEAR(I2[i][j], (i == j ? 1.0f : 0.0f), EPSILON);
82+
83+
for (int i = 0; i < 3; ++i)
84+
for (int j = 0; j < 3; ++j)
85+
EXPECT_NEAR(I3[i][j], (i == j ? 1.0f : 0.0f), EPSILON);
86+
87+
for (int i = 0; i < 4; ++i)
88+
for (int j = 0; j < 4; ++j)
89+
EXPECT_NEAR(I4[i][j], (i == j ? 1.0f : 0.0f), EPSILON);
90+
}
91+
92+
TEST(MatrixTest, Inverse)
93+
{
94+
Mat2f m2(4.0f, 7.0f, 2.0f, 6.0f);
95+
Mat2f inv2 = m2.inverse();
96+
Mat2f shouldBeIdentity2 = m2 * inv2;
97+
for (int i = 0; i < 2; ++i)
98+
for (int j = 0; j < 2; ++j)
99+
EXPECT_NEAR(shouldBeIdentity2[i][j], (i == j ? 1.0f : 0.0f), EPSILON);
100+
101+
Mat3f m3(1, 2, 3, 0, 1, 4, 5, 6, 0);
102+
Mat3f inv3 = m3.inverse();
103+
Mat3f shouldBeIdentity3 = m3 * inv3;
104+
for (int i = 0; i < 3; ++i)
105+
for (int j = 0; j < 3; ++j)
106+
EXPECT_NEAR(shouldBeIdentity3[i][j], (i == j ? 1.0f : 0.0f), EPSILON);
107+
108+
Mat4f m4(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1);
109+
Mat4f inv4 = m4.inverse();
110+
Mat4f shouldBeIdentity4 = m4 * inv4;
111+
for (int i = 0; i < 4; ++i)
112+
for (int j = 0; j < 4; ++j)
113+
EXPECT_NEAR(shouldBeIdentity4[i][j], (i == j ? 1.0f : 0.0f), EPSILON);
114+
}

0 commit comments

Comments
 (0)