Skip to content

Commit

Permalink
Handle test body motion with 0 margin
Browse files Browse the repository at this point in the history 
Margin needs to have a high enough value for test body motion to work
properly (separate using the margin, move without then gather rest info
with the margin again).

Fixes issues with test motion returning no collision in some cases with
margin equal to 0.
  • Loading branch information
@pouleyKetchoupp
pouleyKetchoupp committed Dec 9, 2021
1 parent f1e3c87 commit 0c35404
Show file tree
Hide file tree
Showing 2 changed files with 27 additions and 16 deletions.
24 changes: 15 additions & 9 deletions servers/physics_2d/godot_space_2d.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,7 @@
#include "core/os/os.h"
#include "core/templates/pair.h"

#define TEST_MOTION_MARGIN_MIN_VALUE 0.0001
#define TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR 0.05

_FORCE_INLINE_ static bool _can_collide_with(GodotCollisionObject2D *p_object, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) {
Expand Down Expand Up @@ -439,17 +440,19 @@ bool GodotPhysicsDirectSpaceState2D::rest_info(const ShapeParameters &p_paramete
GodotShape2D *shape = GodotPhysicsServer2D::godot_singleton->shape_owner.get_or_null(p_parameters.shape_rid);
ERR_FAIL_COND_V(!shape, 0);

real_t margin = MAX(p_parameters.margin, TEST_MOTION_MARGIN_MIN_VALUE);

Rect2 aabb = p_parameters.transform.xform(shape->get_aabb());
aabb = aabb.merge(Rect2(aabb.position + p_parameters.motion, aabb.size)); //motion
aabb = aabb.grow(p_parameters.margin);
aabb = aabb.grow(margin);

int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, GodotSpace2D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results);

_RestCallbackData2D rcd;

// Allowed depth can't be lower than motion length, in order to handle contacts at low speed.
real_t motion_length = p_parameters.motion.length();
real_t min_contact_depth = p_parameters.margin * TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR;
real_t min_contact_depth = margin * TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR;
rcd.min_allowed_depth = MIN(motion_length, min_contact_depth);

for (int i = 0; i < amount; i++) {
Expand All @@ -469,7 +472,7 @@ bool GodotPhysicsDirectSpaceState2D::rest_info(const ShapeParameters &p_paramete
rcd.object = col_obj;
rcd.shape = shape_idx;
rcd.local_shape = 0;
bool sc = GodotCollisionSolver2D::solve(shape, p_parameters.transform, p_parameters.motion, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), Vector2(), _rest_cbk_result, &rcd, nullptr, p_parameters.margin);
bool sc = GodotCollisionSolver2D::solve(shape, p_parameters.transform, p_parameters.motion, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), Vector2(), _rest_cbk_result, &rcd, nullptr, margin);
if (!sc) {
continue;
}
Expand Down Expand Up @@ -540,6 +543,7 @@ bool GodotSpace2D::test_body_motion(GodotBody2D *p_body, const PhysicsServer2D::
r_result->collider_id = ObjectID();
r_result->collider_shape = 0;
}

Rect2 body_aabb;

bool shapes_found = false;
Expand All @@ -565,15 +569,17 @@ bool GodotSpace2D::test_body_motion(GodotBody2D *p_body, const PhysicsServer2D::
return false;
}

real_t margin = MAX(p_parameters.margin, TEST_MOTION_MARGIN_MIN_VALUE);

// Undo the currently transform the physics server is aware of and apply the provided one
body_aabb = p_parameters.from.xform(p_body->get_inv_transform().xform(body_aabb));
body_aabb = body_aabb.grow(p_parameters.margin);
body_aabb = body_aabb.grow(margin);

static const int max_excluded_shape_pairs = 32;
ExcludedShapeSW excluded_shape_pairs[max_excluded_shape_pairs];
int excluded_shape_pair_count = 0;

real_t min_contact_depth = p_parameters.margin * TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR;
real_t min_contact_depth = margin * TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR;

real_t motion_length = p_parameters.motion.length();
Vector2 motion_normal = p_parameters.motion / motion_length;
Expand Down Expand Up @@ -630,7 +636,7 @@ bool GodotSpace2D::test_body_motion(GodotBody2D *p_body, const PhysicsServer2D::
cbk.valid_dir = col_obj_shape_xform.get_axis(1).normalized();

real_t owc_margin = col_obj->get_shape_one_way_collision_margin(shape_idx);
cbk.valid_depth = MAX(owc_margin, p_parameters.margin); //user specified, but never less than actual margin or it won't work
cbk.valid_depth = MAX(owc_margin, margin); //user specified, but never less than actual margin or it won't work
cbk.invalid_by_dir = 0;

if (col_obj->get_type() == GodotCollisionObject2D::TYPE_BODY) {
Expand All @@ -655,7 +661,7 @@ bool GodotSpace2D::test_body_motion(GodotBody2D *p_body, const PhysicsServer2D::
bool did_collide = false;

GodotShape2D *against_shape = col_obj->get_shape(shape_idx);
if (GodotCollisionSolver2D::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), cbkres, cbkptr, nullptr, p_parameters.margin)) {
if (GodotCollisionSolver2D::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), cbkres, cbkptr, nullptr, margin)) {
did_collide = cbk.passed > current_passed; //more passed, so collision actually existed
}

Expand Down Expand Up @@ -927,7 +933,7 @@ bool GodotSpace2D::test_body_motion(GodotBody2D *p_body, const PhysicsServer2D::
rcd.valid_dir = col_obj_shape_xform.get_axis(1).normalized();

real_t owc_margin = col_obj->get_shape_one_way_collision_margin(shape_idx);
rcd.valid_depth = MAX(owc_margin, p_parameters.margin); //user specified, but never less than actual margin or it won't work
rcd.valid_depth = MAX(owc_margin, margin); //user specified, but never less than actual margin or it won't work

if (col_obj->get_type() == GodotCollisionObject2D::TYPE_BODY) {
const GodotBody2D *b = static_cast<const GodotBody2D *>(col_obj);
Expand All @@ -949,7 +955,7 @@ bool GodotSpace2D::test_body_motion(GodotBody2D *p_body, const PhysicsServer2D::
rcd.object = col_obj;
rcd.shape = shape_idx;
rcd.local_shape = j;
bool sc = GodotCollisionSolver2D::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), _rest_cbk_result, &rcd, nullptr, p_parameters.margin);
bool sc = GodotCollisionSolver2D::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), _rest_cbk_result, &rcd, nullptr, margin);
if (!sc) {
continue;
}
Expand Down
19 changes: 12 additions & 7 deletions servers/physics_3d/godot_space_3d.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,6 +35,7 @@

#include "core/config/project_settings.h"

#define TEST_MOTION_MARGIN_MIN_VALUE 0.0001
#define TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR 0.05

_FORCE_INLINE_ static bool _can_collide_with(GodotCollisionObject3D *p_object, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) {
Expand Down Expand Up @@ -507,16 +508,18 @@ bool GodotPhysicsDirectSpaceState3D::rest_info(const ShapeParameters &p_paramete
GodotShape3D *shape = GodotPhysicsServer3D::godot_singleton->shape_owner.get_or_null(p_parameters.shape_rid);
ERR_FAIL_COND_V(!shape, 0);

real_t margin = MAX(p_parameters.margin, TEST_MOTION_MARGIN_MIN_VALUE);

AABB aabb = p_parameters.transform.xform(shape->get_aabb());
aabb = aabb.grow(p_parameters.margin);
aabb = aabb.grow(margin);

int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, GodotSpace3D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results);

_RestCallbackData rcd;

// Allowed depth can't be lower than motion length, in order to handle contacts at low speed.
real_t motion_length = p_parameters.motion.length();
real_t min_contact_depth = p_parameters.margin * TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR;
real_t min_contact_depth = margin * TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR;
rcd.min_allowed_depth = MIN(motion_length, min_contact_depth);

for (int i = 0; i < amount; i++) {
Expand All @@ -534,7 +537,7 @@ bool GodotPhysicsDirectSpaceState3D::rest_info(const ShapeParameters &p_paramete

rcd.object = col_obj;
rcd.shape = shape_idx;
bool sc = GodotCollisionSolver3D::solve_static(shape, p_parameters.transform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), _rest_cbk_result, &rcd, nullptr, p_parameters.margin);
bool sc = GodotCollisionSolver3D::solve_static(shape, p_parameters.transform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), _rest_cbk_result, &rcd, nullptr, margin);
if (!sc) {
continue;
}
Expand Down Expand Up @@ -677,11 +680,13 @@ bool GodotSpace3D::test_body_motion(GodotBody3D *p_body, const PhysicsServer3D::
return false;
}

real_t margin = MAX(p_parameters.margin, TEST_MOTION_MARGIN_MIN_VALUE);

// Undo the currently transform the physics server is aware of and apply the provided one
body_aabb = p_parameters.from.xform(p_body->get_inv_transform().xform(body_aabb));
body_aabb = body_aabb.grow(p_parameters.margin);
body_aabb = body_aabb.grow(margin);

real_t min_contact_depth = p_parameters.margin * TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR;
real_t min_contact_depth = margin * TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR;

real_t motion_length = p_parameters.motion.length();
Vector3 motion_normal = p_parameters.motion / motion_length;
Expand Down Expand Up @@ -729,7 +734,7 @@ bool GodotSpace3D::test_body_motion(GodotBody3D *p_body, const PhysicsServer3D::

int shape_idx = intersection_query_subindex_results[i];

if (GodotCollisionSolver3D::solve_static(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), cbkres, cbkptr, nullptr, p_parameters.margin)) {
if (GodotCollisionSolver3D::solve_static(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), cbkres, cbkptr, nullptr, margin)) {
collided = cbk.amount > 0;
}
}
Expand Down Expand Up @@ -949,7 +954,7 @@ bool GodotSpace3D::test_body_motion(GodotBody3D *p_body, const PhysicsServer3D::

rcd.object = col_obj;
rcd.shape = shape_idx;
bool sc = GodotCollisionSolver3D::solve_static(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), _rest_cbk_result, &rcd, nullptr, p_parameters.margin);
bool sc = GodotCollisionSolver3D::solve_static(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), _rest_cbk_result, &rcd, nullptr, margin);
if (!sc) {
continue;
}
Expand Down

0 comments on commit 0c35404

Please sign in to comment.