ksys/phys: Add SupportBoneResource

[committer's note: rebased]

1 files changed, 254 insertions, 0 deletions

diff --git a/src/KingSystem/Physics/SupportBone/physSupportBoneResource.cpp b/src/KingSystem/Physics/SupportBone/physSupportBoneResource.cpp
new file mode 100644
index 00000000..3b885129
--- /dev/null
+++ b/src/KingSystem/Physics/SupportBone/physSupportBoneResource.cpp
@@ -0,0 +1,254 @@
+#include "KingSystem/Physics/SupportBone/physSupportBoneResource.h"
+
+#include <cmath>
+
+namespace ksys::phys {
+
+SupportBoneResource::Bone::~Bone() = default;
+
+SupportBoneResource::BaseBone::BaseBone()
+    : bone(-1, "bone", "bone", this), aim(sead::Vector3f::ez, "aim", "aim", this),
+      up(sead::Vector3f::ey, "up", "up", this), space(-1, "space", "space", this),
+      base_rotate(sead::Quatf::unit, "base_rotate", "base_rotate", this),
+      base_translate(sead::Vector3f::zero, "base_translate", "base_translate", this) {}
+
+SupportBoneResource::SupportBone::SupportBone()
+    : bendH(-1, "bendH", "bendH", this), bendV(-1, "bendV", "bendV", this),
+      roll(-1, "roll", "roll", this), translateX(-1, "translateX", "translateX", this),
+      translateY(-1, "translateY", "translateY", this),
+      translateZ(-1, "translateZ", "translateZ", this) {}
+
+SupportBoneResource::SupportBoneResource()
+    : IParameterIO("physsb", 0), mBoneNum(0, "bone_num", "bone_num", &mSupportBoneHeader),
+      mConnectionLinearNum(0, "connection_linear_num", "connection_linear_num",
+                           &mSupportBoneHeader),
+      mConnectionCurveNum(0, "connection_curve_num", "connection_curve_num", &mSupportBoneHeader),
+      mOutputSingleNum(0, "output_single_num", "output_single_num", &mSupportBoneHeader),
+      mOutputDoubleNum(0, "output_double_num", "output_double_num", &mSupportBoneHeader),
+      // labels have two underscores instead of a single one
+      mMainBoneNum(0, "main_bone_num", "main_bone__num" /* sic */, &mSupportBoneHeader),
+      mSupportBoneNum(0, "support_bone_num", "support_bone__num" /* sic */, &mSupportBoneHeader) {
+    addObj(&mSupportBoneHeader, "support_bone_header");
+    addList(&mSupportBoneData, "support_bone_data");
+}
+
+SupportBoneResource::~SupportBoneResource() {
+    freeBuffers();
+}
+
+void SupportBoneResource::freeBuffers() {
+    boneBuffer.freeBuffer();
+    mConnectionLinearBuffer.freeBuffer();
+    mConnectionCurveBuffer.freeBuffer();
+    mOutputSingleBuffer.freeBuffer();
+    mOutputDoubleBuffer.freeBuffer();
+    mMainBoneBuffer.freeBuffer();
+    mSupportBoneBuffer.freeBuffer();
+}
+
+bool uninlineAllocDoubleBuffer(sead::Buffer<ksys::phys::SupportBoneResource::OutputDouble>* buffer,
+                               int size, sead::Heap* heap, s32 alignment = sizeof(void*)) {
+    return buffer->allocBufferAssert(size, heap, alignment);
+}
+
+void SupportBoneResource::doCreate_(u8* data, u32 actualFileSize, sead::Heap* heap) {
+    init(agl::utl::ResParameterArchive(data), heap);
+}
+
+float SupportBoneResource::somethingGetLinearConnection(u32 a2, float a3) {
+    sead::Vector2f slope_intercept = mConnectionLinearBuffer[a2].slope_intercept.ref();
+    float result = slope_intercept.x * a3 + slope_intercept.y;
+    return std::isnan(result) ? 0.0f : result;
+}
+
+int SupportBoneResource::getSupportBoneIndexByName(const sead::SafeString& name) {
+    s32 support_bone_num = mSupportBoneNum.ref();
+    for (s32 i = 0; i < support_bone_num; i++) {
+        int bone_index = mSupportBoneBuffer[i].bone.ref();
+        sead::SafeString bone_name = boneBuffer[bone_index].name.ref().cstr();
+        if (name == bone_name)
+            return i;
+    }
+    return -1;
+}
+
+bool SupportBoneResource::init(agl::utl::ResParameterArchive archive, sead::Heap* heap) {
+    agl::utl::ResParameterList param_list = archive.getRootList();
+    const auto support_bone_list = param_list.getResParameterList(0);
+    mSupportBoneHeader.applyResParameterObj(param_list.getResParameterObj(0));
+
+    int bone_num = mBoneNum.ref();
+    if (bone_num < 1)
+        return false;
+    mSupportBoneData.addList(&mBoneList, "bone_list");
+    boneBuffer.allocBufferAssert(bone_num, heap);
+    for (int i = 0; i < bone_num; i++) {
+        auto bone_name = sead::FormatFixedSafeString<32>("bone_%d", i);
+        mBoneList.addObj(&boneBuffer[i], bone_name);
+        boneBuffer[i].name.init(sead::FixedSafeString<64>::cEmptyString, "name", "name",
+                                &boneBuffer[i]);
+    }
+
+    int connection_linear_num = mConnectionLinearNum.ref();
+    if (connection_linear_num > 0) {
+        mSupportBoneData.addList(&mConnectionLinearList, "connection_linear_list");
+        mConnectionLinearBuffer.allocBufferAssert(connection_linear_num, heap);
+
+        for (int i = 0; i < connection_linear_num; i++) {
+            auto connection_name = sead::FormatFixedSafeString<32>("connection_linear_%d", i);
+            mConnectionLinearList.addObj(&mConnectionLinearBuffer[i], connection_name);
+            mConnectionLinearBuffer[i].bone_attribute.init(
+                0, "bone_attribute", "bone_attribute",
+                &mConnectionLinearBuffer[i]);  // initialised with 0 instead of -1
+            mConnectionLinearBuffer[i].slope_intercept.init(
+                {1, -1}, "slope_intercept", "slope_intercept", &mConnectionLinearBuffer[i]);
+        }
+    }
+
+    int connection_curve_num = mConnectionCurveNum.ref();
+    if (connection_curve_num > 0) {
+        mSupportBoneData.addList(&mConnectionCurveList, "connection_curve_list");
+        mConnectionCurveBuffer.allocBufferAssert(connection_curve_num, heap);
+
+        for (int i = 0; i < connection_curve_num; i++) {
+            auto connection_name = sead::FormatFixedSafeString<32>("connection_curve_%d", i);
+            mConnectionCurveList.addObj(&mConnectionCurveBuffer[i], connection_name);
+            mConnectionCurveBuffer[i].bone_attribute.init(-1, "bone_attribute", "bone_attribute",
+                                                          &mConnectionCurveBuffer[i]);
+            mConnectionCurveBuffer[i].constant_in.init(true, "constant_in", "constant_in",
+                                                       &mConnectionCurveBuffer[i]);
+            mConnectionCurveBuffer[i].constant_out.init(true, "constant_out", "constant_out",
+                                                        &mConnectionCurveBuffer[i]);
+            mConnectionCurveBuffer[i].key0.init({0, 0, 1, 1}, "key_0", "key_0",
+                                                &mConnectionCurveBuffer[i]);
+            mConnectionCurveBuffer[i].key1.init({0.5, 0.5, 1, 1}, "key_1", "key_1",
+                                                &mConnectionCurveBuffer[i]);
+            mConnectionCurveBuffer[i].key2.init({1, 1, 1, 1}, "key_2", "key_2",
+                                                &mConnectionCurveBuffer[i]);
+        }
+    }
+
+    int output_single_num = mOutputSingleNum.ref();
+    if (output_single_num > 0) {
+        mSupportBoneData.addList(&mOutputSingleList, "output_single_list");
+        mOutputSingleBuffer.allocBufferAssert(output_single_num, heap);
+
+        for (int i = 0; i < output_single_num; i++) {
+            auto output_name = sead::FormatFixedSafeString<32>("output_single_%d", i);
+            mOutputSingleList.addObj(&mOutputSingleBuffer[i], output_name);
+            mOutputSingleBuffer[i].connection.init(0, "connection", "connection",
+                                                   &mOutputSingleBuffer[i]);
+            mOutputSingleBuffer[i].weight.init(0, "weight", "weight", &mOutputSingleBuffer[i]);
+        }
+    }
+
+    int output_double_num = mOutputDoubleNum.ref();
+    if (output_double_num > 0) {
+        mSupportBoneData.addList(&mOutputDoubleList, "output_double_list");
+        uninlineAllocDoubleBuffer(&mOutputDoubleBuffer, output_double_num, heap);
+
+        for (int i = 0; i < output_double_num; i++) {
+            auto output_name = sead::FormatFixedSafeString<32>("output_double_%d", i);
+            mOutputDoubleList.addObj(&mOutputDoubleBuffer[i], output_name);
+            mOutputDoubleBuffer[i].connection_0.init(0, "connection_0", "connection_0",
+                                                     &mOutputDoubleBuffer[i]);
+            mOutputDoubleBuffer[i].weight_0.init(0, "weight_0", "weight_0",
+                                                 &mOutputDoubleBuffer[i]);
+            mOutputDoubleBuffer[i].connection_1.init(0, "connection_1", "connection_1",
+                                                     &mOutputDoubleBuffer[i]);
+            mOutputDoubleBuffer[i].weight_1.init(0, "weight_1", "weight_1",
+                                                 &mOutputDoubleBuffer[i]);
+        }
+    }
+
+    int main_bone_num = mMainBoneNum.ref();
+    if (main_bone_num < 1)
+        return false;
+    mSupportBoneData.addList(&mMainBoneList, "main_bone_list");
+    mMainBoneBuffer.allocBufferAssert(main_bone_num, heap);
+    for (int i = 0; i < main_bone_num; i++) {
+        auto name = sead::FormatFixedSafeString<32>("main_bone_%d", i);
+        mMainBoneList.addObj(&mMainBoneBuffer[i], name);
+    }
+
+    int support_bone_num = mSupportBoneNum.ref();
+    if (support_bone_num < 1)
+        return false;
+    mSupportBoneData.addList(&mSupportBoneList, "support_bone_list");
+    mSupportBoneBuffer.allocBufferAssert(support_bone_num, heap);
+    for (int i = 0; i < support_bone_num; i++) {
+        auto name = sead::FormatFixedSafeString<32>("support_bone_%d", i);
+        mSupportBoneList.addObj(&mSupportBoneBuffer[i], name);
+    }
+
+    mSupportBoneData.applyResParameterList(support_bone_list);
+    return true;
+}
+
+inline float cubicHermiteInterpolate(const sead::Vector4f& key1, const sead::Vector4f& key2,
+                                     float lookup) {
+    if (key1.x == key2.x) {
+        return key2.y;
+    }
+    float diff = key2.x - key1.x;
+    float t = (lookup - key1.x) / diff;
+    float h00 = (2 * t * t * t) - (3 * t * t) + 1;
+    float h01 = (-2 * t * t * t) + (3 * t * t);
+    float h10 = (t * t * t) - (2 * t * t) + t;
+    float h10_scaled = h10 * diff;
+    float h11 = (t * t * t) - (t * t);
+    float h11_scaled = h11 * diff;
+    return (h00 * key1.y) + (h01 * key2.y) + (h10_scaled * key1.w) + (h11_scaled * key2.z);
+}
+
+float SupportBoneResource::getInterpolatedConnectionCurve(u32 index, float lookup) {
+    auto& connection_curve = mConnectionCurveBuffer[index];
+    float result;
+    float x0 = connection_curve.key0->x;  // lowest (always minus pi?)
+    float x1 = connection_curve.key1->x;  // middle (0 in most cases)
+    float x2 = connection_curve.key2->x;  // highest
+    if (x0 == lookup) {
+        result = connection_curve.key0->y;
+    } else if (x1 == lookup) {
+        result = connection_curve.key1->y;
+    } else if (x2 == lookup) {
+        result = connection_curve.key2->y;
+    } else if (x0 > lookup) {
+        result = connection_curve.key0->y;
+        if (!connection_curve.constant_in.ref())
+            result -= (x0 - lookup) * connection_curve.key0->z;
+    } else if (x1 > lookup) {
+        result = cubicHermiteInterpolate(connection_curve.key0.ref(), connection_curve.key1.ref(),
+                                         lookup);
+    } else if (x2 > lookup) {
+        result = cubicHermiteInterpolate(connection_curve.key1.ref(), connection_curve.key2.ref(),
+                                         lookup);
+    } else {
+        if (!connection_curve.constant_out.ref())
+            result = connection_curve.key2->y + ((lookup - x2) * connection_curve.key2->z);
+        else
+            result = connection_curve.key2->y;
+    }
+
+    return std::isnan(result) ? 0.0f : result;
+}
+
+// NON_MATCHING: Due to seadQuat::normalize() mismatch, this function does not match.
+void SupportBoneResource::BaseBone::postRead_() {
+    sead::Vector3f aim_local = aim.ref();
+    sead::Vector3f up_local = up.ref();
+    aim_local.normalize();
+
+    side = sead::cross(aim_local, up_local);
+    side.normalize();
+
+    up_local = sead::cross(side, aim_local);
+    up_local.normalize();
+    aim = aim_local;
+    up = up_local;
+
+    base_rotate->normalize();
+    reverse_rotate = base_rotate->inverse();
+}
+
+}  // namespace ksys::phys
\ No newline at end of file