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

Improved logic for KinematicBody collision recovery depth

Browse Source 
Allows more flexible collision detection with different safe margin values.

Kinematic body motion changes in 2D and 3D:
-Recovery only for depth > min contact depth to help with collision
detection consistency (rest info could be lost if recovery was too much)
-Adaptive min contact depth (based on margin) instead of space parameter
This commit is contained in:
PouleyKetchoupp 2021-10-05 17:00:55 -07:00
parent df69945f1f
commit 9bc1b4b90e
10 changed files with 25 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
doc/classes/Physics2DServer.xml
View File

@ -832,8 +832,6 @@
<constant name="SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS" value="6" enum="SpaceParameter"> <constant name="SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS" value="6" enum="SpaceParameter">
Constant to set/get the default solver bias for all physics constraints. A solver bias is a factor controlling how much two objects "rebound", after violating a constraint, to avoid leaving them in that state because of numerical imprecision. Constant to set/get the default solver bias for all physics constraints. A solver bias is a factor controlling how much two objects "rebound", after violating a constraint, to avoid leaving them in that state because of numerical imprecision.
</constant> </constant>
<constant name="SPACE_PARAM_TEST_MOTION_MIN_CONTACT_DEPTH" value="7" enum="SpaceParameter">
</constant>
<constant name="SHAPE_LINE" value="0" enum="ShapeType"> <constant name="SHAPE_LINE" value="0" enum="ShapeType">
This is the constant for creating line shapes. A line shape is an infinite line with an origin point, and a normal. Thus, it can be used for front/behind checks. This is the constant for creating line shapes. A line shape is an infinite line with an origin point, and a normal. Thus, it can be used for front/behind checks.
</constant> </constant>

2
doc/classes/PhysicsServer.xml
View File

@ -1297,8 +1297,6 @@
<constant name="SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS" value="7" enum="SpaceParameter"> <constant name="SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS" value="7" enum="SpaceParameter">
Constant to set/get the default solver bias for all physics constraints. A solver bias is a factor controlling how much two objects "rebound", after violating a constraint, to avoid leaving them in that state because of numerical imprecision. Constant to set/get the default solver bias for all physics constraints. A solver bias is a factor controlling how much two objects "rebound", after violating a constraint, to avoid leaving them in that state because of numerical imprecision.
</constant> </constant>
<constant name="SPACE_PARAM_TEST_MOTION_MIN_CONTACT_DEPTH" value="8" enum="SpaceParameter">
</constant>
<constant name="BODY_AXIS_LINEAR_X" value="1" enum="BodyAxis"> <constant name="BODY_AXIS_LINEAR_X" value="1" enum="BodyAxis">
</constant> </constant>
<constant name="BODY_AXIS_LINEAR_Y" value="2" enum="BodyAxis"> <constant name="BODY_AXIS_LINEAR_Y" value="2" enum="BodyAxis">

25
servers/physics/space_sw.cpp
View File

@ -34,6 +34,8 @@
#include "core/project_settings.h" #include "core/project_settings.h"
#include "physics_server_sw.h" #include "physics_server_sw.h"
#define TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR 0.05
_FORCE_INLINE_ static bool _can_collide_with(CollisionObjectSW *p_object, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { _FORCE_INLINE_ static bool _can_collide_with(CollisionObjectSW *p_object, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) {
if (!(p_object->get_collision_layer() & p_collision_mask)) { if (!(p_object->get_collision_layer() & p_collision_mask)) {
return false; return false;
@ -432,6 +434,8 @@ bool PhysicsDirectSpaceStateSW::rest_info(RID p_shape, const Transform &p_shape_
ShapeSW *shape = static_cast<PhysicsServerSW *>(PhysicsServer::get_singleton())->shape_owner.get(p_shape); ShapeSW *shape = static_cast<PhysicsServerSW *>(PhysicsServer::get_singleton())->shape_owner.get(p_shape);
ERR_FAIL_COND_V(!shape, 0); ERR_FAIL_COND_V(!shape, 0);
real_t min_contact_depth = p_margin * TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR;
AABB aabb = p_shape_xform.xform(shape->get_aabb()); AABB aabb = p_shape_xform.xform(shape->get_aabb());
aabb = aabb.grow(p_margin); aabb = aabb.grow(p_margin);
@ -441,7 +445,7 @@ bool PhysicsDirectSpaceStateSW::rest_info(RID p_shape, const Transform &p_shape_
rcd.best_len = 0; rcd.best_len = 0;
rcd.best_object = nullptr; rcd.best_object = nullptr;
rcd.best_shape = 0; rcd.best_shape = 0;
rcd.min_allowed_depth = space->test_motion_min_contact_depth; rcd.min_allowed_depth = min_contact_depth;
for (int i = 0; i < amount; i++) { for (int i = 0; i < amount; i++) {
if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) { if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) {
@ -753,6 +757,8 @@ bool SpaceSW::test_body_motion(BodySW *p_body, const Transform &p_from, const Ve
body_aabb = p_from.xform(p_body->get_inv_transform().xform(body_aabb)); body_aabb = p_from.xform(p_body->get_inv_transform().xform(body_aabb));
body_aabb = body_aabb.grow(p_margin); body_aabb = body_aabb.grow(p_margin);
real_t min_contact_depth = p_margin * TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR;
float motion_length = p_motion.length(); float motion_length = p_motion.length();
Vector3 motion_normal = p_motion / motion_length; Vector3 motion_normal = p_motion / motion_length;
@ -815,8 +821,9 @@ bool SpaceSW::test_body_motion(BodySW *p_body, const Transform &p_from, const Ve
break; break;
} }
Vector3 recover_motion; recovered = true;
Vector3 recover_motion;
for (int i = 0; i < cbk.amount; i++) { for (int i = 0; i < cbk.amount; i++) {
Vector3 a = sr[i * 2 + 0]; Vector3 a = sr[i * 2 + 0];
Vector3 b = sr[i * 2 + 1]; Vector3 b = sr[i * 2 + 1];
@ -827,9 +834,9 @@ bool SpaceSW::test_body_motion(BodySW *p_body, const Transform &p_from, const Ve
// Compute depth on recovered motion. // Compute depth on recovered motion.
float depth = n.dot(a + recover_motion) - d; float depth = n.dot(a + recover_motion) - d;
if (depth > 0.0) { if (depth > min_contact_depth + CMP_EPSILON) {
// Only recover if there is penetration. // Only recover if there is penetration.
recover_motion -= n * depth * 0.4; recover_motion -= n * (depth - min_contact_depth) * 0.4;
} }
} }
@ -838,8 +845,6 @@ bool SpaceSW::test_body_motion(BodySW *p_body, const Transform &p_from, const Ve
break; break;
} }
recovered = true;
body_transform.origin += recover_motion; body_transform.origin += recover_motion;
body_aabb.position += recover_motion; body_aabb.position += recover_motion;
@ -991,7 +996,7 @@ bool SpaceSW::test_body_motion(BodySW *p_body, const Transform &p_from, const Ve
rcd.best_shape = 0; rcd.best_shape = 0;
// Allowed depth can't be lower than motion length, in order to handle contacts at low speed. // Allowed depth can't be lower than motion length, in order to handle contacts at low speed.
rcd.min_allowed_depth = MIN(motion_length, test_motion_min_contact_depth); rcd.min_allowed_depth = MIN(motion_length, min_contact_depth);
int from_shape = best_shape != -1 ? best_shape : 0; int from_shape = best_shape != -1 ? best_shape : 0;
int to_shape = best_shape != -1 ? best_shape + 1 : p_body->get_shape_count(); int to_shape = best_shape != -1 ? best_shape + 1 : p_body->get_shape_count();
@ -1233,9 +1238,6 @@ void SpaceSW::set_param(PhysicsServer::SpaceParameter p_param, real_t p_value) {
case PhysicsServer::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS: case PhysicsServer::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS:
constraint_bias = p_value; constraint_bias = p_value;
break; break;
case PhysicsServer::SPACE_PARAM_TEST_MOTION_MIN_CONTACT_DEPTH:
test_motion_min_contact_depth = p_value;
break;
} }
} }
@ -1257,8 +1259,6 @@ real_t SpaceSW::get_param(PhysicsServer::SpaceParameter p_param) const {
return body_angular_velocity_damp_ratio; return body_angular_velocity_damp_ratio;
case PhysicsServer::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS: case PhysicsServer::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS:
return constraint_bias; return constraint_bias;
case PhysicsServer::SPACE_PARAM_TEST_MOTION_MIN_CONTACT_DEPTH:
return test_motion_min_contact_depth;
} }
return 0; return 0;
} }
@ -1289,7 +1289,6 @@ SpaceSW::SpaceSW() {
contact_recycle_radius = 0.01; contact_recycle_radius = 0.01;
contact_max_separation = 0.05; contact_max_separation = 0.05;
contact_max_allowed_penetration = 0.01; contact_max_allowed_penetration = 0.01;
test_motion_min_contact_depth = 0.00001;
constraint_bias = 0.01; constraint_bias = 0.01;
body_linear_velocity_sleep_threshold = GLOBAL_DEF("physics/3d/sleep_threshold_linear", 0.1); body_linear_velocity_sleep_threshold = GLOBAL_DEF("physics/3d/sleep_threshold_linear", 0.1);

1
servers/physics/space_sw.h
View File

@ -94,7 +94,6 @@ private:
real_t contact_max_separation; real_t contact_max_separation;
real_t contact_max_allowed_penetration; real_t contact_max_allowed_penetration;
real_t constraint_bias; real_t constraint_bias;
real_t test_motion_min_contact_depth;
enum { enum {

25
servers/physics_2d/space_2d_sw.cpp
View File

@ -34,6 +34,9 @@
#include "core/os/os.h" #include "core/os/os.h"
#include "core/pair.h" #include "core/pair.h"
#include "physics_2d_server_sw.h" #include "physics_2d_server_sw.h"
#define TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR 0.05
_FORCE_INLINE_ static bool _can_collide_with(CollisionObject2DSW *p_object, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { _FORCE_INLINE_ static bool _can_collide_with(CollisionObject2DSW *p_object, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) {
if (!(p_object->get_collision_layer() & p_collision_mask)) { if (!(p_object->get_collision_layer() & p_collision_mask)) {
return false; return false;
@ -434,6 +437,8 @@ bool Physics2DDirectSpaceStateSW::rest_info(RID p_shape, const Transform2D &p_sh
Shape2DSW *shape = Physics2DServerSW::singletonsw->shape_owner.get(p_shape); Shape2DSW *shape = Physics2DServerSW::singletonsw->shape_owner.get(p_shape);
ERR_FAIL_COND_V(!shape, 0); ERR_FAIL_COND_V(!shape, 0);
real_t min_contact_depth = p_margin * TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR;
Rect2 aabb = p_shape_xform.xform(shape->get_aabb()); Rect2 aabb = p_shape_xform.xform(shape->get_aabb());
aabb = aabb.merge(Rect2(aabb.position + p_motion, aabb.size)); //motion aabb = aabb.merge(Rect2(aabb.position + p_motion, aabb.size)); //motion
aabb = aabb.grow(p_margin); aabb = aabb.grow(p_margin);
@ -444,7 +449,7 @@ bool Physics2DDirectSpaceStateSW::rest_info(RID p_shape, const Transform2D &p_sh
rcd.best_len = 0; rcd.best_len = 0;
rcd.best_object = nullptr; rcd.best_object = nullptr;
rcd.best_shape = 0; rcd.best_shape = 0;
rcd.min_allowed_depth = space->test_motion_min_contact_depth; rcd.min_allowed_depth = min_contact_depth;
for (int i = 0; i < amount; i++) { for (int i = 0; i < amount; i++) {
if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) { if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) {
@ -757,6 +762,8 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
ExcludedShapeSW excluded_shape_pairs[max_excluded_shape_pairs]; ExcludedShapeSW excluded_shape_pairs[max_excluded_shape_pairs];
int excluded_shape_pair_count = 0; int excluded_shape_pair_count = 0;
real_t min_contact_depth = p_margin * TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR;
float motion_length = p_motion.length(); float motion_length = p_motion.length();
Vector2 motion_normal = p_motion / motion_length; Vector2 motion_normal = p_motion / motion_length;
@ -868,6 +875,8 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
break; break;
} }
recovered = true;
Vector2 recover_motion; Vector2 recover_motion;
for (int i = 0; i < cbk.amount; i++) { for (int i = 0; i < cbk.amount; i++) {
Vector2 a = sr[i * 2 + 0]; Vector2 a = sr[i * 2 + 0];
@ -879,9 +888,9 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
// Compute depth on recovered motion. // Compute depth on recovered motion.
float depth = n.dot(a + recover_motion) - d; float depth = n.dot(a + recover_motion) - d;
if (depth > 0.0) { if (depth > min_contact_depth + CMP_EPSILON) {
// Only recover if there is penetration. // Only recover if there is penetration.
recover_motion -= n * depth * 0.4; recover_motion -= n * (depth - min_contact_depth) * 0.4;
} }
} }
@ -890,8 +899,6 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
break; break;
} }
recovered = true;
body_transform.elements[2] += recover_motion; body_transform.elements[2] += recover_motion;
body_aabb.position += recover_motion; body_aabb.position += recover_motion;
@ -1068,7 +1075,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
rcd.best_shape = 0; rcd.best_shape = 0;
// Allowed depth can't be lower than motion length, in order to handle contacts at low speed. // Allowed depth can't be lower than motion length, in order to handle contacts at low speed.
rcd.min_allowed_depth = MIN(motion_length, test_motion_min_contact_depth); rcd.min_allowed_depth = MIN(motion_length, min_contact_depth);
int from_shape = best_shape != -1 ? best_shape : 0; int from_shape = best_shape != -1 ? best_shape : 0;
int to_shape = best_shape != -1 ? best_shape + 1 : p_body->get_shape_count(); int to_shape = best_shape != -1 ? best_shape + 1 : p_body->get_shape_count();
@ -1345,9 +1352,6 @@ void Space2DSW::set_param(Physics2DServer::SpaceParameter p_param, real_t p_valu
case Physics2DServer::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS: case Physics2DServer::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS:
constraint_bias = p_value; constraint_bias = p_value;
break; break;
case Physics2DServer::SPACE_PARAM_TEST_MOTION_MIN_CONTACT_DEPTH:
test_motion_min_contact_depth = p_value;
break;
} }
} }
@ -1367,8 +1371,6 @@ real_t Space2DSW::get_param(Physics2DServer::SpaceParameter p_param) const {
return body_time_to_sleep; return body_time_to_sleep;
case Physics2DServer::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS: case Physics2DServer::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS:
return constraint_bias; return constraint_bias;
case Physics2DServer::SPACE_PARAM_TEST_MOTION_MIN_CONTACT_DEPTH:
return test_motion_min_contact_depth;
} }
return 0; return 0;
} }
@ -1400,7 +1402,6 @@ Space2DSW::Space2DSW() {
contact_recycle_radius = 1.0; contact_recycle_radius = 1.0;
contact_max_separation = 1.5; contact_max_separation = 1.5;
contact_max_allowed_penetration = 0.3; contact_max_allowed_penetration = 0.3;
test_motion_min_contact_depth = 0.005;
constraint_bias = 0.2; constraint_bias = 0.2;
body_linear_velocity_sleep_threshold = GLOBAL_DEF("physics/2d/sleep_threshold_linear", 2.0); body_linear_velocity_sleep_threshold = GLOBAL_DEF("physics/2d/sleep_threshold_linear", 2.0);

1
servers/physics_2d/space_2d_sw.h
View File

@ -102,7 +102,6 @@ private:
real_t contact_max_separation; real_t contact_max_separation;
real_t contact_max_allowed_penetration; real_t contact_max_allowed_penetration;
real_t constraint_bias; real_t constraint_bias;
real_t test_motion_min_contact_depth;
enum { enum {

1
servers/physics_2d_server.cpp
View File

@ -657,7 +657,6 @@ void Physics2DServer::_bind_methods() {
BIND_ENUM_CONSTANT(SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_THRESHOLD); BIND_ENUM_CONSTANT(SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_THRESHOLD);
BIND_ENUM_CONSTANT(SPACE_PARAM_BODY_TIME_TO_SLEEP); BIND_ENUM_CONSTANT(SPACE_PARAM_BODY_TIME_TO_SLEEP);
BIND_ENUM_CONSTANT(SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS); BIND_ENUM_CONSTANT(SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS);
BIND_ENUM_CONSTANT(SPACE_PARAM_TEST_MOTION_MIN_CONTACT_DEPTH);
BIND_ENUM_CONSTANT(SHAPE_LINE); BIND_ENUM_CONSTANT(SHAPE_LINE);
BIND_ENUM_CONSTANT(SHAPE_RAY); BIND_ENUM_CONSTANT(SHAPE_RAY);

1
servers/physics_2d_server.h
View File

@ -261,7 +261,6 @@ public:
SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_THRESHOLD, SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_THRESHOLD,
SPACE_PARAM_BODY_TIME_TO_SLEEP, SPACE_PARAM_BODY_TIME_TO_SLEEP,
SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS, SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS,
SPACE_PARAM_TEST_MOTION_MIN_CONTACT_DEPTH,
}; };
virtual void space_set_param(RID p_space, SpaceParameter p_param, real_t p_value) = 0; virtual void space_set_param(RID p_space, SpaceParameter p_param, real_t p_value) = 0;

1
servers/physics_server.cpp
View File

@ -759,7 +759,6 @@ void PhysicsServer::_bind_methods() {
BIND_ENUM_CONSTANT(SPACE_PARAM_BODY_TIME_TO_SLEEP); BIND_ENUM_CONSTANT(SPACE_PARAM_BODY_TIME_TO_SLEEP);
BIND_ENUM_CONSTANT(SPACE_PARAM_BODY_ANGULAR_VELOCITY_DAMP_RATIO); BIND_ENUM_CONSTANT(SPACE_PARAM_BODY_ANGULAR_VELOCITY_DAMP_RATIO);
BIND_ENUM_CONSTANT(SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS); BIND_ENUM_CONSTANT(SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS);
BIND_ENUM_CONSTANT(SPACE_PARAM_TEST_MOTION_MIN_CONTACT_DEPTH);
BIND_ENUM_CONSTANT(BODY_AXIS_LINEAR_X); BIND_ENUM_CONSTANT(BODY_AXIS_LINEAR_X);
BIND_ENUM_CONSTANT(BODY_AXIS_LINEAR_Y); BIND_ENUM_CONSTANT(BODY_AXIS_LINEAR_Y);

1
servers/physics_server.h
View File

@ -250,7 +250,6 @@ public:
SPACE_PARAM_BODY_TIME_TO_SLEEP, SPACE_PARAM_BODY_TIME_TO_SLEEP,
SPACE_PARAM_BODY_ANGULAR_VELOCITY_DAMP_RATIO, SPACE_PARAM_BODY_ANGULAR_VELOCITY_DAMP_RATIO,
SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS, SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS,
SPACE_PARAM_TEST_MOTION_MIN_CONTACT_DEPTH
}; };
virtual void space_set_param(RID p_space, SpaceParameter p_param, real_t p_value) = 0; virtual void space_set_param(RID p_space, SpaceParameter p_param, real_t p_value) = 0;