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 godotengine#34098
Fixes godotengine#34663
  • Loading branch information
@pouleyKetchoupp
pouleyKetchoupp committed Jan 8, 2021
1 parent 292bbb8 commit ed52bfd
Show file tree
Hide file tree
Showing 6 changed files with 40 additions and 34 deletions.
8 changes: 4 additions & 4 deletions servers/physics/collision_solver_sw.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -71,12 +71,12 @@ bool CollisionSolverSW::solve_static_plane(const ShapeSW *p_shape_A, const Trans
return found;
}

bool CollisionSolverSW::solve_ray(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result) {
bool CollisionSolverSW::solve_ray(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin) {

const RayShapeSW *ray = static_cast<const RayShapeSW *>(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 @@ -214,9 +214,9 @@ bool CollisionSolverSW::solve_static(const ShapeSW *p_shape_A, const Transform &
return false;

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/collision_solver_sw.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,7 +40,7 @@ class CollisionSolverSW {
private:
static void concave_callback(void *p_userdata, ShapeSW *p_convex);
static bool solve_static_plane(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result);
static bool solve_ray(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result);
static bool solve_ray(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *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 ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *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, ShapeSW *p_convex);
static bool solve_distance_plane(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *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/space_sw.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -570,7 +570,7 @@ int SpaceSW::test_body_ray_separation(BodySW *p_body, const Transform &p_transfo

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 @@ -589,6 +589,7 @@ int SpaceSW::test_body_ray_separation(BodySW *p_body, const Transform &p_transfo
do {

Vector3 recover_motion;
int recover_count = 0;

bool collided = false;

Expand All @@ -605,6 +606,8 @@ int SpaceSW::test_body_ray_separation(BodySW *p_body, const Transform &p_transfo

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 CollisionObjectSW *col_obj = intersection_query_results[i];
Expand Down Expand Up @@ -641,18 +644,24 @@ int SpaceSW::test_body_ray_separation(BodySW *p_body, const Transform &p_transfo
if (ray_index != -1) {
PhysicsServer::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 @@ -676,21 +685,15 @@ int SpaceSW::test_body_ray_separation(BodySW *p_body, const Transform &p_transfo
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
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 @@ -72,14 +72,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() == Physics2DServer::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 @@ -230,9 +230,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 = NULL, 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 = NULL);
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 = NULL, 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 = NULL, 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 @@ -536,7 +536,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 @@ -555,6 +555,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 @@ -571,6 +572,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];
Expand Down Expand Up @@ -633,18 +636,24 @@ int Space2DSW::test_body_ray_separation(Body2DSW *p_body, const Transform2D &p_t

Physics2DServer::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 @@ -667,21 +676,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

0 comments on commit ed52bfd

Please sign in to comment.