godotengine
/
godot
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Fix Rayshape recovery in test_body_ray_separation 

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 depth takes into account the current recovery vector (just like
test_body_motion) to fix jittering issues with multiple ray shapes due
to applying too much recovery.
This commit is contained in:
PouleyKetchoupp 2021-01-08 13:07:50 -07:00
parent df69945f1f
commit 255febefb2
6 changed files with 40 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
servers/physics/collision_solver_sw.cpp
View File

@ -89,11 +89,11 @@ 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();
@ -232,9 +232,9 @@ bool CollisionSolverSW::solve_static(const ShapeSW *p_shape_A, const Transform &
}
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) {

2
servers/physics/collision_solver_sw.h
View File

@ -40,7 +40,7 @@ public:
private:
static bool 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 bool 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);

28
servers/physics/space_sw.cpp
View File

@ -589,7 +589,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;
@ -664,18 +664,28 @@ int SpaceSW::test_body_ray_separation(BodySW *p_body, const Transform &p_transfo
Vector3 a = sr[k * 2 + 0];
Vector3 b = sr[k * 2 + 1];
recover_motion += (b - a) / cbk.amount;
// Compute plane on b towards a.
Vector3 n = (a - b).normalized();
float d = n.dot(b);
// Compute depth on recovered motion.
float depth = n.dot(a + recover_motion) - d;
// Apply recovery without margin.
float separation_depth = depth - p_margin;
if (separation_depth > 0.0) {
// Only recover if there is penetration.
recover_motion -= n * separation_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 = -n;
result.collision_local_shape = j;
result.collider = col_obj->get_self();
result.collider_id = col_obj->get_instance_id();
result.collider_shape = shape_idx;
//result.collider_metadata = col_obj->get_shape_metadata(shape_idx);
if (col_obj->get_type() == CollisionObjectSW::TYPE_BODY) {
BodySW *body = (BodySW *)col_obj;
@ -700,14 +710,6 @@ int SpaceSW::test_body_ray_separation(BodySW *p_body, const Transform &p_transfo
} 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;
}

8
servers/physics_2d/collision_solver_2d_sw.cpp
View File

@ -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() == 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();
@ -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) {

2
servers/physics_2d/collision_solver_2d_sw.h
View File

@ -41,7 +41,7 @@ private:
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 bool 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);

27
servers/physics_2d/space_2d_sw.cpp
View File

@ -560,7 +560,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;
@ -660,13 +660,24 @@ int Space2DSW::test_body_ray_separation(Body2DSW *p_body, const Transform2D &p_t
Vector2 a = sr[k * 2 + 0];
Vector2 b = sr[k * 2 + 1];
recover_motion += (b - a) / cbk.amount;
// Compute plane on b towards a.
Vector2 n = (a - b).normalized();
float d = n.dot(b);
// Compute depth on recovered motion.
float depth = n.dot(a + recover_motion) - d;
// Apply recovery without margin.
float separation_depth = depth - p_margin;
if (separation_depth > 0.0) {
// Only recover if there is penetration.
recover_motion -= n * separation_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 = -n;
result.collision_local_shape = j;
result.collider_shape = shape_idx;
result.collider = col_obj->get_self();
@ -696,14 +707,6 @@ int Space2DSW::test_body_ray_separation(Body2DSW *p_body, const Transform2D &p_t
} 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;
}