Skip to content

Commit

Permalink
Fix special case where KinematicBody fails one way collisions by addi…
Browse files Browse the repository at this point in the history 
…ng exclusion lists, closes #9729
  • Loading branch information
@reduz
reduz committed Sep 1, 2017
1 parent d39e79e commit 77ec304
Show file tree
Hide file tree
Showing 3 changed files with 57 additions and 7 deletions.
3 changes: 3 additions & 0 deletions servers/physics_2d/physics_2d_server_sw.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -130,9 +130,12 @@ void Physics2DServerSW::_shape_col_cbk(const Vector2 &p_point_A, const Vector2 &

if (cbk->valid_dir != Vector2()) {
if (p_point_A.distance_squared_to(p_point_B) > cbk->valid_depth * cbk->valid_depth) {
cbk->invalid_by_dir++;
return;
}
if (cbk->valid_dir.dot((p_point_A - p_point_B).normalized()) < 0.7071) {
cbk->invalid_by_dir++;
;
/* print_line("A: "+p_point_A);
print_line("B: "+p_point_B);
print_line("discard too angled "+rtos(cbk->valid_dir.dot((p_point_A-p_point_B))));
Expand Down
1 change: 1 addition & 0 deletions servers/physics_2d/physics_2d_server_sw.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -74,6 +74,7 @@ class Physics2DServerSW : public Physics2DServer {
real_t valid_depth;
int max;
int amount;
int invalid_by_dir;
Vector2 *ptr;
};

Expand Down
60 changes: 53 additions & 7 deletions servers/physics_2d/space_2d_sw.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,8 +30,8 @@
#include "space_2d_sw.h"

#include "collision_solver_2d_sw.h"
#include "pair.h"
#include "physics_2d_server_sw.h"

_FORCE_INLINE_ static bool _match_object_type_query(CollisionObject2DSW *p_object, uint32_t p_collision_layer, uint32_t p_type_mask) {

if ((p_object->get_collision_layer() & p_collision_layer) == 0)
Expand Down Expand Up @@ -517,6 +517,10 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
body_aabb = p_from.xform(p_body->get_inv_transform().xform(body_aabb));
body_aabb = body_aabb.grow(p_margin);

static const int max_excluded_shape_pairs = 32;
Pair<Shape2DSW *, Shape2DSW *> excluded_shape_pairs[max_excluded_shape_pairs];
int excluded_shape_pair_count = 0;

Transform2D body_transform = p_from;

{
Expand All @@ -532,6 +536,8 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
cbk.max = max_results;
cbk.amount = 0;
cbk.ptr = sr;
cbk.invalid_by_dir = 0;
excluded_shape_pair_count = 0; //last step is the one valid

Physics2DServerSW::CollCbkData *cbkptr = &cbk;
CollisionSolver2DSW::CallbackResult cbkres = Physics2DServerSW::_shape_col_cbk;
Expand All @@ -555,14 +561,25 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co

cbk.valid_dir = body_shape_xform.get_axis(1).normalized();
cbk.valid_depth = p_margin; //only valid depth is the collision margin
cbk.invalid_by_dir = 0;

} else {
cbk.valid_dir = Vector2();
cbk.valid_depth = 0;
cbk.invalid_by_dir = 0;
}

if (CollisionSolver2DSW::solve(body_shape, body_shape_xform, Vector2(), col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), Vector2(), cbkres, cbkptr, NULL, p_margin)) {
Shape2DSW *against_shape = col_obj->get_shape(shape_idx);
if (CollisionSolver2DSW::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), Vector2(), cbkres, cbkptr, NULL, p_margin)) {
collided = cbk.amount > 0;
}

if (!collided && cbk.invalid_by_dir > 0) {
//this shape must be excluded
if (excluded_shape_pair_count < max_excluded_shape_pairs) {
excluded_shape_pairs[excluded_shape_pair_count++] = Pair<Shape2DSW *, Shape2DSW *>(body_shape, against_shape);
}
}
}
}

Expand Down Expand Up @@ -622,15 +639,31 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co

const CollisionObject2DSW *col_obj = intersection_query_results[i];
int shape_idx = intersection_query_subindex_results[i];
Shape2DSW *against_shape = col_obj->get_shape(shape_idx);

bool excluded = false;

for (int k = 0; k < excluded_shape_pair_count; k++) {

if (excluded_shape_pairs[k].first == body_shape && excluded_shape_pairs[k].second == against_shape) {
excluded = true;
break;
}
}

if (excluded) {

continue;
}

Transform2D col_obj_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx);
//test initial overlap, does it collide if going all the way?
if (!CollisionSolver2DSW::solve(body_shape, body_shape_xform, p_motion, col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), NULL, NULL, NULL, 0)) {
if (!CollisionSolver2DSW::solve(body_shape, body_shape_xform, p_motion, against_shape, col_obj_xform, Vector2(), NULL, NULL, NULL, 0)) {
continue;
}

//test initial overlap
if (CollisionSolver2DSW::solve(body_shape, body_shape_xform, Vector2(), col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), NULL, NULL, NULL, 0)) {
if (CollisionSolver2DSW::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_xform, Vector2(), NULL, NULL, NULL, 0)) {

if (col_obj->is_shape_set_as_one_way_collision(j)) {
continue;
Expand All @@ -650,7 +683,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
real_t ofs = (low + hi) * 0.5;

Vector2 sep = mnormal; //important optimization for this to work fast enough
bool collided = CollisionSolver2DSW::solve(body_shape, body_shape_xform, p_motion * ofs, col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), NULL, NULL, &sep, 0);
bool collided = CollisionSolver2DSW::solve(body_shape, body_shape_xform, p_motion * ofs, against_shape, col_obj_xform, Vector2(), NULL, NULL, &sep, 0);

if (collided) {

Expand All @@ -669,7 +702,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
cbk.amount = 0;
cbk.ptr = cd;
cbk.valid_dir = body_shape_xform.get_axis(1).normalized();
;

cbk.valid_depth = 10e20;

Vector2 sep = mnormal; //important optimization for this to work fast enough
Expand Down Expand Up @@ -738,6 +771,19 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
const CollisionObject2DSW *col_obj = intersection_query_results[i];
int shape_idx = intersection_query_subindex_results[i];

Shape2DSW *against_shape = col_obj->get_shape(shape_idx);

bool excluded = false;
for (int k = 0; k < excluded_shape_pair_count; k++) {

if (excluded_shape_pairs[k].first == body_shape && excluded_shape_pairs[k].second == against_shape) {
excluded = true;
break;
}
}
if (excluded)
continue;

if (col_obj->is_shape_set_as_one_way_collision(shape_idx)) {

rcd.valid_dir = body_shape_xform.get_axis(1).normalized();
Expand All @@ -749,7 +795,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co

rcd.object = col_obj;
rcd.shape = shape_idx;
bool sc = CollisionSolver2DSW::solve(body_shape, body_shape_xform, Vector2(), col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), Vector2(), _rest_cbk_result, &rcd, NULL, p_margin);
bool sc = CollisionSolver2DSW::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), Vector2(), _rest_cbk_result, &rcd, NULL, p_margin);
if (!sc)
continue;
}
Expand Down

0 comments on commit 77ec304

Please sign in to comment.