ksys/phys: Start adding ClosestPointQuery and iterator stuff

data/uking_functions.csv

@@ -83700,28 +83700,28 @@ Address,Quality,Size,Name
 0x0000007100fb069c,U,000204,
 0x0000007100fb0768,U,000092,
 0x0000007100fb07c4,U,000008,
-0x0000007100fb07cc,U,000084,
+0x0000007100fb07cc,O,000084,_ZN4ksys4phys17ClosestPointQueryC1EPNS0_9RigidBodyEPNS0_21QueryContactPointInfoE
-0x0000007100fb0820,U,000004,nullsub_4239
+0x0000007100fb0820,O,000004,_ZN4ksys4phys17ClosestPointQueryD1Ev
-0x0000007100fb0824,U,000004,j__ZdlPv_1002
+0x0000007100fb0824,O,000004,_ZN4ksys4phys17ClosestPointQueryD0Ev
-0x0000007100fb0828,U,000040,
+0x0000007100fb0828,O,000040,_ZN4ksys4phys17ClosestPointQuery13setLayerMasksERKNS0_16LayerMaskBuilderE
 0x0000007100fb0850,U,001168,
-0x0000007100fb0ce0,U,000008,
+0x0000007100fb0ce0,O,000008,_ZNK4ksys4phys17ClosestPointQuery9isSuccessEv
-0x0000007100fb0ce8,U,000088,
+0x0000007100fb0ce8,O,000088,_ZN4ksys4phys17ClosestPointQuery39setLayerMasksAndBodyCollisionFilterInfoERKNS0_16LayerMaskBuilderE
-0x0000007100fb0d40,U,000112,
+0x0000007100fb0d40,O,000112,_ZNK4ksys4phys17ClosestPointQuery27checkDerivedRuntimeTypeInfoEPKN4sead15RuntimeTypeInfo9InterfaceE
-0x0000007100fb0db0,U,000092,
+0x0000007100fb0db0,O,000092,_ZNK4ksys4phys17ClosestPointQuery18getRuntimeTypeInfoEv
-0x0000007100fb0e0c,U,000072,
+0x0000007100fb0e0c,O,000072,_ZN4ksys4phys25ClosestPointQueryWithInfoD1Ev
-0x0000007100fb0e54,U,000080,
+0x0000007100fb0e54,O,000080,_ZN4ksys4phys25ClosestPointQueryWithInfoD0Ev
 0x0000007100fb0ea4,U,000540,physCollisionQuery::ctor
 0x0000007100fb10c0,U,000544,
 0x0000007100fb12e0,U,000548,
 0x0000007100fb1504,U,000212,
 0x0000007100fb15d8,U,000124,
 0x0000007100fb1654,U,000132,
-0x0000007100fb16d8,U,000204,
+0x0000007100fb16d8,O,000204,_ZNK4ksys4phys25ClosestPointQueryWithInfo27checkDerivedRuntimeTypeInfoEPKN4sead15RuntimeTypeInfo9InterfaceE
-0x0000007100fb17a4,U,000092,
+0x0000007100fb17a4,O,000092,_ZNK4ksys4phys25ClosestPointQueryWithInfo18getRuntimeTypeInfoEv
 0x0000007100fb1800,U,000288,
 0x0000007100fb1920,U,000092,
-0x0000007100fb197c,U,000140,
+0x0000007100fb197c,O,000140,_ZNK4sead15RuntimeTypeInfo6DeriveIN4ksys4phys17ClosestPointQueryEE9isDerivedEPKNS0_9InterfaceE
 0x0000007100fb1a08,U,000140,
 0x0000007100fb1a94,U,000312,
 0x0000007100fb1bcc,U,000240,
@@ -84008,8 +84008,8 @@ Address,Quality,Size,Name
 0x0000007100fc3378,O,000036,_ZN4ksys4phys21QueryContactPointInfoD0Ev
 0x0000007100fc339c,U,000088,
 0x0000007100fc33f4,U,000068,
-0x0000007100fc3438,U,000032,
+0x0000007100fc3438,O,000032,_ZNK4ksys4phys21QueryContactPointInfo8Iterator16getPointPositionEPN4sead7Vector3IfEENS0_16ContactPointInfo8Iterator5PointE
-0x0000007100fc3458,U,000056,
+0x0000007100fc3458,O,000056,_ZNK4ksys4phys21QueryContactPointInfo8Iterator16getPointPositionENS0_16ContactPointInfo8Iterator5PointE
 0x0000007100fc3490,U,000284,
 0x0000007100fc35ac,U,000108,
 0x0000007100fc3618,U,000020,

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
 )

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

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

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

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

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;

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

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

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;

src/KingSystem/Physics/physDefines.h

@@ -13,6 +13,8 @@ enum class ContactLayerType {
     Invalid,
 };
 
+constexpr int NumContactLayerTypes = 2;
+
 SEAD_ENUM(ContactLayer,
 EntityObject,\
 EntitySmallObject,\

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