Skip to content

Commit

Permalink
Fix Rayshape not working properly in move_and_slide
Browse files Browse the repository at this point in the history 
These changes improve Rayshape behavior for Godot Physics 2D and 3D
when using move_and_slide with and without snapping.

Kinematic margin is now applied to ray shapes when handling snapping
collision tests and separation raycasts to help getting consistent
results in slopes and flat surfaces.

Recovery is calculated without the margin and a depth of 0 is still
considered a collision to stabilize results when on flat surface.

Recovery is split based on the amount of shapes to fix cases where
multiple rayshapes would cause the body to bounce.

Fixes #34098
Fixes #34663
  • Loading branch information
@pouleyKetchoupp
pouleyKetchoupp committed Jan 8, 2021
1 parent f1832a6 commit a3d8043
Show file tree
Hide file tree
Showing 6 changed files with 40 additions and 34 deletions.
8 changes: 4 additions & 4 deletions servers/physics_2d/collision_solver_2d_sw.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -73,14 +73,14 @@ bool CollisionSolver2DSW::solve_static_line(const Shape2DSW *p_shape_A, const Tr
return found;
}

bool CollisionSolver2DSW::solve_raycast(const Shape2DSW *p_shape_A, const Vector2 &p_motion_A, const Transform2D &p_transform_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, Vector2 *sep_axis) {
bool CollisionSolver2DSW::solve_raycast(const Shape2DSW *p_shape_A, const Vector2 &p_motion_A, const Transform2D &p_transform_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, Vector2 *sep_axis, real_t p_margin) {
const RayShape2DSW *ray = static_cast<const RayShape2DSW *>(p_shape_A);
if (p_shape_B->get_type() == PhysicsServer2D::SHAPE_RAY) {
return false;
}

Vector2 from = p_transform_A.get_origin();
Vector2 to = from + p_transform_A[1] * ray->get_length();
Vector2 to = from + p_transform_A[1] * (ray->get_length() + p_margin);
if (p_motion_A != Vector2()) {
//not the best but should be enough
Vector2 normal = (to - from).normalized();
Expand Down Expand Up @@ -226,9 +226,9 @@ bool CollisionSolver2DSW::solve(const Shape2DSW *p_shape_A, const Transform2D &p
}

if (swap) {
return solve_raycast(p_shape_B, p_motion_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true, sep_axis);
return solve_raycast(p_shape_B, p_motion_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true, sep_axis, p_margin_B);
} else {
return solve_raycast(p_shape_A, p_motion_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false, sep_axis);
return solve_raycast(p_shape_A, p_motion_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false, sep_axis, p_margin_A);
}

} else if (concave_B) {
Expand Down
2 changes: 1 addition & 1 deletion servers/physics_2d/collision_solver_2d_sw.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,7 +41,7 @@ class CollisionSolver2DSW {
static bool solve_static_line(const Shape2DSW *p_shape_A, const Transform2D &p_transform_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result);
static void concave_callback(void *p_userdata, Shape2DSW *p_convex);
static bool solve_concave(const Shape2DSW *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, Vector2 *sep_axis = nullptr, real_t p_margin_A = 0, real_t p_margin_B = 0);
static bool solve_raycast(const Shape2DSW *p_shape_A, const Vector2 &p_motion_A, const Transform2D &p_transform_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, Vector2 *sep_axis = nullptr);
static bool solve_raycast(const Shape2DSW *p_shape_A, const Vector2 &p_motion_A, const Transform2D &p_transform_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, Vector2 *sep_axis = nullptr, real_t p_margin = 0);

public:
static bool solve(const Shape2DSW *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, CallbackResult p_result_callback, void *p_userdata, Vector2 *sep_axis = nullptr, real_t p_margin_A = 0, real_t p_margin_B = 0);
Expand Down
27 changes: 15 additions & 12 deletions servers/physics_2d/space_2d_sw.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -543,7 +543,7 @@ int Space2DSW::test_body_ray_separation(Body2DSW *p_body, const Transform2D &p_t

for (int i = 0; i < p_result_max; i++) {
//reset results
r_results[i].collision_depth = 0;
r_results[i].collision_depth = -1.0;
}

int rays_found = 0;
Expand All @@ -561,6 +561,7 @@ int Space2DSW::test_body_ray_separation(Body2DSW *p_body, const Transform2D &p_t

do {
Vector2 recover_motion;
int recover_count = 0;

bool collided = false;

Expand All @@ -579,6 +580,8 @@ int Space2DSW::test_body_ray_separation(Body2DSW *p_body, const Transform2D &p_t

Transform2D body_shape_xform = body_transform * p_body->get_shape_transform(j);

Vector2 ray_normal = -body_shape_xform[1];

for (int i = 0; i < amount; i++) {
const CollisionObject2DSW *col_obj = intersection_query_results[i];
int shape_idx = intersection_query_subindex_results[i];
Expand Down Expand Up @@ -638,17 +641,23 @@ int Space2DSW::test_body_ray_separation(Body2DSW *p_body, const Transform2D &p_t
if (ray_index != -1) {
PhysicsServer2D::SeparationResult &result = r_results[ray_index];

recover_count += cbk.amount;
for (int k = 0; k < cbk.amount; k++) {
Vector2 a = sr[k * 2 + 0];
Vector2 b = sr[k * 2 + 1];
Vector2 separation = (b - a);

recover_motion += (b - a) / cbk.amount;
// Apply recovery without margin.
float depth = separation.length();
float separation_depth = depth - p_margin;
if (separation_depth > 0.0) {
recover_motion += separation * (separation_depth / depth);
}

float depth = a.distance_to(b);
if (depth > result.collision_depth) {
result.collision_depth = depth;
result.collision_point = b;
result.collision_normal = (b - a).normalized();
result.collision_normal = ray_normal;
result.collision_local_shape = j;
result.collider_shape = shape_idx;
result.collider = col_obj->get_self();
Expand All @@ -671,21 +680,15 @@ int Space2DSW::test_body_ray_separation(Body2DSW *p_body, const Transform2D &p_t
break;
}

recover_motion /= recover_count;

body_transform.elements[2] += recover_motion;
body_aabb.position += recover_motion;

recover_attempts--;
} while (recover_attempts);
}

//optimize results (remove non colliding)
for (int i = 0; i < rays_found; i++) {
if (r_results[i].collision_depth == 0) {
rays_found--;
SWAP(r_results[i], r_results[rays_found]);
}
}

r_recover_motion = body_transform.elements[2] - p_transform.elements[2];
return rays_found;
}
Expand Down
8 changes: 4 additions & 4 deletions servers/physics_3d/collision_solver_3d_sw.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -72,11 +72,11 @@ bool CollisionSolver3DSW::solve_static_plane(const Shape3DSW *p_shape_A, const T
return found;
}

bool CollisionSolver3DSW::solve_ray(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result) {
bool CollisionSolver3DSW::solve_ray(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin) {
const RayShape3DSW *ray = static_cast<const RayShape3DSW *>(p_shape_A);

Vector3 from = p_transform_A.origin;
Vector3 to = from + p_transform_A.basis.get_axis(2) * ray->get_length();
Vector3 to = from + p_transform_A.basis.get_axis(2) * (ray->get_length() + p_margin);
Vector3 support_A = to;

Transform ai = p_transform_B.affine_inverse();
Expand Down Expand Up @@ -212,9 +212,9 @@ bool CollisionSolver3DSW::solve_static(const Shape3DSW *p_shape_A, const Transfo
}

if (swap) {
return solve_ray(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true);
return solve_ray(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true, p_margin_B);
} else {
return solve_ray(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false);
return solve_ray(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false, p_margin_A);
}

} else if (concave_B) {
Expand Down
2 changes: 1 addition & 1 deletion servers/physics_3d/collision_solver_3d_sw.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,7 +40,7 @@ class CollisionSolver3DSW {
private:
static void concave_callback(void *p_userdata, Shape3DSW *p_convex);
static bool solve_static_plane(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result);
static bool solve_ray(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result);
static bool solve_ray(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin = 0);
static bool solve_concave(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin_A = 0, real_t p_margin_B = 0);
static void concave_distance_callback(void *p_userdata, Shape3DSW *p_convex);
static bool solve_distance_plane(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B);
Expand Down
27 changes: 15 additions & 12 deletions servers/physics_3d/space_3d_sw.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -574,7 +574,7 @@ int Space3DSW::test_body_ray_separation(Body3DSW *p_body, const Transform &p_tra

for (int i = 0; i < p_result_max; i++) {
//reset results
r_results[i].collision_depth = 0;
r_results[i].collision_depth = -1.0;
}

int rays_found = 0;
Expand All @@ -592,6 +592,7 @@ int Space3DSW::test_body_ray_separation(Body3DSW *p_body, const Transform &p_tra

do {
Vector3 recover_motion;
int recover_count = 0;

bool collided = false;

Expand All @@ -610,6 +611,8 @@ int Space3DSW::test_body_ray_separation(Body3DSW *p_body, const Transform &p_tra

Transform body_shape_xform = body_transform * p_body->get_shape_transform(j);

Vector3 ray_normal = -body_shape_xform.basis.get_axis(2);

for (int i = 0; i < amount; i++) {
const CollisionObject3DSW *col_obj = intersection_query_results[i];
int shape_idx = intersection_query_subindex_results[i];
Expand Down Expand Up @@ -645,17 +648,23 @@ int Space3DSW::test_body_ray_separation(Body3DSW *p_body, const Transform &p_tra
if (ray_index != -1) {
PhysicsServer3D::SeparationResult &result = r_results[ray_index];

recover_count += cbk.amount;
for (int k = 0; k < cbk.amount; k++) {
Vector3 a = sr[k * 2 + 0];
Vector3 b = sr[k * 2 + 1];
Vector3 separation = (b - a);

recover_motion += (b - a) / cbk.amount;
// Apply recovery without margin.
float depth = separation.length();
float separation_depth = depth - p_margin;
if (separation_depth > 0.0) {
recover_motion += separation * (separation_depth / depth);
}

float depth = a.distance_to(b);
if (depth > result.collision_depth) {
result.collision_depth = depth;
result.collision_point = b;
result.collision_normal = (b - a).normalized();
result.collision_normal = ray_normal;
result.collision_local_shape = j;
result.collider = col_obj->get_self();
result.collider_id = col_obj->get_instance_id();
Expand All @@ -679,21 +688,15 @@ int Space3DSW::test_body_ray_separation(Body3DSW *p_body, const Transform &p_tra
break;
}

recover_motion /= recover_count;

body_transform.origin += recover_motion;
body_aabb.position += recover_motion;

recover_attempts--;
} while (recover_attempts);
}

//optimize results (remove non colliding)
for (int i = 0; i < rays_found; i++) {
if (r_results[i].collision_depth == 0) {
rays_found--;
SWAP(r_results[i], r_results[rays_found]);
}
}

r_recover_motion = body_transform.origin - p_transform.origin;
return rays_found;
}
Expand Down

0 comments on commit a3d8043

Please sign in to comment.