ksys: Start adding more VFR utils

3 files changed, 222 insertions, 0 deletions

diff --git a/src/KingSystem/System/CMakeLists.txt b/src/KingSystem/System/CMakeLists.txt
index 851e291e..57fb9601 100644
--- a/src/KingSystem/System/CMakeLists.txt
+++ b/src/KingSystem/System/CMakeLists.txt
@@ -19,4 +19,6 @@ target_sources(uking PRIVATE
   Timer.h
   VFR.cpp
   VFR.h
+  VFRValue.cpp
+  VFRValue.h
 )
diff --git a/src/KingSystem/System/VFRValue.cpp b/src/KingSystem/System/VFRValue.cpp
new file mode 100644
index 00000000..a49d4161
--- /dev/null
+++ b/src/KingSystem/System/VFRValue.cpp
@@ -0,0 +1,172 @@
+#include "KingSystem/System/VFRValue.h"
+#include <cmath>
+#include <math/seadMathCalcCommon.h>
+#include <prim/seadMemUtil.h>
+#include "KingSystem/System/VFR.h"
+#include "KingSystem/Utils/InitTimeInfo.h"
+
+namespace ksys {
+
+namespace {
+
+util::InitTimeInfoEx sInitInfo;
+
+f32 getLerpFactor(f32 t) {
+    return 1.0f - std::pow(1.0f - t, VFR::instance()->getDeltaTime());
+}
+
+template <typename T>
+void updateStatsImpl(const T& value, T* prev_value, T* mean) {
+    const T new_mean = ((*prev_value + value) / 2) * VFR::instance()->getDeltaTime();
+    *prev_value = value;
+    *mean = new_mean;
+}
+
+template <typename T>
+void addImpl(T* value, const T& v) {
+    *value += v * VFR::instance()->getDeltaTime();
+}
+
+template <typename T>
+void multiplyImpl(T* value, f32 scalar) {
+    *value *= std::pow(scalar, VFR::instance()->getDeltaTime());
+}
+
+template <typename T>
+void lerpImpl(T* value, const T& b, f32 t) {
+    *value += getLerpFactor(t) * (b - *value);
+}
+
+template <typename T>
+void lerpImpl(T* value, const T& b, f32 t, f32 max_delta) {
+    const auto f = getLerpFactor(t);
+    const auto max_d = VFR::instance()->getDeltaTime() * max_delta;
+    const auto diff = b - *value;
+    const auto d = f * sead::absf(diff);
+    if (d > max_d)
+        *value += diff < 0.0 ? -max_d : max_d;
+    else
+        *value += f * diff;
+}
+
+template <typename T>
+bool lerpImpl(T* value, const T& b, f32 t, f32 max_delta, f32 min_delta) {
+    const auto f = getLerpFactor(t);
+    const auto max_d = VFR::instance()->getDeltaTime() * max_delta;
+    const auto min_d = VFR::instance()->getDeltaTime() * min_delta;
+
+    const auto diff = b - *value;
+    const auto d = f * sead::absf(diff);
+
+    if (sead::absf(diff) <= min_d) {
+        *value = b;
+        return true;
+    }
+
+    if (d > max_d) {
+        *value += diff < 0.0 ? -max_d : max_d;
+    } else if (d < min_d) {
+        *value += diff < 0.0 ? -min_d : min_d;
+    } else {
+        *value += f * diff;
+    }
+    return false;
+}
+
+}  // namespace
+
+VFRValue::VFRValue() = default;
+
+VFRValue::VFRValue(const f32& value) : value{value}, prev_value{value}, mean{value} {}
+
+void VFRValue::updateStats() {
+    updateStatsImpl(value, &prev_value, &mean);
+}
+
+void VFRValue::operator+=(const f32& rhs) {
+    addImpl(&value, rhs);
+}
+
+void VFRValue::operator*=(f32 scalar) {
+    multiplyImpl(&value, scalar);
+}
+
+void VFRValue::lerp(const f32& b, f32 t) {
+    lerpImpl(&value, b, t);
+}
+
+void VFRValue::lerp(const f32& b, f32 t, f32 max_delta) {
+    lerpImpl(&value, b, t, max_delta);
+}
+
+bool VFRValue::lerp(const f32& b, f32 t, f32 max_delta, f32 min_delta) {
+    return lerpValue(&value, b, t, max_delta, min_delta);
+}
+
+bool VFRValue::lerpValue(f32* value, const f32& b, f32 t, f32 max_delta, f32 min_delta) {
+    return lerpImpl(value, b, t, max_delta, min_delta);
+}
+
+bool VFRValue::chase(const f32& target, f32 step) {
+    const auto delta = step * VFR::instance()->getDeltaTime();
+    return sead::Mathf::chase(&value, target, delta);
+}
+
+VFRVec3f::VFRVec3f() : value{0, 0, 0}, prev_value{0, 0, 0}, mean{0, 0, 0} {}
+
+VFRVec3f::VFRVec3f(const sead::Vector3f& value) : value{value}, prev_value{value}, mean{value} {}
+
+// NON_MATCHING: float regalloc
+void VFRVec3f::updateStats() {
+    updateStatsImpl(value, &prev_value, &mean);
+}
+
+void VFRVec3f::operator*=(f32 scalar) {
+    multiplyImpl(&value, scalar);
+}
+
+void VFRVec3f::lerp(const sead::Vector3f& b, f32 t) {
+    lerpImpl(&value, b, t);
+}
+
+bool VFRVec3f::chase(const sead::Vector3f& target, f32 t) {
+    const auto delta = VFR::instance()->getDeltaTime() * t;
+    const auto diff = target - value;
+    const auto norm = sead::norm2(diff);
+
+    if (norm <= delta) {
+        sead::MemUtil::copy(&value, &target, sizeof(value));
+        return true;
+    }
+
+    value += diff * (1.0f / norm) * delta;
+    return false;
+}
+
+void VFRValue::setToMax(const f32& max) {
+    const auto a = value;
+    const auto b = max;
+    value = a < b ? b : a;
+}
+
+void VFRValue::setToMin(const f32& min) {
+    const auto a = value;
+    const auto b = min;
+    value = a > b ? b : a;
+}
+
+void VFRValue::clamp(const f32& min, const f32& max) {
+    const auto a = min;
+    const auto b = max;
+    value = sead::clamp(value, a, b);
+}
+
+void VFRVec3f::normalize(f32 new_norm) {
+    if (sead::norm2(value) > new_norm) {
+        const auto norm = sead::norm2(value);
+        if (norm > 0.0)
+            value *= new_norm / norm;
+    }
+}
+
+}  // namespace ksys
diff --git a/src/KingSystem/System/VFRValue.h b/src/KingSystem/System/VFRValue.h
new file mode 100644
index 00000000..34acbdcc
--- /dev/null
+++ b/src/KingSystem/System/VFRValue.h
@@ -0,0 +1,48 @@
+#pragma once
+
+#include <basis/seadTypes.h>
+#include <math/seadVector.h>
+
+namespace ksys {
+
+/// A time-scaled floating-point value.
+struct VFRValue {
+    VFRValue();
+    explicit VFRValue(const f32& value);
+
+    void updateStats();
+    void operator+=(const f32& rhs);
+    void operator*=(f32 scalar);
+    void lerp(const f32& b, f32 t);
+    void lerp(const f32& b, f32 t, f32 max_delta);
+    bool lerp(const f32& b, f32 t, f32 max_delta, f32 min_delta);
+    static bool lerpValue(f32* value, const f32& b, f32 t, f32 max_delta, f32 min_delta);
+    bool chase(const f32& target, f32 step);
+    void setToMax(const f32& max);
+    void setToMin(const f32& min);
+    void clamp(const f32& min, const f32& max);
+
+    f32 value{};
+    f32 prev_value{};
+    f32 mean{};
+};
+
+/// A time-scaled Vector3f.
+struct VFRVec3f {
+    VFRVec3f();
+    explicit VFRVec3f(const sead::Vector3f& value);
+
+    void updateStats();
+    void operator*=(f32 scalar);
+    void lerp(const sead::Vector3f& b, f32 t);
+    bool chase(const sead::Vector3f& target, f32 t);
+    /// Normalize the `value` vector if its norm is greater than `new_norm`.
+    /// Normalization will make the norm equal to `new_norm`.
+    void normalize(f32 new_norm);
+
+    sead::Vector3f value;
+    sead::Vector3f prev_value;
+    sead::Vector3f mean;
+};
+
+}  // namespace ksys