From cad048c36897c23ad1cc3e8d8e529919310f5ac2 Mon Sep 17 00:00:00 2001 From: Mikael Hermansson Date: Fri, 3 Apr 2026 14:11:37 +0200 Subject: [PATCH] Move the Jolt `body_test_motion` contact filtering to collector Co-authored-by: Jorrit Rouwe --- .../jolt_physics_direct_space_state_3d.cpp | 23 ++------ .../jolt_physics_direct_space_state_3d.h | 2 +- .../spaces/jolt_query_collectors.h | 54 +++++++++++++++++-- 3 files changed, 55 insertions(+), 24 deletions(-) diff --git a/modules/jolt_physics/spaces/jolt_physics_direct_space_state_3d.cpp b/modules/jolt_physics/spaces/jolt_physics_direct_space_state_3d.cpp index d117a7eb86..fadfdb7b97 100644 --- a/modules/jolt_physics/spaces/jolt_physics_direct_space_state_3d.cpp +++ b/modules/jolt_physics/spaces/jolt_physics_direct_space_state_3d.cpp @@ -190,7 +190,7 @@ bool JoltPhysicsDirectSpaceState3D::_body_motion_recover(const JoltBody3D &p_bod for (int i = 0; i < JoltProjectSettings::motion_query_recovery_iterations; ++i) { collector.reset(); - _collide_shape_kinematics(jolt_shape, JPH::Vec3::sOne(), to_jolt_r(transform_com), settings, to_jolt_r(base_offset), collector, motion_filter, motion_filter, motion_filter, motion_filter); + _collide_shape_motion(jolt_shape, JPH::Vec3::sOne(), to_jolt_r(transform_com), settings, to_jolt_r(base_offset), collector, motion_filter, motion_filter, motion_filter, motion_filter); if (!collector.had_hit()) { break; @@ -316,8 +316,8 @@ bool JoltPhysicsDirectSpaceState3D::_body_motion_collide(const JoltBody3D &p_bod const Vector3 &base_offset = transform_com.origin; const JoltMotionFilter3D motion_filter(p_body, p_excluded_bodies, p_excluded_objects); - JoltQueryCollectorClosestMulti collector(p_max_collisions); - _collide_shape_kinematics(jolt_shape, JPH::Vec3::sOne(), to_jolt_r(transform_com), settings, to_jolt_r(base_offset), collector, motion_filter, motion_filter, motion_filter, motion_filter); + JoltQueryCollectorMotion collector(p_motion, p_margin, p_max_collisions); + _collide_shape_motion(jolt_shape, JPH::Vec3::sOne(), to_jolt_r(transform_com), settings, to_jolt_r(base_offset), collector, motion_filter, motion_filter, motion_filter, motion_filter); if (!collector.had_hit() || p_result == nullptr) { return collector.had_hit(); @@ -328,21 +328,8 @@ bool JoltPhysicsDirectSpaceState3D::_body_motion_collide(const JoltBody3D &p_bod for (int i = 0; i < collector.get_hit_count(); ++i) { const JPH::CollideShapeResult &hit = collector.get_hit(i); - const float penetration_depth = hit.mPenetrationDepth + p_margin; - - if (penetration_depth <= 0.0f) { - continue; - } - const Vector3 normal = to_godot(-hit.mPenetrationAxis.Normalized()); - - if (p_motion.length_squared() > 0) { - const Vector3 direction = p_motion.normalized(); - - if (direction.dot(normal) >= -CMP_EPSILON) { - continue; - } - } + const float penetration_depth = hit.mPenetrationDepth + p_margin; JPH::ContactPoints contact_points1; JPH::ContactPoints contact_points2; @@ -439,7 +426,7 @@ void JoltPhysicsDirectSpaceState3D::_collide_shape_queries( } } -void JoltPhysicsDirectSpaceState3D::_collide_shape_kinematics( +void JoltPhysicsDirectSpaceState3D::_collide_shape_motion( const JPH::Shape *p_shape, JPH::Vec3Arg p_scale, JPH::RMat44Arg p_transform_com, diff --git a/modules/jolt_physics/spaces/jolt_physics_direct_space_state_3d.h b/modules/jolt_physics/spaces/jolt_physics_direct_space_state_3d.h index d28af77b04..f8738abaff 100644 --- a/modules/jolt_physics/spaces/jolt_physics_direct_space_state_3d.h +++ b/modules/jolt_physics/spaces/jolt_physics_direct_space_state_3d.h @@ -64,7 +64,7 @@ class JoltPhysicsDirectSpaceState3D final : public PhysicsDirectSpaceState3D { void _generate_manifold(const JPH::CollideShapeResult &p_hit, JPH::ContactPoints &r_contact_points1, JPH::ContactPoints &r_contact_points2 JPH_IF_DEBUG_RENDERER(, JPH::RVec3Arg p_center_of_mass)) const; void _collide_shape_queries(const JPH::Shape *p_shape, JPH::Vec3Arg p_scale, JPH::RMat44Arg p_transform_com, const JPH::CollideShapeSettings &p_settings, JPH::RVec3Arg p_base_offset, JPH::CollideShapeCollector &p_collector, const JPH::BroadPhaseLayerFilter &p_broad_phase_layer_filter = JPH::BroadPhaseLayerFilter(), const JPH::ObjectLayerFilter &p_object_layer_filter = JPH::ObjectLayerFilter(), const JPH::BodyFilter &p_body_filter = JPH::BodyFilter(), const JPH::ShapeFilter &p_shape_filter = JPH::ShapeFilter()) const; - void _collide_shape_kinematics(const JPH::Shape *p_shape, JPH::Vec3Arg p_scale, JPH::RMat44Arg p_transform_com, const JPH::CollideShapeSettings &p_settings, JPH::RVec3Arg p_base_offset, JPH::CollideShapeCollector &p_collector, const JPH::BroadPhaseLayerFilter &p_broad_phase_layer_filter = JPH::BroadPhaseLayerFilter(), const JPH::ObjectLayerFilter &p_object_layer_filter = JPH::ObjectLayerFilter(), const JPH::BodyFilter &p_body_filter = JPH::BodyFilter(), const JPH::ShapeFilter &p_shape_filter = JPH::ShapeFilter()) const; + void _collide_shape_motion(const JPH::Shape *p_shape, JPH::Vec3Arg p_scale, JPH::RMat44Arg p_transform_com, const JPH::CollideShapeSettings &p_settings, JPH::RVec3Arg p_base_offset, JPH::CollideShapeCollector &p_collector, const JPH::BroadPhaseLayerFilter &p_broad_phase_layer_filter = JPH::BroadPhaseLayerFilter(), const JPH::ObjectLayerFilter &p_object_layer_filter = JPH::ObjectLayerFilter(), const JPH::BodyFilter &p_body_filter = JPH::BodyFilter(), const JPH::ShapeFilter &p_shape_filter = JPH::ShapeFilter()) const; public: JoltPhysicsDirectSpaceState3D() = default; diff --git a/modules/jolt_physics/spaces/jolt_query_collectors.h b/modules/jolt_physics/spaces/jolt_query_collectors.h index 9de4cb72b2..3849986e70 100644 --- a/modules/jolt_physics/spaces/jolt_query_collectors.h +++ b/modules/jolt_physics/spaces/jolt_query_collectors.h @@ -30,6 +30,8 @@ #pragma once +#include "core/math/vector3.h" + #include #include @@ -37,7 +39,7 @@ #include template -class JoltQueryCollectorAll final : public TBase { +class JoltQueryCollectorAll : public TBase { public: typedef typename TBase::ResultType Hit; typedef JPH::Array> HitArray; @@ -75,7 +77,7 @@ public: }; template -class JoltQueryCollectorAny final : public TBase { +class JoltQueryCollectorAny : public TBase { public: typedef typename TBase::ResultType Hit; @@ -106,7 +108,7 @@ public: }; template -class JoltQueryCollectorAnyMulti final : public TBase { +class JoltQueryCollectorAnyMulti : public TBase { public: typedef typename TBase::ResultType Hit; typedef JPH::Array> HitArray; @@ -154,7 +156,7 @@ public: }; template -class JoltQueryCollectorClosest final : public TBase { +class JoltQueryCollectorClosest : public TBase { public: typedef typename TBase::ResultType Hit; @@ -189,7 +191,7 @@ public: }; template -class JoltQueryCollectorClosestMulti final : public TBase { +class JoltQueryCollectorClosestMulti : public TBase { public: typedef typename TBase::ResultType Hit; typedef JPH::Array> HitArray; @@ -240,3 +242,45 @@ public: } } }; + +template +class JoltQueryCollectorMotion : public JoltQueryCollectorClosestMulti { +public: + typedef JoltQueryCollectorClosestMulti Base; + typedef typename Base::Hit Hit; + +private: + Vector3 direction; + float distance_sq = 0.0f; + float margin = 0.0f; + +public: + JoltQueryCollectorMotion(const Vector3 &p_motion, float p_margin, int p_max_hits = TDefaultCapacity) : + Base(p_max_hits), + direction(p_motion.normalized()), + distance_sq(p_motion.length_squared()), + margin(p_margin) {} + + virtual void AddHit(const Hit &p_hit) override { + // Ignore hits that are outside of the margin. + const float penetration_depth = p_hit.mPenetrationDepth + margin; + if (penetration_depth <= 0.0f) { + return; + } + + // Ignore hits that don't oppose the motion direction. + // + // This is a deliberate divergence from the Godot Physics reference implementation (which + // does not do this type of filtering) and is known to cause issues. However, not having + // this results in a problematic amount of ghost collisions with `move_and_slide`, for + // reasons that are still unclear as of writing this. + if (distance_sq > 0) { + const Vector3 normal = to_godot(-p_hit.mPenetrationAxis.Normalized()); + if (direction.dot(normal) >= -CMP_EPSILON) { + return; + } + } + + Base::AddHit(p_hit); + } +};