ksys/phys: Add ShapeCast

1 files changed, 94 insertions, 0 deletions

diff --git a/src/KingSystem/Physics/System/physShapeCastWithInfo.cpp b/src/KingSystem/Physics/System/physShapeCastWithInfo.cpp
new file mode 100644
index 00000000..cf0a8ac0
--- /dev/null
+++ b/src/KingSystem/Physics/System/physShapeCastWithInfo.cpp
@@ -0,0 +1,94 @@
+#include "KingSystem/Physics/System/physShapeCastWithInfo.h"
+#include <Havok/Physics2012/Collide/Agent/Collidable/hkpCollidable.h>
+#include <Havok/Physics2012/Collide/Query/Collector/PointCollector/hkpAllCdPointCollector.h>
+#include <Havok/Physics2012/Dynamics/Entity/hkpRigidBody.h>
+#include <Havok/Physics2012/Dynamics/World/hkpWorld.h>
+#include "KingSystem/Physics/RigidBody/Shape/Sphere/physSphereRigidBody.h"
+#include "KingSystem/Physics/RigidBody/Shape/Sphere/physSphereShape.h"
+#include "KingSystem/Physics/RigidBody/physRigidBody.h"
+#include "KingSystem/Physics/System/physQueryContactPointInfo.h"
+#include "KingSystem/Physics/System/physSystem.h"
+#include "KingSystem/Physics/physConversions.h"
+
+namespace ksys::phys {
+
+ShapeCastWithInfo::ShapeCastWithInfo(RigidBody* body, int num_contact_points, Mode mode,
+                                     const sead::SafeString& name, LowPriority low_priority)
+    : ShapeCast(body, nullptr, mode) {
+    mStatus = Status::_1;
+    auto* heap = System::instance()->getPhysicsTempHeap(low_priority);
+    mContactPointInfo = QueryContactPointInfo::make(heap, num_contact_points, name, 0, 0);
+}
+
+ShapeCastWithInfo::ShapeCastWithInfo(RigidBody* body, QueryContactPointInfo* contact_point_info,
+                                     Mode mode)
+    : ShapeCast(body, contact_point_info, mode) {}
+
+ShapeCastWithInfo::~ShapeCastWithInfo() {
+    if (mStatus == Status::_1 || mStatus == Status::_3)
+        QueryContactPointInfo::free(mContactPointInfo);
+}
+
+// NON_MATCHING: mMode test + regalloc
+bool ShapeCastWithInfo::executeQuery(WeldClosestPoints weld_closest_points) {
+    if (mMode == Mode::_1 || mMode == Mode::_0) {
+        FilteredClosestCdPointCollector cast_collector{mBody, mContactPointInfo};
+        hkpAllCdPointCollector start_collector;
+        return doExecuteQuery(cast_collector, &start_collector, weld_closest_points,
+                              OnlyLockIfNeeded::Yes);
+    } else {
+        hkpAllCdPointCollector cast_collector;
+        hkpAllCdPointCollector start_collector;
+        return doExecuteQuery(cast_collector, &start_collector, weld_closest_points,
+                              OnlyLockIfNeeded::Yes);
+    }
+}
+
+void ShapeCastWithInfo::doCast(const hkpLinearCastInput& input, hkpCdPointCollector& cast_collector,
+                               hkpCdPointCollector* start_collector) {
+    hkTransformf transform;
+    const auto translate = mFrom;
+    transform.set(toHkQuat(mRotation), toHkVec4(translate));
+
+    hkpRigidBody* hk_body = mBody->getHkBody();
+    hkpCollidable collidable{hk_body->getCollidable()->getShape(),
+                             hk_body->getMotion()->getMotionState()};
+    collidable.setTransform(&transform);
+    collidable.setCollisionFilterInfo(hk_body->getCollisionFilterInfo());
+
+    auto* world = System::instance()->getHavokWorld(mBody->getLayerType());
+    world->linearCast(&collidable, input, cast_collector, start_collector);
+}
+
+SphereCast::SphereCast(ContactLayer layer, GroundHit ground_hit, SystemGroupHandler* group_handler,
+                       int num_contact_points, ShapeCast::Mode mode, const sead::Vector3f& from,
+                       float sphere_radius, const sead::SafeString& name, LowPriority low_priority)
+    : ShapeCastWithInfo(nullptr, num_contact_points, mode, name, 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.translate = from;
+    sphere_param.contact_layer = layer;
+    sphere_param.groundhit = ground_hit;
+    sphere_param.motion_type = MotionType::Fixed;
+    sphere_param.system_group_handler = group_handler;
+    sphere_param._90 = true;
+
+    mSphere = SphereRigidBody::make(&sphere_param, heap);
+    mBody = mSphere;
+    mStatus = Status::_3;
+}
+
+SphereCast::~SphereCast() {
+    if (mStatus == Status::_2 || mStatus == Status::_3)
+        delete mSphere;
+}
+
+void SphereCast::setRadius(float radius) {
+    mSphere->setRadius(radius);
+    mSphere->x_40();
+}
+
+}  // namespace ksys::phys