ksys/phys: Add SphereBasedClosestPointQuery

2 files changed, 126 insertions, 1 deletions

diff --git a/src/KingSystem/Physics/System/physClosestPointQueryWithInfo.cpp b/src/KingSystem/Physics/System/physClosestPointQueryWithInfo.cpp
index b0125f27..65f34b68 100644
--- a/src/KingSystem/Physics/System/physClosestPointQueryWithInfo.cpp
+++ b/src/KingSystem/Physics/System/physClosestPointQueryWithInfo.cpp
@@ -1,4 +1,6 @@
 #include "KingSystem/Physics/System/physClosestPointQueryWithInfo.h"
+#include "KingSystem/Physics/RigidBody/Shape/Sphere/physSphereRigidBody.h"
+#include "KingSystem/Physics/RigidBody/Shape/Sphere/physSphereShape.h"
 #include "KingSystem/Physics/System/physQueryContactPointInfo.h"
 #include "KingSystem/Physics/System/physSystem.h"
 
@@ -19,4 +21,91 @@ ClosestPointQueryWithInfo::~ClosestPointQueryWithInfo() {
     }
 }
 
+SphereBasedClosestPointQuery::SphereBasedClosestPointQuery(
+    const sead::Vector3f& position, float sphere_radius, ContactLayer layer, GroundHit ground_hit,
+    SystemGroupHandler* group_handler, int num_points, const sead::SafeString& name, int a,
+    LowPriority low_priority)
+    : ClosestPointQueryWithInfo(nullptr, num_points, name, a, low_priority) {
+    auto* heap = System::instance()->getPhysicsTempHeap(low_priority);
+
+    SphereParam sphere_param;
+    sphere_param.name = sead::SafeString::cEmptyString.cstr();
+    sphere_param.radius = sphere_radius;
+    sphere_param.contact_layer = layer;
+    sphere_param.groundhit = ground_hit;
+    sphere_param.motion_type = MotionType::Fixed;
+    sphere_param._90 = true;
+    sphere_param.system_group_handler = group_handler;
+    makeAndSetSphere(&sphere_param, heap, position);
+}
+
+SphereBasedClosestPointQuery::SphereBasedClosestPointQuery(
+    const sead::Vector3f& position, float sphere_radius, ContactLayer layer,
+    const LayerMaskBuilder& layer_mask_builder, int num_points, const sead::SafeString& name, int a,
+    LowPriority low_priority)
+    : ClosestPointQueryWithInfo(nullptr, num_points, name, a, low_priority) {
+    auto* heap = System::instance()->getPhysicsTempHeap(low_priority);
+
+    SphereParam sphere_param;
+    sphere_param.name = sead::SafeString::cEmptyString.cstr();
+    sphere_param.radius = sphere_radius;
+    sphere_param.contact_layer = layer;
+    sphere_param.motion_type = MotionType::Fixed;
+    sphere_param._90 = true;
+    makeAndSetSphere(&sphere_param, heap, position, &layer_mask_builder);
+}
+
+SphereBasedClosestPointQuery::SphereBasedClosestPointQuery(
+    const sead::Vector3f& position, float sphere_radius, const SensorCollisionMask& mask,
+    SystemGroupHandler* group_handler, int num_points, const sead::SafeString& name, int a,
+    LowPriority low_priority)
+    : ClosestPointQueryWithInfo(nullptr, num_points, name, a, low_priority) {
+    auto* heap = System::instance()->getPhysicsTempHeap(low_priority);
+
+    SphereParam sphere_param;
+    sphere_param.name = sead::SafeString::cEmptyString.cstr();
+    sphere_param.radius = sphere_radius;
+    sphere_param.contact_layer = ContactLayer::SensorCustomReceiver;
+    sphere_param.groundhit = GroundHit::HitAll;
+    sphere_param.receiver_mask = mask;
+    sphere_param.motion_type = MotionType::Fixed;
+    sphere_param.system_group_handler = group_handler;
+    sphere_param._90 = true;
+    makeAndSetSphere(&sphere_param, heap, position);
+}
+
+SphereBasedClosestPointQuery::SphereBasedClosestPointQuery(RigidBody* sphere,
+                                                           const sead::Vector3f& position,
+                                                           int num_points,
+                                                           const sead::SafeString& name, int a,
+                                                           LowPriority low_priority)
+    : ClosestPointQueryWithInfo(sphere, num_points, name, a, low_priority) {
+    [[maybe_unused]] auto* heap = System::instance()->getPhysicsTempHeap(low_priority);
+    mSphere = sphere;
+    mMtx.setTranslation(position);
+}
+
+SphereBasedClosestPointQuery::~SphereBasedClosestPointQuery() {
+    if (mStatus == Status::_2 || mStatus == Status::_3) {
+        delete mSphere;
+    }
+}
+
+void SphereBasedClosestPointQuery::makeAndSetSphere(RigidBodyInstanceParam* sphere_param,
+                                                    sead::Heap* heap,
+                                                    const sead::Vector3f& position,
+                                                    const LayerMaskBuilder* layer_mask_builder) {
+    mSphere = SphereRigidBody::make(sphere_param, heap);
+    mBody = mSphere;
+
+    if (layer_mask_builder != nullptr) {
+        setLayerMasksAndBodyCollisionFilterInfo(*layer_mask_builder);
+    }
+
+    if (mBody) {
+        mMtx.setTranslation(position);
+        mStatus = Status::_3;
+    }
+}
+
 }  // namespace ksys::phys
diff --git a/src/KingSystem/Physics/System/physClosestPointQueryWithInfo.h b/src/KingSystem/Physics/System/physClosestPointQueryWithInfo.h
index d90afdd3..ce693dc7 100644
--- a/src/KingSystem/Physics/System/physClosestPointQueryWithInfo.h
+++ b/src/KingSystem/Physics/System/physClosestPointQueryWithInfo.h
@@ -1,11 +1,14 @@
 #pragma once
 
+#include <math/seadVector.h>
 #include "KingSystem/Physics/System/physClosestPointQuery.h"
 #include "KingSystem/Physics/System/physSystem.h"
 
 namespace ksys::phys {
 
-// A ClosestPointQuery with an owned QueryContactPointInfo.
+struct RigidBodyInstanceParam;
+
+/// A ClosestPointQuery with an owned QueryContactPointInfo.
 class ClosestPointQueryWithInfo : public ClosestPointQuery {
     SEAD_RTTI_OVERRIDE(ClosestPointQueryWithInfo, ClosestPointQuery)
 public:
@@ -23,4 +26,37 @@ protected:
     Status mStatus{};
 };
 
+/// A closest point query using a sphere rigid body.
+class SphereBasedClosestPointQuery : public ClosestPointQueryWithInfo {
+    SEAD_RTTI_OVERRIDE(SphereBasedClosestPointQuery, ClosestPointQueryWithInfo)
+public:
+    SphereBasedClosestPointQuery(const sead::Vector3f& position, float sphere_radius,
+                                 ContactLayer layer, GroundHit ground_hit,
+                                 SystemGroupHandler* group_handler, int num_points,
+                                 const sead::SafeString& name, int a, LowPriority low_priority);
+
+    SphereBasedClosestPointQuery(const sead::Vector3f& position, float sphere_radius,
+                                 ContactLayer layer, const LayerMaskBuilder& layer_mask_builder,
+                                 int num_points, const sead::SafeString& name, int a,
+                                 LowPriority low_priority);
+
+    SphereBasedClosestPointQuery(const sead::Vector3f& position, float sphere_radius,
+                                 const SensorCollisionMask& mask, SystemGroupHandler* group_handler,
+                                 int num_points, const sead::SafeString& name, int a,
+                                 LowPriority low_priority);
+
+    // TODO: figure out if this is really only used with spheres
+    SphereBasedClosestPointQuery(RigidBody* sphere, const sead::Vector3f& position, int num_points,
+                                 const sead::SafeString& name, int a, LowPriority low_priority);
+
+    ~SphereBasedClosestPointQuery() override;
+
+protected:
+    void makeAndSetSphere(RigidBodyInstanceParam* sphere_param, sead::Heap* heap,
+                          const sead::Vector3f& position,
+                          const LayerMaskBuilder* layer_mask_builder = nullptr);
+
+    RigidBody* mSphere;
+};
+
 }  // namespace ksys::phys