From 94dec52caedd878d221156b92de62fac10ef27c0 Mon Sep 17 00:00:00 2001
From: Phillip Stephens <antidote.crk@gmail.com>
Date: Tue, 10 Jan 2023 15:03:38 -0800
Subject: [PATCH] Add CMatrix4f, nearly matches

Former-commit-id: dcb2ebb2fd920436a507dd4524ed5e3814148896
---
 configure.py                     |  2 +-
 include/Kyoto/Math/CMatrix4f.hpp | 10 +++++-
 src/Kyoto/Math/CMatrix4f.cpp     | 52 ++++++++++++++++++++++++++++++++
 3 files changed, 62 insertions(+), 2 deletions(-)
 create mode 100644 src/Kyoto/Math/CMatrix4f.cpp

diff --git a/configure.py b/configure.py
index 57c45d85..b143b7ff 100755
--- a/configure.py
+++ b/configure.py
@@ -563,7 +563,7 @@ LIBS = [
             ["Kyoto/Graphics/DolphinCTexture", False],
             ["Kyoto/Math/CloseEnough", True],
             "Kyoto/Math/CMatrix3f",
-            "Kyoto/Math/CMatrix4f",
+            ["Kyoto/Math/CMatrix4f", False],
             "Kyoto/Math/CQuaternion",
             ["Kyoto/CRandom16", True],
             "Kyoto/Math/CTransform4f",
diff --git a/include/Kyoto/Math/CMatrix4f.hpp b/include/Kyoto/Math/CMatrix4f.hpp
index cd79c002..1d03dbaf 100644
--- a/include/Kyoto/Math/CMatrix4f.hpp
+++ b/include/Kyoto/Math/CMatrix4f.hpp
@@ -3,14 +3,22 @@
 
 #include "types.h"
 
+#include "Kyoto/Math/CVector3f.hpp"
+
 class CMatrix4f {
 public:
   CMatrix4f(float, float, float, float, float, float, float, float, float, float, float, float,
             float, float, float, float);
 
-  CVector3f MultiplyOneOverW(const CVector3f& vec) const;
+  CVector3f operator*(const CVector3f& vec) const;
 
+  CVector3f MultiplyOneOverW(const CVector3f& vec) const;
+  float MultiplyGetW(const CVector3f& vec) const;
+
+  static const CMatrix4f& Identity() { return sIdentity; }
+  
 private:
+  static const CMatrix4f sIdentity;
   float m00;
   float m01;
   float m02;
diff --git a/src/Kyoto/Math/CMatrix4f.cpp b/src/Kyoto/Math/CMatrix4f.cpp
new file mode 100644
index 00000000..97788f07
--- /dev/null
+++ b/src/Kyoto/Math/CMatrix4f.cpp
@@ -0,0 +1,52 @@
+#include "Kyoto/Math/CMatrix4f.hpp"
+
+const CMatrix4f CMatrix4f::sIdentity =
+    CMatrix4f(1.f, 0.f, 0.f, 0.f, 0.f, 1.f, 0.f, 0.f, 0.f, 0.f, 1.f, 0.f, 0.f, 0.f, 0.f, 1.f);
+
+CMatrix4f::CMatrix4f(float m00, float m01, float m02, float m03, float m10, float m11, float m12,
+                     float m13, float m20, float m21, float m22, float m23, float m30, float m31,
+                     float m32, float m33)
+: m00(m00)
+, m01(m01)
+, m02(m02)
+, m03(m03)
+, m10(m10)
+, m11(m11)
+, m12(m12)
+, m13(m13)
+, m20(m20)
+, m21(m21)
+, m22(m22)
+, m23(m23)
+, m30(m30)
+, m31(m31)
+, m32(m32)
+, m33(m33) {}
+
+CVector3f CMatrix4f::operator*(const CVector3f& vec) const {
+  const float vecX = vec.GetX();
+  const float vecY = vec.GetY();
+  const float vecZ = vec.GetZ();
+  const float x = (vecX * m00) + (vecY * m01) + (vecZ * m02) + m03;
+  const float y = (vecX * m10) + (vecY * m11) + (vecZ * m12) + m13;
+  const float z = (vecX * m20) + (vecY * m21) + (vecZ * m22) + m23;
+  return CVector3f(x, y, z);
+}
+
+CVector3f CMatrix4f::MultiplyOneOverW(const CVector3f& vec) const {
+  const float vecX = vec.GetX();
+  const float vecY = vec.GetY();
+  const float vecZ = vec.GetZ();
+  const float w = 1.f / ((vecX * m30) + (vecY * m31) + (vecZ * m32) + m33);
+  const float x = (vecX * m00) + (vecY * m01) + (vecZ * m02) + m03;
+  const float y = (vecX * m10) + (vecY * m11) + (vecZ * m12) + m13;
+  const float z = (vecX * m20) + (vecY * m21) + (vecZ * m22) + m23;
+  return CVector3f(x * w, y * w, z * w);
+}
+
+float CMatrix4f::MultiplyGetW(const CVector3f& vec) const {
+  const float vecX = vec.GetX();
+  const float vecY = vec.GetY();
+  const float vecZ = vec.GetZ();
+  return ((vecX * m30) + (vecY * m31) + (vecZ * m32) + m33);
+}