ksys/phys: Start adding ClosestPointQuery and iterator stuff

1 files changed, 22 insertions, 0 deletions

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