From 58715df2cbe3555f4de0d3bcc6a8a8ede1239fc8 Mon Sep 17 00:00:00 2001
From: lepelog <25211966+lepelog@users.noreply.github.com>
Date: Tue, 9 Nov 2021 21:09:33 +0100
Subject: [PATCH] cLib_addCalcPos

---
 include/SSystem/SComponent/c_lib.h |  5 +++++
 include/SSystem/SComponent/c_xyz.h |  5 +++++
 libs/SSystem/SComponent/c_lib.cpp  | 27 +++++++++++++++++++++++++++
 3 files changed, 37 insertions(+)

diff --git a/include/SSystem/SComponent/c_lib.h b/include/SSystem/SComponent/c_lib.h
index 8ff0092bc12..cdbfb6356e6 100644
--- a/include/SSystem/SComponent/c_lib.h
+++ b/include/SSystem/SComponent/c_lib.h
@@ -4,6 +4,11 @@
 #include "SSystem/SComponent/c_xyz.h"
 #include "dolphin/mtx/mtx.h"
 #include "dolphin/types.h"
+#include "msl_c/math.h"
+
+inline bool cLib_IsZero(f32 f) {
+    return fabsf(f) < 8e-11;
+}
 
 void cLib_memCpy(void* dst, const void* src, unsigned long size);
 void cLib_memSet(void* ptr, int value, unsigned long size);
diff --git a/include/SSystem/SComponent/c_xyz.h b/include/SSystem/SComponent/c_xyz.h
index 0f28e22048d..5cef049e7f9 100644
--- a/include/SSystem/SComponent/c_xyz.h
+++ b/include/SSystem/SComponent/c_xyz.h
@@ -3,6 +3,7 @@
 
 #include "dolphin/mtx/vec.h"
 #include "global.h"
+#include "msl_c/math.h"
 
 struct cXyz : Vec {
     static const cXyz Zero;
@@ -50,11 +51,13 @@ struct cXyz : Vec {
         y -= f;
         z -= f;
     }
+    void operator-=(const Vec& other) { PSVECSubtract(this, &other, this); }
     void operator+=(const Vec& vec) {
         x += vec.x;
         y += vec.y;
         z += vec.z;
     }
+    void operator*=(f32 scale) { PSVECScale(this, this, scale); }
     /* 80266C6C */ cXyz getCrossProduct(Vec const&) const;
     /* 80266CBC */ cXyz outprod(Vec const&) const;
     /* 80266CE4 */ cXyz norm() const;
@@ -116,6 +119,8 @@ struct cXyz : Vec {
         cXyz tmp2(other.x, 0, other.z);
         return tmp.abs2(tmp2);
     }
+    f32 abs() const { return sqrtf(this->abs2()); }
+    f32 abs(const Vec& other) const { return sqrtf(this->abs2(other)); }
     f32 getMagXZ() const { return cXyz(this->x, 0, this->z).getSquareMag(); }
 };
 
diff --git a/libs/SSystem/SComponent/c_lib.cpp b/libs/SSystem/SComponent/c_lib.cpp
index 4f47d34749d..516c7d71512 100644
--- a/libs/SSystem/SComponent/c_lib.cpp
+++ b/libs/SSystem/SComponent/c_lib.cpp
@@ -158,6 +158,32 @@ SECTION_SDATA2 static f32 lit_2382[1 + 1 /* padding */] = {
 };
 
 /* 8026FAB8-8026FDF4 26A3F8 033C+00 0/0 3/3 78/78 .text cLib_addCalcPos__FP4cXyzRC4cXyzfff */
+#ifdef NON_MATCHING
+// matches besides data
+f32 cLib_addCalcPos(cXyz* pValue, cXyz const& target, f32 scale, f32 param_3, f32 param_4) {
+    if (*pValue != target) {
+        cXyz diff = (*pValue - target);
+        f32 step = diff.abs();
+        if (step < param_4) {
+            *pValue = target;
+        } else {
+            step *= scale;
+            diff *= scale;
+            if (!cLib_IsZero(step)) {
+                if (step > param_3) {
+                    diff *= (param_3 / step);
+                } else if (step < param_4) {
+                    diff *= (param_4 / step);
+                }
+                *pValue -= diff;
+            } else {
+                *pValue = target;
+            }
+        }
+    }
+    return pValue->abs(target);
+}
+#else
 #pragma push
 #pragma optimization_level 0
 #pragma optimizewithasm off
@@ -166,6 +192,7 @@ asm f32 cLib_addCalcPos(cXyz* param_0, cXyz const& param_1, f32 param_2, f32 par
 #include "asm/SSystem/SComponent/c_lib/cLib_addCalcPos__FP4cXyzRC4cXyzfff.s"
 }
 #pragma pop
+#endif
 
 /* 8026FDF4-80270178 26A734 0384+00 0/0 1/1 4/4 .text cLib_addCalcPosXZ__FP4cXyzRC4cXyzfff */
 #pragma push