d_linkage is largely done (#176)

1 files changed, 47 insertions, 1 deletions

diff --git a/src/m/m_mtx.cpp b/src/m/m_mtx.cpp
index 712d612c..521cbb7a 100644
--- a/src/m/m_mtx.cpp
+++ b/src/m/m_mtx.cpp
@@ -1,6 +1,8 @@
+#include "m/m_mtx.h"
+
 #include "c/c_math.h"
 #include "m/m_math.h"
-#include "nw4r/math.h"
+#include "m/m_vec.h"
 
 void mMtx_c::toRot(mAng3_c &out) const {
     f32 t1 = 0.0f;
@@ -10,3 +12,47 @@ void mMtx_c::toRot(mAng3_c &out) const {
     }
     s16 anglex = cM::atan2s(-m[1][2], t1);
 }
+
+void mMtx_c::fn_802F1C40(s32 i, s32 j) {
+    f32 *pJ = &arr[j];
+    f32 *pI = &arr[i];
+
+    mVec3_c v0(pI[0], pI[4], pI[8]);
+    mVec3_c v1(pJ[0], pJ[4], pJ[8]);
+
+    mVec3_c v2;
+    v0.normalize();
+
+    f32 z, y, x;
+    if (j - i == 1 || j - i == -2) {
+        z = (v0.x * v1.y) - (v0.y * v1.x);
+        y = (v0.z * v1.x) - (v0.x * v1.z);
+        x = (v0.y * v1.z) - (v0.z * v1.y);
+        v2 = mVec3_c(x, y, z);
+
+        z = (v2.x * v0.y) - (v2.y * v0.x);
+        y = (v2.z * v0.x) - (v2.x * v0.z);
+        x = (v2.y * v0.z) - (v2.z * v0.y);
+        v1 = mVec3_c(x, y, z);
+    } else {
+        z = (v1.x * v0.y) - (v1.y * v0.x);
+        y = (v1.z * v0.x) - (v1.x * v0.z);
+        x = (v1.y * v0.z) - (v1.z * v0.y);
+        v2 = mVec3_c(x, y, z);
+
+        z = (v0.x * v2.y) - (v0.y * v2.x);
+        y = (v0.z * v2.x) - (v0.x * v2.z);
+        x = (v0.y * v2.z) - (v0.z * v2.y);
+        v1 = mVec3_c(x, y, z);
+    }
+    v2.normalize();
+    v1.normalize();
+
+    f32 *pK = &arr[((3 - i) - j)];
+
+    // clang-format off
+    pI[0] = v0.x; pI[4] = v0.y; pI[8] = v0.z;
+    pJ[0] = v1.x; pJ[4] = v1.y; pJ[8] = v1.z;
+    pK[0] = v2.x; pK[4] = v2.y; pK[8] = v2.z;
+    // clang-format on
+}