Rough dynamic collision setup (#24)

* Rough collider shape setup

* d_a_obj_toD3_stone_figure OK

* Cleanup

* Cleanup 2

5 files changed, 586 insertions, 0 deletions

diff --git a/src/toBeSorted/cc/d_cc_d.cpp b/src/toBeSorted/cc/d_cc_d.cpp
new file mode 100644
index 00000000..5a894ad0
--- /dev/null
+++ b/src/toBeSorted/cc/d_cc_d.cpp
@@ -0,0 +1,395 @@
+#include <d/a/d_a_base.h>
+#include <f/f_base_id.h>
+#include <m/m_vec.h>
+#include <toBeSorted/cc/d_cc_d.h>
+
+inline bool cM3d_IsZero(f32 f);
+
+mVec3_c dCcD_ShapeAttr::m_virtual_center = mVec3_c::Zero;
+
+dCcD_DivideInfo::dCcD_DivideInfo() {}
+dCcD_DivideInfo::~dCcD_DivideInfo() {}
+
+dCcD_DivideArea::dCcD_DivideArea() {}
+dCcD_DivideArea::~dCcD_DivideArea() {}
+
+void d_cc_d_float_order() {
+    1.0f / 32.0f;
+    1.0f;
+    0.0f;
+    FLT_EPSILON;
+    1.0f / 3.0f;
+    1.0e9f;
+    -1.0e9f;
+    -1.0f;
+    0.5f;
+}
+
+// Very certain
+void dCcD_DivideArea::SetArea(Aabb const &aab) {
+    Set(aab.mMin, aab.mMax);
+
+    mScaledXDiff = 1.0f / 32.0f * (mMax.x - mMin.x);
+    mXDiffIsZero = cM3d_IsZero(mScaledXDiff);
+    if (!mXDiffIsZero) {
+        mInvScaledXDiff = 1.0f / mScaledXDiff;
+    }
+
+    mScaledYDiff = 1.0f / 32.0f * (mMax.y - mMin.y);
+    mYDiffIsZero = cM3d_IsZero(mScaledYDiff);
+    if (!mYDiffIsZero) {
+        mInvScaledYDiff = 1.0f / mScaledYDiff;
+    }
+
+    mScaledZDiff = 1.0f / 32.0f * (mMax.z - mMin.z);
+    mZDiffIsZero = cM3d_IsZero(mScaledZDiff);
+    if (!mZDiffIsZero) {
+        mInvScaledZDiff = 1.0f / mScaledZDiff;
+    }
+}
+
+inline bool cM3d_IsZero(f32 f) {
+    return fabsf(f) < FLT_EPSILON;
+}
+
+static u32 const l_base[32] = {
+        0x00000001,
+        0x00000003,
+        0x00000007,
+        0x0000000F,
+        0x0000001F,
+        0x0000003F,
+        0x0000007F,
+        0x000000FF,
+        0x000001FF,
+        0x000003FF,
+        0x000007FF,
+        0x00000FFF,
+        0x00001FFF,
+        0x00003FFF,
+        0x00007FFF,
+        0x0000FFFF,
+        0x0001FFFF,
+        0x0003FFFF,
+        0x0007FFFF,
+        0x000FFFFF,
+        0x001FFFFF,
+        0x003FFFFF,
+        0x007FFFFF,
+        0x00FFFFFF,
+        0x01FFFFFF,
+        0x03FFFFFF,
+        0x07FFFFFF,
+        0x0FFFFFFF,
+        0x1FFFFFFF,
+        0x3FFFFFFF,
+        0x7FFFFFFF,
+        0xFFFFFFFF,
+};
+
+// TODO
+
+UnkCCDStruct::UnkCCDStruct(dAcBase_c *arg) : field_0x00(0), field_0x04(0), field_0x08(0), field_0x0C(0) {
+    field_0x38 = 0;
+    field_0x34 = arg;
+    reset();
+}
+
+void UnkCCDStruct::postExecute() {
+    field_0x04 = field_0x00;
+    field_0x00 = 0;
+    field_0x0C = field_0x08;
+    field_0x08 = 0;
+}
+
+void UnkCCDStruct::reset() {
+    field_0x10 = mVec3_c::Zero;
+    field_0x1C = mVec3_c::Zero;
+    posIncrements = mVec3_c::Zero;
+}
+
+fBaseID_e UnkCCDStruct::getId() {
+    if (field_0x34 != nullptr) {
+        return field_0x34->unique_ID;
+    }
+    return (fBaseID_e)0;
+}
+
+static u32 const sth[] = {0x4B, 0x5A, 0x64, 0x00};
+
+u32 UnkCCDStruct::getSomething(int arg) {
+    if (field_0x38 == 0x0D || arg == 0) {
+        return 0;
+    }
+
+    if (field_0x38 == 0 || arg == 0x0D) {
+        return 100;
+    }
+
+    if (field_0x38 == arg) {
+        return 50;
+    }
+
+    if (field_0x38 == 0x0C || arg == 0x01) {
+        return 0;
+    }
+
+    if (field_0x38 == 1 || arg == 0x0C) {
+        return 100;
+    }
+
+    arg = arg - field_0x38;
+    if (arg > 0) {
+        return sth[(arg <= 3 ? arg : 3) - 1];
+    } else {
+        return 100 - sth[(-arg <= 3 ? -arg : 3) - 1];
+    }
+}
+
+dCcD_GAtTgCoCommonBase::dCcD_GAtTgCoCommonBase() : field_0x00(0), field_0x14(0), mEffCounter(0) {}
+
+dCcD_GAtTgCoCommonBase::~dCcD_GAtTgCoCommonBase() {
+    unlink();
+    field_0x14 = 0;
+    unlink();
+    decreaseCount();
+    field_0x00 = 0;
+}
+
+void dCcD_GAtTgCoCommonBase::unlink() {
+    mActor.unlink();
+}
+
+void dCcD_GAtTgCoCommonBase::setActor(dAcBase_c *actor) {
+    field_0x14 |= 1;
+    mActor.link(actor);
+}
+
+dAcBase_c *dCcD_GAtTgCoCommonBase::getActor() {
+    return mActor.get();
+}
+
+void dCcD_GAtTgCoCommonBase::decreaseCount() {
+    if (mEffCounter <= 0) {
+        return;
+    }
+    mEffCounter--;
+}
+
+dCcD_GObjInf::dCcD_GObjInf() {
+    field_0x104 = nullptr;
+    field_0x108 = 0;
+}
+
+dCcD_GObjInf::~dCcD_GObjInf() {
+    field_0x104 = nullptr;
+}
+
+bool dCcD_GObjInf::weirdConditionCheck(dAcBase_c *ac, u32 arg) {
+    return ac != nullptr && ac->isActorPlayer() && arg == 2;
+}
+
+void dCcD_GObjInf::clear() {
+    mGObjAt.clearFlag();
+    mGObjTg.clearFlag();
+    mGObjCo.clearFlag();
+}
+
+void dCcD_GObjInf::init(const dCcD_SrcGObjInf &src) {
+    mGObjAt.init(&src.mGObjAt);
+    mGObjTg.init(&src.mGObjTg);
+    mGObjCo.init(&src.mGObjCo);
+}
+
+void dCcD_GObjInf::ResetAtHit() {
+    mGObjAt.resetField0x14();
+    mGObjAt.unlink();
+    mGObjAt.decreaseCount();
+    mGObjAt.resetEffCounter();
+}
+
+void dCcD_GObjInf::ResetTgHit() {
+    mGObjTg.resetField0x14();
+    mGObjTg.unlink();
+    mGObjTg.decreaseCount();
+    mGObjTg.resetEffCounter();
+}
+
+void dCcD_GObjInf::ResetCoHit() {
+    mGObjCo.resetField0x14();
+    mGObjCo.unlink();
+    mGObjCo.decreaseCount();
+    mGObjCo.resetEffCounter();
+}
+
+mVec3_c *dCcD_GObjInf::GetAtHitPosP() {
+    return mGObjAt.GetHitPosP();
+}
+
+mVec3_c *dCcD_GObjInf::GetAtHitPosP2() {
+    return mGObjAt.GetHitPosP();
+}
+
+void dCcD_GObjInf::dCcD_GObjInf_0x10() {}
+
+dAcBase_c *dCcD_GObjInf::GetAtActor() {
+    return mGObjAt.getActor();
+}
+
+dAcBase_c *dCcD_GObjInf::GetTgActor() {
+    return mGObjTg.getActor();
+}
+
+dAcBase_c *dCcD_GObjInf::GetCoActor() {
+    return mGObjCo.getActor();
+}
+
+void dCcD_GObjInf::adjustHitPos(f32 dx, f32 dz) {
+    mGObjAt.adjustHitPos(dx, dz);
+    mGObjTg.adjustHitPos(dx, dz);
+    mGObjCo.adjustHitPos(dx, dz);
+}
+
+/**
+ *
+ * Geometric shape colliders
+ *
+ */
+
+dCcD_ShapeAttr::dCcD_ShapeAttr() {}
+
+dCcD_ShapeAttr::~dCcD_ShapeAttr() {}
+
+mVec3_c *dCcD_ShapeAttr::getVirtualCenter() {
+    return &m_virtual_center;
+}
+
+dCcD_ShapeAttr2::dCcD_ShapeAttr2() {}
+dCcD_ShapeAttr2::~dCcD_ShapeAttr2() {}
+
+dCcD_ShapeAttr5::dCcD_ShapeAttr5() {}
+dCcD_ShapeAttr5::~dCcD_ShapeAttr5() {}
+
+void dCcD_ShapeAttr5::init(const dCcD_SrcAabbAttr &src) {
+    setMinMax(mVec3_c(src.minX, src.minY, src.minZ), mVec3_c(src.maxX, src.maxY, src.maxZ));
+}
+
+mVec3_c *dCcD_ShapeAttr5::getVirtualCenter() {
+    return &mVirtualCenter;
+}
+
+dCcD_ShapeAttr1::dCcD_ShapeAttr1() {}
+dCcD_ShapeAttr1::~dCcD_ShapeAttr1() {}
+
+mVec3_c *dCcD_ShapeAttr1::getVirtualCenter() {
+    return &mVirtualCenter;
+}
+
+dCcD_ShapeAttr3::dCcD_ShapeAttr3() {}
+
+dCcD_ShapeAttr3::~dCcD_ShapeAttr3() {}
+
+void dCcD_ShapeAttr3::init(const dCcD_SrcCylAttr &src) {
+    setR(src.mRadius);
+    setH(src.mHeight);
+    mVec3_c v;
+    v.z = 0.0f;
+    v.y = 0.0f;
+    v.x = 0.0f;
+    setC(v);
+}
+
+mVec3_c *dCcD_ShapeAttr3::getVirtualCenter() {
+    return &mCenter;
+}
+
+dCcD_ShapeAttr4::dCcD_ShapeAttr4() {}
+dCcD_ShapeAttr4::~dCcD_ShapeAttr4() {}
+
+void dCcD_ShapeAttr4::init(const dCcD_SrcSphAttr &src) {
+    setR(src.mRadius);
+    mVec3_c v;
+    v.z = 0.0f;
+    v.y = 0.0f;
+    v.x = 0.0f;
+    setC(v);
+}
+
+mVec3_c *dCcD_ShapeAttr4::getVirtualCenter() {
+    return &mCenter;
+}
+
+/**
+ *
+ * At/Tg/Co collider
+ *
+ */
+
+dCcD_GObjAt::dCcD_GObjAt() {
+    field_0x58 = 0;
+}
+
+dCcD_GObjAt::~dCcD_GObjAt() {}
+
+void dCcD_GObjAt::init(const dCcD_SrcGObjAt *src) {
+    mEffCounter = 0;
+    mSrc = *src;
+    mHitPos = mVec3_c::Zero;
+}
+
+void dCcD_GObjAt::setSomeAtFlags(u32 flags) {
+    mSrc.field_0x04 = mSrc.field_0x04 & 0xFFFFFFC1 | flags;
+}
+
+void dCcD_GObjAt::adjustHitPos(f32 dx, f32 dz) {
+    mHitPos.x += dx;
+    mHitPos.z += dz;
+}
+
+dCcD_GObjTg::dCcD_GObjTg() {
+    field_0x44 = 0;
+    field_0x4A = 0;
+    field_0x4B = 0;
+    field_0x4C = 0;
+    field_0x50 = 0;
+    field_0x54 = 0;
+    field_0x6C.x = 0.0f;
+    field_0x6C.y = 0.0f;
+    field_0x6C.z = 0.0f;
+    field_0x78 = 0;
+}
+
+dCcD_GObjTg::~dCcD_GObjTg() {}
+
+void dCcD_GObjTg::init(const dCcD_SrcGObjTg *src) {
+    mEffCounter = 0;
+    mSrc = *src;
+    field_0x4C = 0;
+    mHitPos = mVec3_c::Zero;
+    field_0x44 = 0;
+    field_0x48 = 0x4000;
+}
+
+void dCcD_GObjTg::adjustHitPos(f32 dx, f32 dz) {
+    mHitPos.x += dx;
+    mHitPos.z += dz;
+    field_0x6C.x += dx;
+    field_0x6C.z += dz;
+}
+
+dCcD_GObjCo::dCcD_GObjCo() {
+    field_0x20 = 0;
+    field_0x28 = 0;
+}
+
+dCcD_GObjCo::~dCcD_GObjCo() {}
+
+void dCcD_GObjCo::init(const dCcD_SrcGObjCo *src) {
+    mEffCounter = 0;
+    mSrc = *src;
+    setSomeAtFlags(src->mBase.mGFlag & 0x1E0);
+}
+
+void dCcD_GObjCo::setSomeAtFlags(u32 flags) {}
+
+void dCcD_GObjCo::adjustHitPos(f32 dx, f32 dz) {}
diff --git a/src/toBeSorted/cc/d_cc_m3d.cpp b/src/toBeSorted/cc/d_cc_m3d.cpp
new file mode 100644
index 00000000..1f2ab896
--- /dev/null
+++ b/src/toBeSorted/cc/d_cc_m3d.cpp
@@ -0,0 +1,33 @@
+#include <math.h>
+#include <nw4r/math/math_types.h>
+#include <rvl/MTX.h>
+
+
+using namespace nw4r::math;
+
+inline VEC3 cM3d_Cross(const VEC3 *pVecA, const VEC3 *pVecB, const VEC3 *pVecC, VEC3 *pVecOut) {
+    VEC3 tmp1;
+    VEC3 tmp2;
+    VEC3Sub(&tmp1, pVecB, pVecA);
+    VEC3Sub(&tmp2, pVecC, pVecA);
+
+    VEC3Cross(pVecOut, &tmp1, &tmp2);
+}
+
+inline f32 cM3d_Dot(const VEC3 *pVecA, const VEC3 *pVecB) {
+    return VEC3Dot(pVecA, pVecB);
+}
+
+void cM3d_CalcPla(const VEC3 *pVecA, const VEC3 *pVecB, const VEC3 *pVecC, VEC3 *pVecOut, f32 *pD) {
+    cM3d_Cross(pVecA, pVecB, pVecC, pVecOut);
+    f32 mag = VEC3Len(pVecOut);
+    if (fabsf(mag) > 0.05f) {
+        VEC3Scale(pVecOut, pVecOut, 1.0f / mag);
+        *pD = -cM3d_Dot(pVecOut, pVecA);
+    } else {
+        pVecOut->y = 0.0f;
+        *pD = 0.0f;
+        pVecOut->z = 0.0f;
+        pVecOut->x = 0.0f;
+    }
+}
diff --git a/src/toBeSorted/cc/d_cc_m3d_g_cyl.cpp b/src/toBeSorted/cc/d_cc_m3d_g_cyl.cpp
new file mode 100644
index 00000000..f166c335
--- /dev/null
+++ b/src/toBeSorted/cc/d_cc_m3d_g_cyl.cpp
@@ -0,0 +1,26 @@
+#include <m/m_vec.h>
+#include <toBeSorted/cc/d_cc_m3d_g_cyl.h>
+
+void cM3dGCyl::setC(const mVec3_c &c, f32 radius, f32 height) {
+    setC(c);
+    setR(radius);
+    setH(height);
+}
+
+void cM3dGCyl::setC(const mVec3_c &c) {
+    mCenter = c;
+}
+
+void cM3dGCyl::setH(f32 h) {
+    if (h < 0.0f) {
+        h = 0.0f;
+    }
+    mHeight = h;
+}
+
+void cM3dGCyl::setR(f32 r) {
+    if (r < 0.0f) {
+        r = 0.0f;
+    }
+    mRadius = r;
+}
diff --git a/src/toBeSorted/cc/d_cc_m3d_g_sph.cpp b/src/toBeSorted/cc/d_cc_m3d_g_sph.cpp
new file mode 100644
index 00000000..b4f12a06
--- /dev/null
+++ b/src/toBeSorted/cc/d_cc_m3d_g_sph.cpp
@@ -0,0 +1,26 @@
+#include <m/m_vec.h>
+#include <toBeSorted/cc/d_cc_m3d_g_sph.h>
+
+cM3dGSph::cM3dGSph() {
+    field_0x10 = 1.0f;
+}
+
+void cM3dGSph::setC(const mVec3_c &c) {
+    mCenter = c;
+}
+
+void cM3dGSph::set(const mVec3_c &c, f32 r) {
+    setC(c);
+    setR(r);
+}
+
+void cM3dGSph::setR(f32 r) {
+    if (r < 0.0f) {
+        r = 0.0f;
+    }
+    mRadius = r;
+}
+
+void cM3dGSph::setC(f32 x, f32 y, f32 z) {
+    setC(mVec3_c(x, y, z));
+}
diff --git a/src/toBeSorted/cc/d_cc_shape_colliders.cpp b/src/toBeSorted/cc/d_cc_shape_colliders.cpp
new file mode 100644
index 00000000..3d9ddfe5
--- /dev/null
+++ b/src/toBeSorted/cc/d_cc_shape_colliders.cpp
@@ -0,0 +1,106 @@
+#include <m/m_vec.h>
+#include <toBeSorted/cc/d_cc_d.h>
+#include <toBeSorted/cc/d_cc_shape_colliders.h>
+
+dCcD_Unk1::dCcD_Unk1() {}
+dCcD_Unk1::~dCcD_Unk1() {}
+
+void dCcD_Unk1::init(const dCcD_SrcUnk &src) {
+    dCcD_GObjInf::init(src.mObjInf);
+    dCcD_ShapeAttr1::init(src.mUnk1Inf);
+}
+
+void *dCcD_Unk1::dCcD_GObjInf_0x0C() {
+    if (this != nullptr) {
+        return &UNK_0x00;
+    }
+    return this;
+}
+
+void dCcD_Unk1::unknownCalc() {
+    PSVECSubtract(*&field_0x0C, *&field_0x00, *getAtVec());
+}
+
+dCcD_Empty::dCcD_Empty() {}
+dCcD_Empty::~dCcD_Empty() {}
+
+void dCcD_Empty::init(const dCcD_SrcEmpty &src) {
+    dCcD_GObjInf::init(src.mObjInf);
+}
+
+void *dCcD_Empty::dCcD_GObjInf_0x0C() {
+    if (this != nullptr) {
+        return &UNK_0x00;
+    }
+    return this;
+}
+
+dCcD_Cyl::dCcD_Cyl() {}
+dCcD_Cyl::~dCcD_Cyl() {}
+
+void dCcD_Cyl::init(const dCcD_SrcCyl &src) {
+    dCcD_GObjInf::init(src.mObjInf);
+    dCcD_ShapeAttr3::init(src.mCylAttr);
+}
+
+void *dCcD_Cyl::dCcD_GObjInf_0x0C() {
+    if (this != nullptr) {
+        return &UNK_0x00;
+    }
+    return this;
+}
+
+void dCcD_Cyl::setCenter(const mVec3_c &c) {
+    setAtVec(mVec3_c::Zero);
+    setC(c);
+}
+
+void dCcD_Cyl::moveCenter(const mVec3_c &c) {
+    setAtVec(c - GetC());
+    setC(c);
+}
+
+dCcD_Sph::dCcD_Sph() {}
+dCcD_Sph::~dCcD_Sph() {}
+
+void dCcD_Sph::init(const dCcD_SrcSph &src) {
+    dCcD_GObjInf::init(src.mObjInf);
+    dCcD_ShapeAttr4::init(src.mSphAttr);
+}
+
+void dCcD_Sph::setCenter(const mVec3_c &c) {
+    setAtVec(mVec3_c::Zero);
+    setC(c);
+}
+
+void dCcD_Sph::moveCenter(const mVec3_c &c) {
+    setAtVec(c - GetC());
+    setC(c);
+}
+
+void dCcD_Sph::setCenterAndAtVec(const mVec3_c &a, const mVec3_c &b) {
+    setAtVec(b);
+    setC(a);
+}
+
+void *dCcD_Sph::dCcD_GObjInf_0x0C() {
+    if (this != nullptr) {
+        return &UNK_0x00;
+    }
+    return this;
+}
+
+dCcD_Aabb::dCcD_Aabb() {}
+dCcD_Aabb::~dCcD_Aabb() {}
+
+void dCcD_Aabb::init(const dCcD_SrcAabb &src) {
+    dCcD_GObjInf::init(src.mObjInf);
+    dCcD_ShapeAttr5::init(src.mAabbAttr);
+}
+
+void *dCcD_Aabb::dCcD_GObjInf_0x0C() {
+    if (this != nullptr) {
+        return &UNK_0x00;
+    }
+    return this;
+}