maxmustermann/godot
1
0
Fork 0
Code Issues Pull requests Projects Releases Wiki Activity

Fix RayShape collision detection

Browse source 
One-way collision is disabled for both rigid bodies and character
bodies.

Kinematic margin is now applied to ray shapes to help getting consistent
results in slopes and flat surfaces.

Convex shapes don't return inverted normals when a segment test starts
inside (raycasting will be made consistent in a separate patch).

Ray shapes also discard contacts when fully contained inside a shape
and when the contact direction is inverted, so the behavior is
consistent with all shape types. Now they always separate only when
intersecting the top of a shape (for downward rays).
This commit is contained in:
PouleyKetchoupp 2021-08-17 18:41:21 -07:00
parent 45c7af9862
commit 829fb4fba1
8 changed files with 57 additions and 34 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

4
servers/physics_2d/body_pair_2d_sw.cpp
View file

@ -298,7 +298,7 @@ bool BodyPair2DSW::setup(real_t p_step) {
}
if (!prev_collided) {
if (A->is_shape_set_as_one_way_collision(shape_A)) {
if (shape_B_ptr->allows_one_way_collision() && A->is_shape_set_as_one_way_collision(shape_A)) {
Vector2 direction = xform_A.get_axis(1).normalized();
bool valid = false;
for (int i = 0; i < contact_count; i++) {
@ -319,7 +319,7 @@ bool BodyPair2DSW::setup(real_t p_step) {
}
}
if (B->is_shape_set_as_one_way_collision(shape_B)) {
if (shape_A_ptr->allows_one_way_collision() && B->is_shape_set_as_one_way_collision(shape_B)) {
Vector2 direction = xform_B.get_axis(1).normalized();
bool valid = false;
for (int i = 0; i < contact_count; i++) {

40
servers/physics_2d/collision_solver_2d_sw.cpp
View file

@ -73,14 +73,14 @@ bool CollisionSolver2DSW::solve_static_world_margin(const Shape2DSW *p_shape_A,
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 *r_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();
@ -94,8 +94,24 @@ bool CollisionSolver2DSW::solve_raycast(const Shape2DSW *p_shape_A, const Vector
Vector2 p, n;
if (!p_shape_B->intersect_segment(from, to, p, n)) {
if (sep_axis) {
*sep_axis = p_transform_A[1].normalized();
if (r_sep_axis) {
*r_sep_axis = p_transform_A[1].normalized();
}
return false;
}
// Discard contacts when the ray is fully contained inside the shape.
if (n == Vector2()) {
if (r_sep_axis) {
*r_sep_axis = p_transform_A[1].normalized();
}
return false;
}
// Discard contacts in the wrong direction.
if (n.dot(from - to) < CMP_EPSILON) {
if (r_sep_axis) {
*r_sep_axis = p_transform_A[1].normalized();
}
return false;
}
@ -149,7 +165,7 @@ void CollisionSolver2DSW::concave_callback(void *p_userdata, Shape2DSW *p_convex
cinfo.collisions++;
}
bool CollisionSolver2DSW::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, real_t p_margin_A, real_t p_margin_B) {
bool CollisionSolver2DSW::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 *r_sep_axis, real_t p_margin_A, real_t p_margin_B) {
const ConcaveShape2DSW *concave_B = static_cast<const ConcaveShape2DSW *>(p_shape_B);
_ConcaveCollisionInfo2D cinfo;
@ -162,7 +178,7 @@ bool CollisionSolver2DSW::solve_concave(const Shape2DSW *p_shape_A, const Transf
cinfo.swap_result = p_swap_result;
cinfo.collided = false;
cinfo.collisions = 0;
cinfo.sep_axis = sep_axis;
cinfo.sep_axis = r_sep_axis;
cinfo.margin_A = p_margin_A;
cinfo.margin_B = p_margin_B;
@ -193,7 +209,7 @@ bool CollisionSolver2DSW::solve_concave(const Shape2DSW *p_shape_A, const Transf
return cinfo.collided;
}
bool CollisionSolver2DSW::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, real_t p_margin_A, real_t p_margin_B) {
bool CollisionSolver2DSW::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 *r_sep_axis, real_t p_margin_A, real_t p_margin_B) {
PhysicsServer2D::ShapeType type_A = p_shape_A->get_type();
PhysicsServer2D::ShapeType type_B = p_shape_B->get_type();
bool concave_A = p_shape_A->is_concave();
@ -226,9 +242,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, r_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, r_sep_axis, p_margin_A);
}
} else if (concave_B) {
@ -237,12 +253,12 @@ bool CollisionSolver2DSW::solve(const Shape2DSW *p_shape_A, const Transform2D &p
}
if (!swap) {
return solve_concave(p_shape_A, p_transform_A, p_motion_A, p_shape_B, p_transform_B, p_motion_B, p_result_callback, p_userdata, false, sep_axis, margin_A, margin_B);
return solve_concave(p_shape_A, p_transform_A, p_motion_A, p_shape_B, p_transform_B, p_motion_B, p_result_callback, p_userdata, false, r_sep_axis, margin_A, margin_B);
} else {
return solve_concave(p_shape_B, p_transform_B, p_motion_B, p_shape_A, p_transform_A, p_motion_A, p_result_callback, p_userdata, true, sep_axis, margin_A, margin_B);
return solve_concave(p_shape_B, p_transform_B, p_motion_B, p_shape_A, p_transform_A, p_motion_A, p_result_callback, p_userdata, true, r_sep_axis, margin_A, margin_B);
}
} else {
return collision_solver(p_shape_A, p_transform_A, p_motion_A, p_shape_B, p_transform_B, p_motion_B, p_result_callback, p_userdata, false, sep_axis, margin_A, margin_B);
return collision_solver(p_shape_A, p_transform_A, p_motion_A, p_shape_B, p_transform_B, p_motion_B, p_result_callback, p_userdata, false, r_sep_axis, margin_A, margin_B);
}
}

6
servers/physics_2d/collision_solver_2d_sw.h
View file

@ -40,11 +40,11 @@ public:
private:
static bool solve_static_world_margin(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_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 *r_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 *r_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);
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 *r_sep_axis = nullptr, real_t p_margin_A = 0, real_t p_margin_B = 0);
};
#endif // COLLISION_SOLVER_2D_SW_H

9
servers/physics_2d/shape_2d_sw.cpp
View file

@ -570,14 +570,7 @@ bool ConvexPolygonShape2DSW::intersect_segment(const Vector2 &p_begin, const Vec
}
}
if (inters) {
if (n.dot(r_normal) > 0) {
r_normal = -r_normal;
}
}
//return get_aabb().intersects_segment(p_begin,p_end,&r_point,&r_normal);
return inters; //todo
return inters;
}
real_t ConvexPolygonShape2DSW::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const {

4
servers/physics_2d/shape_2d_sw.h
View file

@ -64,6 +64,8 @@ public:
_FORCE_INLINE_ Rect2 get_aabb() const { return aabb; }
_FORCE_INLINE_ bool is_configured() const { return configured; }
virtual bool allows_one_way_collision() const { return true; }
virtual bool is_concave() const { return false; }
virtual bool contains_point(const Vector2 &p_point) const = 0;
@ -187,6 +189,8 @@ public:
virtual PhysicsServer2D::ShapeType get_type() const { return PhysicsServer2D::SHAPE_RAY; }
virtual bool allows_one_way_collision() const override { return false; }
virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal, p_transform, r_min, r_max); }
virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const;

8
servers/physics_2d/space_2d_sw.cpp
View file

@ -621,7 +621,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
Transform2D col_obj_shape_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx);
if (col_obj->is_shape_set_as_one_way_collision(shape_idx)) {
if (body_shape->allows_one_way_collision() && col_obj->is_shape_set_as_one_way_collision(shape_idx)) {
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);
@ -762,7 +762,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
//test initial overlap
if (CollisionSolver2DSW::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), nullptr, nullptr, nullptr, 0)) {
if (col_obj->is_shape_set_as_one_way_collision(col_shape_idx)) {
if (body_shape->allows_one_way_collision() && col_obj->is_shape_set_as_one_way_collision(col_shape_idx)) {
Vector2 direction = col_obj_shape_xform.get_axis(1).normalized();
if (motion_normal.dot(direction) < 0) {
continue;
@ -806,7 +806,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
}
}
if (col_obj->is_shape_set_as_one_way_collision(col_shape_idx)) {
if (body_shape->allows_one_way_collision() && col_obj->is_shape_set_as_one_way_collision(col_shape_idx)) {
Vector2 cd[2];
PhysicsServer2DSW::CollCbkData cbk;
cbk.max = 1;
@ -904,7 +904,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
Transform2D col_obj_shape_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx);
if (col_obj->is_shape_set_as_one_way_collision(shape_idx)) {
if (body_shape->allows_one_way_collision() && col_obj->is_shape_set_as_one_way_collision(shape_idx)) {
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);

18
servers/physics_3d/collision_solver_3d_sw.cpp
View file

@ -89,11 +89,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 Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result) {
bool CollisionSolver3DSW::solve_ray(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &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;
Transform3D ai = p_transform_B.affine_inverse();
@ -106,6 +106,16 @@ bool CollisionSolver3DSW::solve_ray(const Shape3DSW *p_shape_A, const Transform3
return false;
}
// Discard contacts when the ray is fully contained inside the shape.
if (n == Vector3()) {
return false;
}
// Discard contacts in the wrong direction.
if (n.dot(from - to) < CMP_EPSILON) {
return false;
}
Vector3 support_B = p_transform_B.xform(p);
if (ray->get_slips_on_slope()) {
Vector3 global_n = ai.basis.xform_inv(n).normalized();
@ -370,9 +380,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 (type_B == PhysicsServer3D::SHAPE_SOFT_BODY) {

2
servers/physics_3d/collision_solver_3d_sw.h
View file

@ -43,7 +43,7 @@ private:
static void soft_body_concave_callback(void *p_userdata, Shape3DSW *p_convex);
static void concave_callback(void *p_userdata, Shape3DSW *p_convex);
static bool solve_static_plane(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result);
static bool solve_ray(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result);
static bool solve_ray(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin = 0);
static bool solve_soft_body(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result);
static bool solve_concave(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &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);