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

Merge pull request #52953 from nekomatata/fix-collision-recovery-depth

Browse Source
This commit is contained in:
Rémi Verschelde 2021-09-28 12:11:38 +02:00 committed by GitHub
commit 60988a06c2
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
14 changed files with 49 additions and 49 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
doc/classes/CharacterBody2D.xml
View File

@ -133,7 +133,7 @@
<member name="floor_max_angle" type="float" setter="set_floor_max_angle" getter="get_floor_max_angle" default="0.785398"> <member name="floor_max_angle" type="float" setter="set_floor_max_angle" getter="get_floor_max_angle" default="0.785398">
Maximum angle (in radians) where a slope is still considered a floor (or a ceiling), rather than a wall, when calling [method move_and_slide]. The default value equals 45 degrees. Maximum angle (in radians) where a slope is still considered a floor (or a ceiling), rather than a wall, when calling [method move_and_slide]. The default value equals 45 degrees.
</member> </member>
<member name="floor_snap_length" type="float" setter="set_floor_snap_length" getter="get_floor_snap_length" default="0.0"> <member name="floor_snap_length" type="float" setter="set_floor_snap_length" getter="get_floor_snap_length" default="1.0">
Sets a snapping distance. When set to a value different from [code]0.0[/code], the body is kept attached to slopes when calling [method move_and_slide]. The snapping vector is determined by the given distance along the opposite direction of the [member up_direction]. Sets a snapping distance. When set to a value different from [code]0.0[/code], the body is kept attached to slopes when calling [method move_and_slide]. The snapping vector is determined by the given distance along the opposite direction of the [member up_direction].
As long as the snapping vector is in contact with the ground and the body moves against `up_direction`, the body will remain attached to the surface. Snapping is not applied if the body moves along `up_direction`, so it will be able to detach from the ground when jumping. As long as the snapping vector is in contact with the ground and the body moves against `up_direction`, the body will remain attached to the surface. Snapping is not applied if the body moves along `up_direction`, so it will be able to detach from the ground when jumping.
</member> </member>

2
doc/classes/PhysicsServer2D.xml
View File

@ -851,8 +851,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_WORLD_BOUNDARY" value="0" enum="ShapeType"> <constant name="SHAPE_WORLD_BOUNDARY" value="0" enum="ShapeType">
This is the constant for creating world boundary shapes. A world boundary shape is an [i]infinite[/i] line with an origin point, and a normal. Thus, it can be used for front/behind checks. This is the constant for creating world boundary shapes. A world boundary shape is an [i]infinite[/i] line with an origin point, and a normal. Thus, it can be used for front/behind checks.
</constant> </constant>

2
doc/classes/PhysicsServer3D.xml
View File

@ -1361,8 +1361,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">

22
scene/2d/physics_body_2d.cpp
View File

@ -1090,9 +1090,7 @@ void CharacterBody2D::_move_and_slide_grounded(double p_delta, bool p_was_on_flo
if (on_floor && floor_stop_on_slope && (linear_velocity.normalized() + up_direction).length() < 0.01) { if (on_floor && floor_stop_on_slope && (linear_velocity.normalized() + up_direction).length() < 0.01) {
Transform2D gt = get_global_transform(); Transform2D gt = get_global_transform();
if (result.travel.length() > margin) { if (result.travel.length() <= margin + CMP_EPSILON) {
gt.elements[2] -= result.travel.slide(up_direction);
} else {
gt.elements[2] -= result.travel; gt.elements[2] -= result.travel;
} }
set_global_transform(gt); set_global_transform(gt);
@ -1111,7 +1109,7 @@ void CharacterBody2D::_move_and_slide_grounded(double p_delta, bool p_was_on_flo
// Avoid to move forward on a wall if floor_block_on_wall is true. // Avoid to move forward on a wall if floor_block_on_wall is true.
if (p_was_on_floor && !on_floor && !vel_dir_facing_up) { if (p_was_on_floor && !on_floor && !vel_dir_facing_up) {
// If the movement is large the body can be prevented from reaching the walls. // If the movement is large the body can be prevented from reaching the walls.
if (result.travel.length() <= margin) { if (result.travel.length() <= margin + CMP_EPSILON) {
// Cancels the motion. // Cancels the motion.
Transform2D gt = get_global_transform(); Transform2D gt = get_global_transform();
gt.elements[2] -= result.travel; gt.elements[2] -= result.travel;
@ -1240,13 +1238,16 @@ void CharacterBody2D::_move_and_slide_free(double p_delta) {
} }
void CharacterBody2D::_snap_on_floor(bool was_on_floor, bool vel_dir_facing_up) { void CharacterBody2D::_snap_on_floor(bool was_on_floor, bool vel_dir_facing_up) {
if (Math::is_equal_approx(floor_snap_length, 0) || on_floor || !was_on_floor || vel_dir_facing_up) { if (on_floor || !was_on_floor || vel_dir_facing_up) {
return; return;
} }
// Snap by at least collision margin to keep floor state consistent.
real_t length = MAX(floor_snap_length, margin);
Transform2D gt = get_global_transform(); Transform2D gt = get_global_transform();
PhysicsServer2D::MotionResult result; PhysicsServer2D::MotionResult result;
if (move_and_collide(up_direction * -floor_snap_length, result, margin, true, false, true)) { if (move_and_collide(-up_direction * length, result, margin, true, false, true)) {
bool apply = true; bool apply = true;
if (result.get_angle(up_direction) <= floor_max_angle + FLOOR_ANGLE_THRESHOLD) { if (result.get_angle(up_direction) <= floor_max_angle + FLOOR_ANGLE_THRESHOLD) {
on_floor = true; on_floor = true;
@ -1274,12 +1275,15 @@ void CharacterBody2D::_snap_on_floor(bool was_on_floor, bool vel_dir_facing_up)
} }
bool CharacterBody2D::_on_floor_if_snapped(bool was_on_floor, bool vel_dir_facing_up) { bool CharacterBody2D::_on_floor_if_snapped(bool was_on_floor, bool vel_dir_facing_up) {
if (Math::is_equal_approx(floor_snap_length, 0) || up_direction == Vector2() || on_floor || !was_on_floor || vel_dir_facing_up) { if (up_direction == Vector2() || on_floor || !was_on_floor || vel_dir_facing_up) {
return false; return false;
} }
// Snap by at least collision margin to keep floor state consistent.
real_t length = MAX(floor_snap_length, margin);
PhysicsServer2D::MotionResult result; PhysicsServer2D::MotionResult result;
if (move_and_collide(up_direction * -floor_snap_length, result, margin, true, false, true)) { if (move_and_collide(-up_direction * length, result, margin, true, false, true)) {
if (result.get_angle(up_direction) <= floor_max_angle + FLOOR_ANGLE_THRESHOLD) { if (result.get_angle(up_direction) <= floor_max_angle + FLOOR_ANGLE_THRESHOLD) {
return true; return true;
} }
@ -1568,7 +1572,7 @@ void CharacterBody2D::_bind_methods() {
ADD_PROPERTY(PropertyInfo(Variant::BOOL, "floor_constant_speed"), "set_floor_constant_speed_enabled", "is_floor_constant_speed_enabled"); ADD_PROPERTY(PropertyInfo(Variant::BOOL, "floor_constant_speed"), "set_floor_constant_speed_enabled", "is_floor_constant_speed_enabled");
ADD_PROPERTY(PropertyInfo(Variant::BOOL, "floor_block_on_wall"), "set_floor_block_on_wall_enabled", "is_floor_block_on_wall_enabled"); ADD_PROPERTY(PropertyInfo(Variant::BOOL, "floor_block_on_wall"), "set_floor_block_on_wall_enabled", "is_floor_block_on_wall_enabled");
ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "floor_max_angle", PROPERTY_HINT_RANGE, "0,180,0.1,radians"), "set_floor_max_angle", "get_floor_max_angle"); ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "floor_max_angle", PROPERTY_HINT_RANGE, "0,180,0.1,radians"), "set_floor_max_angle", "get_floor_max_angle");
ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "floor_snap_length", PROPERTY_HINT_RANGE, "0,1000,0.1"), "set_floor_snap_length", "get_floor_snap_length"); ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "floor_snap_length", PROPERTY_HINT_RANGE, "0,32,0.1,or_greater"), "set_floor_snap_length", "get_floor_snap_length");
ADD_GROUP("Moving platform", "moving_platform"); ADD_GROUP("Moving platform", "moving_platform");
ADD_PROPERTY(PropertyInfo(Variant::INT, "moving_platform_floor_layers", PROPERTY_HINT_LAYERS_2D_PHYSICS), "set_moving_platform_floor_layers", "get_moving_platform_floor_layers"); ADD_PROPERTY(PropertyInfo(Variant::INT, "moving_platform_floor_layers", PROPERTY_HINT_LAYERS_2D_PHYSICS), "set_moving_platform_floor_layers", "get_moving_platform_floor_layers");
ADD_PROPERTY(PropertyInfo(Variant::INT, "moving_platform_wall_layers", PROPERTY_HINT_LAYERS_2D_PHYSICS), "set_moving_platform_wall_layers", "get_moving_platform_wall_layers"); ADD_PROPERTY(PropertyInfo(Variant::INT, "moving_platform_wall_layers", PROPERTY_HINT_LAYERS_2D_PHYSICS), "set_moving_platform_wall_layers", "get_moving_platform_wall_layers");

2
scene/2d/physics_body_2d.h
View File

@ -334,7 +334,7 @@ private:
int max_slides = 4; int max_slides = 4;
int platform_layer; int platform_layer;
real_t floor_max_angle = Math::deg2rad((real_t)45.0); real_t floor_max_angle = Math::deg2rad((real_t)45.0);
real_t floor_snap_length = 0; real_t floor_snap_length = 1;
real_t free_mode_min_slide_angle = Math::deg2rad((real_t)15.0); real_t free_mode_min_slide_angle = Math::deg2rad((real_t)15.0);
Vector2 up_direction = Vector2(0.0, -1.0); Vector2 up_direction = Vector2(0.0, -1.0);
uint32_t moving_platform_floor_layers = UINT32_MAX; uint32_t moving_platform_floor_layers = UINT32_MAX;

14
scene/3d/physics_body_3d.cpp
View File

@ -1146,8 +1146,7 @@ void CharacterBody3D::_move_and_slide_grounded(double p_delta, bool p_was_on_flo
if (collision_state.floor && floor_stop_on_slope && (linear_velocity.normalized() + up_direction).length() < 0.01) { if (collision_state.floor && floor_stop_on_slope && (linear_velocity.normalized() + up_direction).length() < 0.01) {
Transform3D gt = get_global_transform(); Transform3D gt = get_global_transform();
real_t travel_total = result.travel.length(); if (result.travel.length() <= margin + CMP_EPSILON) {
if (travel_total <= margin + CMP_EPSILON) {
gt.origin -= result.travel; gt.origin -= result.travel;
} }
set_global_transform(gt); set_global_transform(gt);
@ -1186,7 +1185,7 @@ void CharacterBody3D::_move_and_slide_grounded(double p_delta, bool p_was_on_flo
Transform3D gt = get_global_transform(); Transform3D gt = get_global_transform();
real_t travel_total = result.travel.length(); real_t travel_total = result.travel.length();
real_t cancel_dist_max = MIN(0.1, margin * 20); real_t cancel_dist_max = MIN(0.1, margin * 20);
if (travel_total < margin + CMP_EPSILON) { if (travel_total <= margin + CMP_EPSILON) {
gt.origin -= result.travel; gt.origin -= result.travel;
} else if (travel_total < cancel_dist_max) { // If the movement is large the body can be prevented from reaching the walls. } else if (travel_total < cancel_dist_max) { // If the movement is large the body can be prevented from reaching the walls.
gt.origin -= result.travel.slide(up_direction); gt.origin -= result.travel.slide(up_direction);
@ -1377,7 +1376,9 @@ void CharacterBody3D::_snap_on_floor(bool was_on_floor, bool vel_dir_facing_up)
return; return;
} }
// Snap by at least collision margin to keep floor state consistent.
real_t length = MAX(floor_snap_length, margin); real_t length = MAX(floor_snap_length, margin);
Transform3D gt = get_global_transform(); Transform3D gt = get_global_transform();
PhysicsServer3D::MotionResult result; PhysicsServer3D::MotionResult result;
if (move_and_collide(-up_direction * length, result, margin, true, 4, false, true)) { if (move_and_collide(-up_direction * length, result, margin, true, 4, false, true)) {
@ -1403,12 +1404,15 @@ void CharacterBody3D::_snap_on_floor(bool was_on_floor, bool vel_dir_facing_up)
} }
bool CharacterBody3D::_on_floor_if_snapped(bool was_on_floor, bool vel_dir_facing_up) { bool CharacterBody3D::_on_floor_if_snapped(bool was_on_floor, bool vel_dir_facing_up) {
if (Math::is_zero_approx(floor_snap_length) || up_direction == Vector3() || collision_state.floor || !was_on_floor || vel_dir_facing_up) { if (up_direction == Vector3() || collision_state.floor || !was_on_floor || vel_dir_facing_up) {
return false; return false;
} }
// Snap by at least collision margin to keep floor state consistent.
real_t length = MAX(floor_snap_length, margin);
PhysicsServer3D::MotionResult result; PhysicsServer3D::MotionResult result;
if (move_and_collide(-up_direction * floor_snap_length, result, margin, true, 4, false, true)) { if (move_and_collide(-up_direction * length, result, margin, true, 4, false, true)) {
CollisionState result_state; CollisionState result_state;
// Don't apply direction for any type. // Don't apply direction for any type.
_set_collision_direction(result, result_state, CollisionState()); _set_collision_direction(result, result_state, CollisionState());

24
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/templates/pair.h" #include "core/templates/pair.h"
#include "physics_server_2d_sw.h" #include "physics_server_2d_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;
@ -435,6 +438,8 @@ bool PhysicsDirectSpaceState2DSW::rest_info(RID p_shape, const Transform2D &p_sh
Shape2DSW *shape = PhysicsServer2DSW::singletonsw->shape_owner.getornull(p_shape); Shape2DSW *shape = PhysicsServer2DSW::singletonsw->shape_owner.getornull(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);
@ -445,7 +450,7 @@ bool PhysicsDirectSpaceState2DSW::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)) {
@ -569,6 +574,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;
real_t motion_length = p_motion.length(); real_t motion_length = p_motion.length();
Vector2 motion_normal = p_motion / motion_length; Vector2 motion_normal = p_motion / motion_length;
@ -671,6 +678,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];
@ -682,9 +691,9 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
// Compute depth on recovered motion. // Compute depth on recovered motion.
real_t depth = n.dot(a + recover_motion) - d; real_t 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;
} }
} }
@ -693,8 +702,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;
@ -870,7 +877,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();
@ -1141,9 +1148,6 @@ void Space2DSW::set_param(PhysicsServer2D::SpaceParameter p_param, real_t p_valu
case PhysicsServer2D::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS: case PhysicsServer2D::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS:
constraint_bias = p_value; constraint_bias = p_value;
break; break;
case PhysicsServer2D::SPACE_PARAM_TEST_MOTION_MIN_CONTACT_DEPTH:
test_motion_min_contact_depth = p_value;
break;
} }
} }
@ -1163,8 +1167,6 @@ real_t Space2DSW::get_param(PhysicsServer2D::SpaceParameter p_param) const {
return body_time_to_sleep; return body_time_to_sleep;
case PhysicsServer2D::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS: case PhysicsServer2D::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS:
return constraint_bias; return constraint_bias;
case PhysicsServer2D::SPACE_PARAM_TEST_MOTION_MIN_CONTACT_DEPTH:
return test_motion_min_contact_depth;
} }
return 0; return 0;
} }

1
servers/physics_2d/space_2d_sw.h
View File

@ -102,7 +102,6 @@ private:
real_t contact_max_separation = 1.5; real_t contact_max_separation = 1.5;
real_t contact_max_allowed_penetration = 0.3; real_t contact_max_allowed_penetration = 0.3;
real_t constraint_bias = 0.2; real_t constraint_bias = 0.2;
real_t test_motion_min_contact_depth = 0.005;
enum { enum {
INTERSECTION_QUERY_MAX = 2048 INTERSECTION_QUERY_MAX = 2048

24
servers/physics_3d/space_3d_sw.cpp
View File

@ -34,6 +34,8 @@
#include "core/config/project_settings.h" #include "core/config/project_settings.h"
#include "physics_server_3d_sw.h" #include "physics_server_3d_sw.h"
#define TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR 0.05
_FORCE_INLINE_ static bool _can_collide_with(CollisionObject3DSW *p_object, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { _FORCE_INLINE_ static bool _can_collide_with(CollisionObject3DSW *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;
@ -488,13 +490,15 @@ bool PhysicsDirectSpaceState3DSW::rest_info(RID p_shape, const Transform3D &p_sh
Shape3DSW *shape = PhysicsServer3DSW::singletonsw->shape_owner.getornull(p_shape); Shape3DSW *shape = PhysicsServer3DSW::singletonsw->shape_owner.getornull(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);
int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, Space3DSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, Space3DSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results);
_RestCallbackData rcd; _RestCallbackData rcd;
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)) {
@ -658,6 +662,8 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform3D &p_from, co
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;
real_t motion_length = p_motion.length(); real_t motion_length = p_motion.length();
Vector3 motion_normal = p_motion / motion_length; Vector3 motion_normal = p_motion / motion_length;
@ -711,8 +717,9 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform3D &p_from, co
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];
@ -723,9 +730,9 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform3D &p_from, co
// Compute depth on recovered motion. // Compute depth on recovered motion.
real_t depth = n.dot(a + recover_motion) - d; real_t 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;
} }
} }
@ -734,8 +741,6 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform3D &p_from, co
break; break;
} }
recovered = true;
body_transform.origin += recover_motion; body_transform.origin += recover_motion;
body_aabb.position += recover_motion; body_aabb.position += recover_motion;
@ -889,7 +894,7 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform3D &p_from, co
} }
// 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();
@ -1158,9 +1163,6 @@ void Space3DSW::set_param(PhysicsServer3D::SpaceParameter p_param, real_t p_valu
case PhysicsServer3D::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS: case PhysicsServer3D::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS:
constraint_bias = p_value; constraint_bias = p_value;
break; break;
case PhysicsServer3D::SPACE_PARAM_TEST_MOTION_MIN_CONTACT_DEPTH:
test_motion_min_contact_depth = p_value;
break;
} }
} }
@ -1182,8 +1184,6 @@ real_t Space3DSW::get_param(PhysicsServer3D::SpaceParameter p_param) const {
return body_angular_velocity_damp_ratio; return body_angular_velocity_damp_ratio;
case PhysicsServer3D::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS: case PhysicsServer3D::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS:
return constraint_bias; return constraint_bias;
case PhysicsServer3D::SPACE_PARAM_TEST_MOTION_MIN_CONTACT_DEPTH:
return test_motion_min_contact_depth;
} }
return 0; return 0;
} }

1
servers/physics_3d/space_3d_sw.h
View File

@ -96,7 +96,6 @@ private:
real_t contact_max_separation = 0.05; real_t contact_max_separation = 0.05;
real_t contact_max_allowed_penetration = 0.01; real_t contact_max_allowed_penetration = 0.01;
real_t constraint_bias = 0.01; real_t constraint_bias = 0.01;
real_t test_motion_min_contact_depth = 0.00001;
enum { enum {
INTERSECTION_QUERY_MAX = 2048 INTERSECTION_QUERY_MAX = 2048

1
servers/physics_server_2d.cpp
View File

@ -670,7 +670,6 @@ void PhysicsServer2D::_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_WORLD_BOUNDARY); BIND_ENUM_CONSTANT(SHAPE_WORLD_BOUNDARY);
BIND_ENUM_CONSTANT(SHAPE_SEPARATION_RAY); BIND_ENUM_CONSTANT(SHAPE_SEPARATION_RAY);

1
servers/physics_server_2d.h
View File

@ -262,7 +262,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_3d.cpp
View File

@ -867,7 +867,6 @@ void PhysicsServer3D::_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_3d.h
View File

@ -271,7 +271,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;