ksys/phys: Add ShapeCast

1 files changed, 256 insertions, 0 deletions

diff --git a/src/KingSystem/Physics/System/physShapeCast.cpp b/src/KingSystem/Physics/System/physShapeCast.cpp
new file mode 100644
index 00000000..cefed97b
--- /dev/null
+++ b/src/KingSystem/Physics/System/physShapeCast.cpp
@@ -0,0 +1,256 @@
+#include "KingSystem/Physics/System/physShapeCast.h"
+#include <Havok/Physics2012/Collide/Agent/Collidable/hkpCdPoint.h>
+#include <Havok/Physics2012/Collide/Agent/hkpProcessCollisionInput.h>
+#include <Havok/Physics2012/Collide/Query/CastUtil/hkpLinearCastInput.h>
+#include <Havok/Physics2012/Collide/Query/Collector/PointCollector/hkpAllCdPointCollector.h>
+#include "KingSystem/Physics/System/physContactMgr.h"
+#include "KingSystem/Physics/System/physEntityContactListener.h"
+#include "KingSystem/Physics/System/physGroupFilter.h"
+#include "KingSystem/Physics/System/physQueryContactPointInfo.h"
+#include "KingSystem/Physics/System/physSystem.h"
+#include "KingSystem/Physics/physConversions.h"
+#include "KingSystem/Physics/physLayerMaskBuilder.h"
+
+namespace ksys::phys {
+
+ShapeCast::ShapeCast(RigidBody* body, QueryContactPointInfo* contact_point_info, Mode mode)
+    : mContactPointInfo(contact_point_info), mMode(mode) {
+    if (body)
+        mBody = body;
+
+    mTo = sead::Vector3f::zero;
+    mFrom = sead::Vector3f::zero;
+    mRotation.set(1, 0, 0, 0);
+    reset();
+}
+
+ShapeCast::~ShapeCast() = default;
+
+void ShapeCast::reset() {
+    _40 = {};
+    _41 = {};
+    _42 = {};
+    _44 = {};
+
+    if (mContactPointInfo)
+        mContactPointInfo->getNumContactPoints() = 0;
+}
+
+void ShapeCast::setRotation(sead::Quatf rotation) {
+    mRotation = rotation;
+}
+
+void ShapeCast::setRotation(const sead::Matrix33f& rotation_matrix) {
+    auto quat = sead::Quatf::unit;
+    rotation_matrix.toQuat(quat);
+    setRotation(quat);
+}
+
+void ShapeCast::setRotation(const sead::Matrix34f& transform_matrix) {
+    auto quat = sead::Quatf::unit;
+    transform_matrix.toQuat(quat);
+    setRotation(quat);
+}
+
+void ShapeCast::setStartAndEnd(const sead::Vector3f& start, const sead::Vector3f& end) {
+    mFrom = start;
+    mTo = end;
+}
+
+void ShapeCast::setStartAndDisplacement(const sead::Vector3f& start,
+                                        const sead::Vector3f& displacement) {
+    mFrom = start;
+    mTo = start + displacement;
+}
+
+void ShapeCast::setStartAndDisplacementScaled(const sead::Vector3f& start,
+                                              const sead::Vector3f& displacement,
+                                              float displacement_scale) {
+    mFrom = start;
+    mTo = start + displacement * displacement_scale;
+}
+
+void ShapeCast::setLayerMasks(const LayerMaskBuilder& mask_builder) {
+    mContactPointInfo->setLayerMasks(mask_builder);
+}
+
+void ShapeCast::setColors(const sead::Color4f& color1, const sead::Color4f& color2,
+                          const sead::Color4f& color3, bool enabled) {
+    // Stubbed in release builds.
+}
+
+// NON_MATCHING: the last if block is a complete mess. Should be equivalent, though.
+bool ShapeCast::doExecuteQuery(hkpCdPointCollector& cast_collector,
+                               hkpAllCdPointCollector* start_collector,
+                               ShapeCast::WeldClosestPoints weld_closest_points,
+                               OnlyLockIfNeeded only_lock_if_needed) {
+    const auto layer_type = mBody->getLayerType();
+
+    System::instance()->lockWorld(layer_type, "shape_cast", 0, only_lock_if_needed);
+
+    hkpLinearCastInput input;
+    loadFromVec3(&input.m_to, mTo);
+
+    // Reset internal state.
+    cast_collector.reset();
+    if (_44 == 1)
+        reset();
+
+    System::instance()->getHavokWorld(layer_type)->m_collisionInput->m_weldClosestPoints =
+        bool(weld_closest_points);
+
+    doCast(input, cast_collector, start_collector);
+
+    System::instance()->unlockWorld(layer_type, "shape_cast", 0, only_lock_if_needed);
+
+    // Register every point that is in start_collector.
+    auto* body = mBody;
+    for (int i = 0, n = start_collector->getNumHits(); i < n; ++i) {
+        if (registerContactPoint(start_collector->getHits()[i], body, ClampDistance::Yes)) {
+            _40 = true;
+            if (mMode == Mode::_0)
+                break;
+        }
+    }
+
+    _41 = _40;
+
+    if (mMode == Mode::_2 || !_40) {
+        int num_hits = 0;
+        FilteredClosestCdPointCollector* filtered_collector = nullptr;
+        hkpAllCdPointCollector* all_point_collector = nullptr;
+        bool ok = false;
+
+        if (mMode == Mode::_1 || mMode == Mode::_0) {
+            filtered_collector = static_cast<FilteredClosestCdPointCollector*>(&cast_collector);
+            num_hits = 1;
+            ok = filtered_collector->hasHit();
+        } else {
+            all_point_collector = static_cast<hkpAllCdPointCollector*>(&cast_collector);
+            num_hits = all_point_collector->getNumHits();
+            ok = num_hits >= 0;
+        }
+
+        if (ok) {
+            for (int i = 0; i < num_hits; ++i) {
+                const auto& point = mMode == Mode::_2 ? all_point_collector->getHits()[i] :
+                                                        filtered_collector->getHit();
+                if (registerContactPoint(point, body, ClampDistance::No))
+                    _40 = true;
+            }
+        }
+
+        _42 = _40;
+    }
+
+    _44 = 1;
+    return _40;
+}
+
+bool ShapeCast::registerContactPoint(const hkpRootCdPoint& point, RigidBody* body,
+                                     ClampDistance clamp_distance) {
+    auto* hit_entity = getHkpEntity(*point.m_rootCollidableB);
+    if (!hit_entity)
+        return false;
+
+    if (!mContactPointInfo->testContactPointDistance(point.getContact().getDistance()))
+        return false;
+
+    auto* hit_body = getRigidBody(hit_entity);
+
+    if (System::instance()->getEntityContactListenerField91() && hit_body->isEntity() &&
+        EntityContactListener::isObjectOrGroundOrNPCOrTree(*hit_body)) {
+        return false;
+    }
+
+    RigidBodyCollisionMasks masks_a, masks_b;
+    sead::Vector3f contact_position;
+    storeToVec3(&contact_position, point.getContact().getPosition());
+
+    body->getCollisionMasks(&masks_a, &point.m_shapeKeyA, contact_position);
+    hit_body->getCollisionMasks(&masks_b, &point.m_shapeKeyB, contact_position);
+
+    auto* filter = System::instance()->getGroupFilter(hit_body->getLayerType());
+    auto layer_b = filter->getCollisionFilterInfoLayer(masks_b.collision_filter_info);
+
+    if (!mContactPointInfo->isLayerSubscribed(layer_b))
+        return false;
+
+    ContactPoint contact_point;
+    contact_point.flags.makeAllZero();
+    contact_point.body_a = body;
+    contact_point.body_b = hit_body;
+    contact_point.material_mask_a = MaterialMaskData(masks_a.material_mask);
+    contact_point.material_mask_b = MaterialMaskData(masks_b.material_mask);
+    storeToVec3(&contact_point.position, point.getContact().getPosition());
+    storeToVec3(&contact_point.separating_normal, point.getContact().getSeparatingNormal());
+
+    float distance = point.getContact().getDistance();
+    if (distance < 0 && bool(clamp_distance))
+        distance = 0;
+
+    contact_point.separating_distance = distance;
+    contact_point.shape_key_a = point.m_shapeKeyA;
+    contact_point.shape_key_b = point.m_shapeKeyB;
+    contact_point.separating_normal *= -1;
+
+    return System::instance()->getContactMgr()->registerContactPoint(mContactPointInfo,
+                                                                     contact_point, true);
+}
+
+FilteredClosestCdPointCollector::FilteredClosestCdPointCollector(
+    RigidBody* body, QueryContactPointInfo* contact_point_info)
+    : mBody(body), mContactPointInfo(contact_point_info) {}
+
+// NON_MATCHING: the original code has a useless `if (this == nullptr)` check for some reason
+FilteredClosestCdPointCollector::~FilteredClosestCdPointCollector() = default;
+
+void FilteredClosestCdPointCollector::addCdPoint(const hkpCdPoint& point) {
+    auto* body = mBody;
+    const auto point_distance = point.getContact().getDistanceSimdReal();
+
+    if (hasHit() && !point_distance.isLess(getHitContact().getDistanceSimdReal()))
+        return;
+
+    auto* hit_entity = getHkpEntity(*point.m_cdBodyB.getRootCollidable());
+    if (!hit_entity)
+        return;
+
+    auto* hit_body = getRigidBody(hit_entity);
+
+    if (System::instance()->getEntityContactListenerField91() && hit_body->isEntity() &&
+        EntityContactListener::isObjectOrGroundOrNPCOrTree(*hit_body)) {
+        return;
+    }
+
+    RigidBodyCollisionMasks masks_a, masks_b;
+    sead::Vector3f contact_position;
+    storeToVec3(&contact_position, point.getContact().getPosition());
+    auto shape_key_a = point.m_cdBodyA.getShapeKey();
+    auto shape_key_b = point.m_cdBodyB.getShapeKey();
+
+    body->getCollisionMasks(&masks_a, &shape_key_a, contact_position);
+    hit_body->getCollisionMasks(&masks_b, &shape_key_b, contact_position);
+
+    auto* filter = System::instance()->getGroupFilter(hit_body->getLayerType());
+    auto layer_b = filter->getCollisionFilterInfoLayer(masks_b.collision_filter_info);
+
+    if (!mContactPointInfo->isLayerSubscribed(layer_b))
+        return;
+
+    m_hitPoint.setContact(point.getContact());
+    m_hitPoint.m_rootCollidableA = point.m_cdBodyA.getRootCollidable();
+    m_hitPoint.m_shapeKeyA = point.m_cdBodyA.getShapeKey();
+    m_hitPoint.m_rootCollidableB = point.m_cdBodyB.getRootCollidable();
+    m_hitPoint.m_shapeKeyB = point.m_cdBodyB.getShapeKey();
+    // Set m_earlyOutDistance to the current distance as an optimisation.
+    m_earlyOutDistance = point_distance;
+    m_unweldedNormal = point.getUnweldedNormal();
+}
+
+void FilteredClosestCdPointCollector::reset() {
+    hkpClosestCdPointCollector::reset();
+    m_earlyOutDistance = 1.0f;
+}
+
+}  // namespace ksys::phys