ksys/phys: Start adding ClosestPointQuery and iterator stuff

11 files changed, 234 insertions, 1 deletions

diff --git a/src/KingSystem/Physics/CMakeLists.txt b/src/KingSystem/Physics/CMakeLists.txt
index d853e9cf..d437e5ef 100644
--- a/src/KingSystem/Physics/CMakeLists.txt
+++ b/src/KingSystem/Physics/CMakeLists.txt
@@ -103,6 +103,10 @@ target_sources(uking PRIVATE
 
   System/physCharacterControllerParam.cpp
   System/physCharacterControllerParam.h
+  System/physClosestPointQuery.cpp
+  System/physClosestPointQuery.h
+  System/physClosestPointQueryWithInfo.cpp
+  System/physClosestPointQueryWithInfo.h
   System/physCollisionInfo.cpp
   System/physCollisionInfo.h
   System/physContactInfoParam.cpp
@@ -142,6 +146,7 @@ target_sources(uking PRIVATE
   physDefines.h
   physConversions.h
   physHeapUtil.h
+  physLayerMaskBuilder.h
   physMaterialMask.cpp
   physMaterialMask.h
 )
diff --git a/src/KingSystem/Physics/System/physClosestPointQuery.cpp b/src/KingSystem/Physics/System/physClosestPointQuery.cpp
new file mode 100644
index 00000000..c27220a0
--- /dev/null
+++ b/src/KingSystem/Physics/System/physClosestPointQuery.cpp
@@ -0,0 +1,43 @@
+#include "KingSystem/Physics/System/physClosestPointQuery.h"
+#include "KingSystem/Physics/RigidBody/physRigidBody.h"
+#include "KingSystem/Physics/System/physQueryContactPointInfo.h"
+#include "KingSystem/Physics/physDefines.h"
+#include "KingSystem/Physics/physLayerMaskBuilder.h"
+
+namespace ksys::phys {
+
+ClosestPointQuery::ClosestPointQuery(RigidBody* body, QueryContactPointInfo* contact_point_info)
+    : mBody(body), mContactPointInfo(contact_point_info) {
+    reset();
+}
+
+ClosestPointQuery::~ClosestPointQuery() = default;
+
+void ClosestPointQuery::reset() {
+    mIsSuccess = false;
+    _18 = 0;
+}
+
+void ClosestPointQuery::setLayerMasks(const LayerMaskBuilder& builder) {
+    auto* info = mContactPointInfo;
+    for (int i = 0; i < NumContactLayerTypes; ++i) {
+        info->mSubscribedLayers[i] = builder.getMasks()[i].layers;
+        info->mLayerMask2[i] = builder.getMasks()[i].layers2;
+    }
+}
+
+bool ClosestPointQuery::isSuccess() const {
+    return mIsSuccess;
+}
+
+void ClosestPointQuery::setLayerMasksAndBodyCollisionFilterInfo(const LayerMaskBuilder& builder) {
+    EntityCollisionMask mask;
+    mask.data.query_custom_receiver_layer_mask =
+        builder.getMasks()[int(ContactLayerType::Entity)].layers;
+    mask.data.layer = ContactLayer::EntityQueryCustomReceiver;
+    mBody->setCollisionFilterInfo(mask.raw);
+
+    setLayerMasks(builder);
+}
+
+}  // namespace ksys::phys
diff --git a/src/KingSystem/Physics/System/physClosestPointQuery.h b/src/KingSystem/Physics/System/physClosestPointQuery.h
new file mode 100644
index 00000000..8a5b0b86
--- /dev/null
+++ b/src/KingSystem/Physics/System/physClosestPointQuery.h
@@ -0,0 +1,39 @@
+#pragma once
+
+#include <basis/seadTypes.h>
+#include <math/seadMatrix.h>
+#include <prim/seadRuntimeTypeInfo.h>
+
+namespace ksys::phys {
+
+class LayerMaskBuilder;
+class RigidBody;
+class QueryContactPointInfo;
+
+class ClosestPointQuery {
+    SEAD_RTTI_BASE(ClosestPointQuery)
+public:
+    ClosestPointQuery(RigidBody* body, QueryContactPointInfo* contact_point_info);
+    virtual ~ClosestPointQuery();
+
+    RigidBody* getBody() const { return mBody; }
+    QueryContactPointInfo* getContactPointInfo() const { return mContactPointInfo; }
+
+    void setLayerMasks(const LayerMaskBuilder& builder);
+    void setLayerMasksAndBodyCollisionFilterInfo(const LayerMaskBuilder& builder);
+
+    // 0x0000007100fb0850
+    bool execute(bool unk);
+    bool isSuccess() const;
+
+protected:
+    void reset();
+
+    RigidBody* mBody{};
+    QueryContactPointInfo* mContactPointInfo{};
+    u32 _18{};
+    bool mIsSuccess = false;
+    sead::Matrix34f mMtx = sead::Matrix34f::ident;
+};
+
+}  // namespace ksys::phys
diff --git a/src/KingSystem/Physics/System/physClosestPointQueryWithInfo.cpp b/src/KingSystem/Physics/System/physClosestPointQueryWithInfo.cpp
new file mode 100644
index 00000000..b0125f27
--- /dev/null
+++ b/src/KingSystem/Physics/System/physClosestPointQueryWithInfo.cpp
@@ -0,0 +1,22 @@
+#include "KingSystem/Physics/System/physClosestPointQueryWithInfo.h"
+#include "KingSystem/Physics/System/physQueryContactPointInfo.h"
+#include "KingSystem/Physics/System/physSystem.h"
+
+namespace ksys::phys {
+
+ClosestPointQueryWithInfo::ClosestPointQueryWithInfo(RigidBody* body, int num_points,
+                                                     const sead::SafeString& name, int a,
+                                                     LowPriority low_priority)
+    : ClosestPointQuery(body, nullptr) {
+    mStatus = Status::_1;
+    auto* heap = System::instance()->getPhysicsTempHeap(low_priority);
+    mContactPointInfo = QueryContactPointInfo::make(heap, num_points, name, a, 0);
+}
+
+ClosestPointQueryWithInfo::~ClosestPointQueryWithInfo() {
+    if (mStatus == Status::_1 || mStatus == Status::_3) {
+        QueryContactPointInfo::free(mContactPointInfo);
+    }
+}
+
+}  // namespace ksys::phys
diff --git a/src/KingSystem/Physics/System/physClosestPointQueryWithInfo.h b/src/KingSystem/Physics/System/physClosestPointQueryWithInfo.h
new file mode 100644
index 00000000..d90afdd3
--- /dev/null
+++ b/src/KingSystem/Physics/System/physClosestPointQueryWithInfo.h
@@ -0,0 +1,26 @@
+#pragma once
+
+#include "KingSystem/Physics/System/physClosestPointQuery.h"
+#include "KingSystem/Physics/System/physSystem.h"
+
+namespace ksys::phys {
+
+// A ClosestPointQuery with an owned QueryContactPointInfo.
+class ClosestPointQueryWithInfo : public ClosestPointQuery {
+    SEAD_RTTI_OVERRIDE(ClosestPointQueryWithInfo, ClosestPointQuery)
+public:
+    enum class Status {
+        _1 = 1,
+        _2 = 2,
+        _3 = 3,
+    };
+
+    ClosestPointQueryWithInfo(RigidBody* body, int num_points, const sead::SafeString& name, int a,
+                              LowPriority low_priority);
+    ~ClosestPointQueryWithInfo() override;
+
+protected:
+    Status mStatus{};
+};
+
+}  // namespace ksys::phys
diff --git a/src/KingSystem/Physics/System/physContactPointInfo.h b/src/KingSystem/Physics/System/physContactPointInfo.h
index 82437534..f8861e40 100644
--- a/src/KingSystem/Physics/System/physContactPointInfo.h
+++ b/src/KingSystem/Physics/System/physContactPointInfo.h
@@ -136,7 +136,7 @@ public:
     auto begin() const { return Iterator(mPoints, mNumContactPoints); }
     auto end() const { return Iterator(mPoints, mNumContactPoints, Iterator::IsEnd::Yes); }
 
-private:
+protected:
     friend class ContactMgr;
 
     Points mPoints;
diff --git a/src/KingSystem/Physics/System/physQueryContactPointInfo.cpp b/src/KingSystem/Physics/System/physQueryContactPointInfo.cpp
index 9a972ed7..c5098227 100644
--- a/src/KingSystem/Physics/System/physQueryContactPointInfo.cpp
+++ b/src/KingSystem/Physics/System/physQueryContactPointInfo.cpp
@@ -1,4 +1,5 @@
 #include "KingSystem/Physics/System/physQueryContactPointInfo.h"
+#include "KingSystem/Physics/System/physContactMgr.h"
 
 namespace ksys::phys {
 
@@ -16,4 +17,14 @@ void QueryContactPointInfo::free(QueryContactPointInfo* info) {
 
 QueryContactPointInfo::~QueryContactPointInfo() = default;
 
+void QueryContactPointInfo::Iterator::getPointPosition(sead::Vector3f* out, Point point) const {
+    out->e = getPoint()->position.e;
+}
+
+sead::Vector3f QueryContactPointInfo::Iterator::getPointPosition(Point point) const {
+    sead::Vector3f pos;
+    getPointPosition(&pos, point);
+    return pos;
+}
+
 }  // namespace ksys::phys
diff --git a/src/KingSystem/Physics/System/physQueryContactPointInfo.h b/src/KingSystem/Physics/System/physQueryContactPointInfo.h
index a3778ff1..c28039ac 100644
--- a/src/KingSystem/Physics/System/physQueryContactPointInfo.h
+++ b/src/KingSystem/Physics/System/physQueryContactPointInfo.h
@@ -6,12 +6,25 @@ namespace ksys::phys {
 
 class QueryContactPointInfo : public ContactPointInfo {
 public:
+    class Iterator : public ContactPointInfo::Iterator {
+    public:
+        using ContactPointInfo::Iterator::Iterator;
+
+        void getPointPosition(sead::Vector3f* out, Point point) const override;
+        sead::Vector3f getPointPosition(Point point) const override;
+    };
+
     static QueryContactPointInfo* make(sead::Heap* heap, int num_points,
                                        const sead::SafeString& name, int a, int b);
     static void free(QueryContactPointInfo* info);
 
     using ContactPointInfo::ContactPointInfo;
     ~QueryContactPointInfo() override;
+
+    auto begin() const { return Iterator(mPoints, mNumContactPoints); }
+    auto end() const { return Iterator(mPoints, mNumContactPoints, Iterator::IsEnd::Yes); }
+
+    friend class ClosestPointQuery;
 };
 
 }  // namespace ksys::phys
diff --git a/src/KingSystem/Physics/System/physSystem.h b/src/KingSystem/Physics/System/physSystem.h
index 9ad2a173..0f3766cd 100644
--- a/src/KingSystem/Physics/System/physSystem.h
+++ b/src/KingSystem/Physics/System/physSystem.h
@@ -25,6 +25,11 @@ enum class IsIndoorStage {
     Yes,
 };
 
+enum class LowPriority : bool {
+    Yes = true,
+    No = false,
+};
+
 class System {
     SEAD_SINGLETON_DISPOSER(System)
     System();
@@ -72,6 +77,9 @@ public:
     void lockWorld(ContactLayerType type, void* a = nullptr, int b = 0, bool c = false);
     void unlockWorld(ContactLayerType type, void* a = nullptr, int b = 0, bool c = false);
 
+    // 0x0000007101216cec
+    sead::Heap* getPhysicsTempHeap(LowPriority low_priority) const;
+
 private:
     u8 _28[0x64 - 0x28];
     float _64 = 1.0 / 30.0;
diff --git a/src/KingSystem/Physics/physDefines.h b/src/KingSystem/Physics/physDefines.h
index befc5ae7..565600ed 100644
--- a/src/KingSystem/Physics/physDefines.h
+++ b/src/KingSystem/Physics/physDefines.h
@@ -13,6 +13,8 @@ enum class ContactLayerType {
     Invalid,
 };
 
+constexpr int NumContactLayerTypes = 2;
+
 SEAD_ENUM(ContactLayer,
 EntityObject,\
 EntitySmallObject,\
diff --git a/src/KingSystem/Physics/physLayerMaskBuilder.h b/src/KingSystem/Physics/physLayerMaskBuilder.h
new file mode 100644
index 00000000..87880300
--- /dev/null
+++ b/src/KingSystem/Physics/physLayerMaskBuilder.h
@@ -0,0 +1,64 @@
+#pragma once
+
+#include <container/seadSafeArray.h>
+#include <prim/seadBitFlag.h>
+#include "KingSystem/Physics/physDefines.h"
+
+namespace ksys::phys {
+
+class LayerMaskBuilder {
+public:
+    struct Masks {
+        sead::BitFlag32 layers;
+        // TODO: rename
+        sead::BitFlag32 layers2;
+    };
+
+    LayerMaskBuilder() = default;
+    // XXX: this doesn't need to be virtual...
+    virtual ~LayerMaskBuilder() = default;
+
+    LayerMaskBuilder& addLayer(ContactLayer layer);
+    LayerMaskBuilder& removeLayer(ContactLayer layer);
+    bool hasLayer(ContactLayer layer) const;
+
+    // TODO: rename
+    LayerMaskBuilder& addLayer2(ContactLayer layer);
+    LayerMaskBuilder& removeLayer2(ContactLayer layer);
+    bool hasLayer2(ContactLayer layer) const;
+
+    const auto& getMasks() const { return mMasks; }
+
+private:
+    sead::SafeArray<Masks, NumContactLayerTypes> mMasks;
+};
+
+inline LayerMaskBuilder& LayerMaskBuilder::addLayer(ContactLayer layer) {
+    mMasks[int(getContactLayerType(layer))].layers.set(makeContactLayerMask(layer));
+    return *this;
+}
+
+inline LayerMaskBuilder& LayerMaskBuilder::removeLayer(ContactLayer layer) {
+    mMasks[int(getContactLayerType(layer))].layers.reset(makeContactLayerMask(layer));
+    return *this;
+}
+
+inline bool LayerMaskBuilder::hasLayer(ContactLayer layer) const {
+    return (mMasks[int(getContactLayerType(layer))].layers & makeContactLayerMask(layer)) != 0;
+}
+
+inline LayerMaskBuilder& LayerMaskBuilder::addLayer2(ContactLayer layer) {
+    mMasks[int(getContactLayerType(layer))].layers2.set(makeContactLayerMask(layer));
+    return *this;
+}
+
+inline LayerMaskBuilder& LayerMaskBuilder::removeLayer2(ContactLayer layer) {
+    mMasks[int(getContactLayerType(layer))].layers2.reset(makeContactLayerMask(layer));
+    return *this;
+}
+
+inline bool LayerMaskBuilder::hasLayer2(ContactLayer layer) const {
+    return (mMasks[int(getContactLayerType(layer))].layers2 & makeContactLayerMask(layer)) != 0;
+}
+
+}  // namespace ksys::phys