diff --git a/src/common/gsvector.cpp b/src/common/gsvector.cpp index 787fb7d66..c62da4d8a 100644 --- a/src/common/gsvector.cpp +++ b/src/common/gsvector.cpp @@ -87,25 +87,25 @@ GSMatrix4x4::GSMatrix4x4(float e00, float e01, float e02, float e03, float e10, E[3][3] = e33; } - GSMatrix4x4::GSMatrix4x4(const GSMatrix2x2& m) +GSMatrix4x4::GSMatrix4x4(const GSMatrix2x2& m) { - E[0][0] = m.E[0][0]; - E[0][1] = m.E[0][1]; - E[0][2] = 0.0f; - E[0][3] = 0.0f; - E[1][0] = m.E[1][0]; - E[1][1] = m.E[1][1]; - E[1][2] = 0.0f; - E[1][3] = 0.0f; - E[2][0] = 0.0f; - E[2][1] = 0.0f; - E[2][2] = 1.0f; - E[2][3] = 0.0f; - E[3][0] = 0.0f; - E[3][1] = 0.0f; - E[3][2] = 0.0f; - E[3][3] = 1.0f; - } + E[0][0] = m.E[0][0]; + E[0][1] = m.E[0][1]; + E[0][2] = 0.0f; + E[0][3] = 0.0f; + E[1][0] = m.E[1][0]; + E[1][1] = m.E[1][1]; + E[1][2] = 0.0f; + E[1][3] = 0.0f; + E[2][0] = 0.0f; + E[2][1] = 0.0f; + E[2][2] = 1.0f; + E[2][3] = 0.0f; + E[3][0] = 0.0f; + E[3][1] = 0.0f; + E[3][2] = 0.0f; + E[3][3] = 1.0f; +} GSMatrix4x4 GSMatrix4x4::operator*(const GSMatrix4x4& m) const { @@ -136,12 +136,41 @@ GSMatrix4x4 GSMatrix4x4::operator*(const GSMatrix4x4& m) const return res; } +GSMatrix4x4& GSMatrix4x4::operator*=(const GSMatrix4x4& m) +{ + const GSMatrix4x4 copy(*this); + +#define MultRC(rw, cl) \ + copy.E[rw][0] * m.E[0][cl] + copy.E[rw][1] * m.E[1][cl] + copy.E[rw][2] * m.E[2][cl] + copy.E[rw][3] * m.E[3][cl] + + E[0][0] = MultRC(0, 0); + E[0][1] = MultRC(0, 1); + E[0][2] = MultRC(0, 2); + E[0][3] = MultRC(0, 3); + E[1][0] = MultRC(1, 0); + E[1][1] = MultRC(1, 1); + E[1][2] = MultRC(1, 2); + E[1][3] = MultRC(1, 3); + E[2][0] = MultRC(2, 0); + E[2][1] = MultRC(2, 1); + E[2][2] = MultRC(2, 2); + E[2][3] = MultRC(2, 3); + E[3][0] = MultRC(3, 0); + E[3][1] = MultRC(3, 1); + E[3][2] = MultRC(3, 2); + E[3][3] = MultRC(3, 3); + +#undef MultRC + + return *this; +} + GSVector4 GSMatrix4x4::operator*(const GSVector4& v) const { const GSVector4 r0 = row(0); const GSVector4 r1 = row(1); const GSVector4 r2 = row(2); - const GSVector4 r3 = row(4); + const GSVector4 r3 = row(3); return GSVector4(r0.dot(v), r1.dot(v), r2.dot(v), r3.dot(v)); } @@ -199,6 +228,11 @@ GSMatrix4x4 GSMatrix4x4::RotationZ(float angle_in_radians) 0.0f, 0.0f, 1.0f); } +GSMatrix4x4 GSMatrix4x4::Translation(float x, float y, float z) +{ + return GSMatrix4x4(1.0f, 0.0f, 0.0f, x, 0.0f, 1.0f, 0.0f, y, 0.0f, 0.0f, 1.0f, z, 0.0f, 0.0f, 0.0f, 1.0f); +} + GSMatrix4x4 GSMatrix4x4::OffCenterOrthographicProjection(float left, float top, float right, float bottom, float zNear, float zFar) { @@ -222,6 +256,61 @@ GSVector4 GSMatrix4x4::col(size_t i) const return GSVector4(E[0][i], E[1][i], E[2][i], E[3][i]); } +GSMatrix4x4 GSMatrix4x4::Invert() const +{ + GSMatrix4x4 ret; + + float v0 = E[2][0] * E[3][1] - E[2][1] * E[3][0]; + float v1 = E[2][0] * E[3][2] - E[2][2] * E[3][0]; + float v2 = E[2][0] * E[3][3] - E[2][3] * E[3][0]; + float v3 = E[2][1] * E[3][2] - E[2][2] * E[3][1]; + float v4 = E[2][1] * E[3][3] - E[2][3] * E[3][1]; + float v5 = E[2][2] * E[3][3] - E[2][3] * E[3][2]; + + const float t00 = +(v5 * E[1][1] - v4 * E[1][2] + v3 * E[1][3]); + const float t10 = -(v5 * E[1][0] - v2 * E[1][2] + v1 * E[1][3]); + const float t20 = +(v4 * E[1][0] - v2 * E[1][1] + v0 * E[1][3]); + const float t30 = -(v3 * E[1][0] - v1 * E[1][1] + v0 * E[1][2]); + + const float inv_det = 1.0f / (t00 * E[0][0] + t10 * E[0][1] + t20 * E[0][2] + t30 * E[0][3]); + + ret.E[0][0] = t00 * inv_det; + ret.E[1][0] = t10 * inv_det; + ret.E[2][0] = t20 * inv_det; + ret.E[3][0] = t30 * inv_det; + + ret.E[0][1] = -(v5 * E[0][1] - v4 * E[0][2] + v3 * E[0][3]) * inv_det; + ret.E[1][1] = +(v5 * E[0][0] - v2 * E[0][2] + v1 * E[0][3]) * inv_det; + ret.E[2][1] = -(v4 * E[0][0] - v2 * E[0][1] + v0 * E[0][3]) * inv_det; + ret.E[3][1] = +(v3 * E[0][0] - v1 * E[0][1] + v0 * E[0][2]) * inv_det; + + v0 = E[1][0] * E[3][1] - E[1][1] * E[3][0]; + v1 = E[1][0] * E[3][2] - E[1][2] * E[3][0]; + v2 = E[1][0] * E[3][3] - E[1][3] * E[3][0]; + v3 = E[1][1] * E[3][2] - E[1][2] * E[3][1]; + v4 = E[1][1] * E[3][3] - E[1][3] * E[3][1]; + v5 = E[1][2] * E[3][3] - E[1][3] * E[3][2]; + + ret.E[0][2] = +(v5 * E[0][1] - v4 * E[0][2] + v3 * E[0][3]) * inv_det; + ret.E[1][2] = -(v5 * E[0][0] - v2 * E[0][2] + v1 * E[0][3]) * inv_det; + ret.E[2][2] = +(v4 * E[0][0] - v2 * E[0][1] + v0 * E[0][3]) * inv_det; + ret.E[3][2] = -(v3 * E[0][0] - v1 * E[0][1] + v0 * E[0][2]) * inv_det; + + v0 = E[2][1] * E[1][0] - E[2][0] * E[1][1]; + v1 = E[2][2] * E[1][0] - E[2][0] * E[1][2]; + v2 = E[2][3] * E[1][0] - E[2][0] * E[1][3]; + v3 = E[2][2] * E[1][1] - E[2][1] * E[1][2]; + v4 = E[2][3] * E[1][1] - E[2][1] * E[1][3]; + v5 = E[2][3] * E[1][2] - E[2][2] * E[1][3]; + + ret.E[0][3] = -(v5 * E[0][1] - v4 * E[0][2] + v3 * E[0][3]) * inv_det; + ret.E[1][3] = +(v5 * E[0][0] - v2 * E[0][2] + v1 * E[0][3]) * inv_det; + ret.E[2][3] = -(v4 * E[0][0] - v2 * E[0][1] + v0 * E[0][3]) * inv_det; + ret.E[3][3] = +(v3 * E[0][0] - v1 * E[0][1] + v0 * E[0][2]) * inv_det; + + return ret; +} + void GSMatrix4x4::store(void* m) { std::memcpy(m, &E[0][0], sizeof(E)); diff --git a/src/common/gsvector.h b/src/common/gsvector.h index 106e29b35..4a3b1306c 100644 --- a/src/common/gsvector.h +++ b/src/common/gsvector.h @@ -47,6 +47,7 @@ public: GSMatrix4x4(const GSMatrix2x2& m); GSMatrix4x4 operator*(const GSMatrix4x4& m) const; + GSMatrix4x4& operator*=(const GSMatrix4x4& m); GSVector4 operator*(const GSVector4& v) const; @@ -55,6 +56,7 @@ public: static GSMatrix4x4 RotationX(float angle_in_radians); static GSMatrix4x4 RotationY(float angle_in_radians); static GSMatrix4x4 RotationZ(float angle_in_radians); + static GSMatrix4x4 Translation(float x, float y, float z); static GSMatrix4x4 OffCenterOrthographicProjection(float left, float top, float right, float bottom, float zNear, float zFar); @@ -63,6 +65,8 @@ public: GSVector4 row(size_t i) const; GSVector4 col(size_t i) const; + GSMatrix4x4 Invert() const; + void store(void* m); float E[4][4];