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

Merge pull request #50063 from nekomatata/more-accurate-move-and-slide 

Make move_and_slide collision detection more accurate
This commit is contained in:
Rémi Verschelde 2021-07-13 20:41:09 +02:00 committed by GitHub
parent 7bc52e56c0 cf1ddfdb90
commit bc6ea71771
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
4 changed files with 154 additions and 69 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
scene/2d/physics_body_2d.cpp
View File

@ -86,11 +86,11 @@ bool PhysicsBody2D::move_and_collide(const Vector2 &p_motion, bool p_infinite_in
// Restore direction of motion to be along original motion,
// in order to avoid sliding due to recovery,
// but only if collision depth is low enough to avoid tunneling.
real_t motion_length = p_motion.length();
if (motion_length > CMP_EPSILON) {
if (p_cancel_sliding) {
real_t motion_length = p_motion.length();
real_t precision = 0.001;
if (colliding && p_cancel_sliding) {
if (colliding) {
// Can't just use margin as a threshold because collision depth is calculated on unsafe motion,
// so even in normal resting cases the depth can be a bit more than the margin.
precision += motion_length * (r_result.collision_unsafe_fraction - r_result.collision_safe_fraction);
@ -101,16 +101,21 @@ bool PhysicsBody2D::move_and_collide(const Vector2 &p_motion, bool p_infinite_in
}
if (p_cancel_sliding) {
// When motion is null, recovery is the resulting motion.
Vector2 motion_normal;
if (motion_length > CMP_EPSILON) {
motion_normal = p_motion / motion_length;
}
// Check depth of recovery.
Vector2 motion_normal = p_motion / motion_length;
real_t dot = r_result.motion.dot(motion_normal);
Vector2 recovery = r_result.motion - motion_normal * dot;
real_t projected_length = r_result.motion.dot(motion_normal);
Vector2 recovery = r_result.motion - motion_normal * projected_length;
real_t recovery_length = recovery.length();
// Fixes cases where canceling slide causes the motion to go too deep into the ground,
// Becauses we're only taking rest information into account and not general recovery.
// because we're only taking rest information into account and not general recovery.
if (recovery_length < (real_t)p_margin + precision) {
// Apply adjustment to motion.
r_result.motion = motion_normal * dot;
r_result.motion = motion_normal * projected_length;
r_result.remainder = p_motion - r_result.motion;
}
}
@ -978,8 +983,9 @@ void CharacterBody2D::move_and_slide() {
floor_normal = Vector2();
floor_velocity = Vector2();
// No sliding on first attempt to keep floor motion stable when possible.
bool sliding_enabled = false;
// No sliding on first attempt to keep floor motion stable when possible,
// when stop on slope is enabled.
bool sliding_enabled = !stop_on_slope;
for (int iteration = 0; iteration < max_slides; ++iteration) {
PhysicsServer2D::MotionResult result;
bool found_collision = false;
@ -1018,7 +1024,11 @@ void CharacterBody2D::move_and_slide() {
if (stop_on_slope) {
if ((body_velocity_normal + up_direction).length() < 0.01) {
Transform2D gt = get_global_transform();
gt.elements[2] -= result.motion.slide(up_direction);
if (result.motion.length() > margin) {
gt.elements[2] -= result.motion.slide(up_direction);
} else {
gt.elements[2] -= result.motion;
}
set_global_transform(gt);
linear_velocity = Vector2();
return;
@ -1054,7 +1064,7 @@ void CharacterBody2D::move_and_slide() {
// Apply snap.
Transform2D gt = get_global_transform();
PhysicsServer2D::MotionResult result;
if (move_and_collide(snap, infinite_inertia, result, margin, false, true)) {
if (move_and_collide(snap, infinite_inertia, result, margin, false, true, false)) {
bool apply = true;
if (up_direction != Vector2()) {
if (Math::acos(result.collision_normal.dot(up_direction)) <= floor_max_angle + FLOOR_ANGLE_THRESHOLD) {
@ -1065,9 +1075,12 @@ void CharacterBody2D::move_and_slide() {
if (stop_on_slope) {
// move and collide may stray the object a bit because of pre un-stucking,
// so only ensure that motion happens on floor direction in this case.
result.motion = up_direction * up_direction.dot(result.motion);
if (result.motion.length() > margin) {
result.motion = up_direction * up_direction.dot(result.motion);
} else {
result.motion = Vector2();
}
}
} else {
apply = false;
}

42
scene/3d/physics_body_3d.cpp
View File

@ -125,11 +125,11 @@ bool PhysicsBody3D::move_and_collide(const Vector3 &p_motion, bool p_infinite_in
// Restore direction of motion to be along original motion,
// in order to avoid sliding due to recovery,
// but only if collision depth is low enough to avoid tunneling.
real_t motion_length = p_motion.length();
if (motion_length > CMP_EPSILON) {
real_t precision = CMP_EPSILON;
if (p_cancel_sliding) {
real_t motion_length = p_motion.length();
real_t precision = 0.001;
if (colliding && p_cancel_sliding) {
if (colliding) {
// Can't just use margin as a threshold because collision depth is calculated on unsafe motion,
// so even in normal resting cases the depth can be a bit more than the margin.
precision += motion_length * (r_result.collision_unsafe_fraction - r_result.collision_safe_fraction);
@ -140,16 +140,21 @@ bool PhysicsBody3D::move_and_collide(const Vector3 &p_motion, bool p_infinite_in
}
if (p_cancel_sliding) {
// When motion is null, recovery is the resulting motion.
Vector3 motion_normal;
if (motion_length > CMP_EPSILON) {
motion_normal = p_motion / motion_length;
}
// Check depth of recovery.
Vector3 motion_normal = p_motion / motion_length;
real_t dot = r_result.motion.dot(motion_normal);
Vector3 recovery = r_result.motion - motion_normal * dot;
real_t projected_length = r_result.motion.dot(motion_normal);
Vector3 recovery = r_result.motion - motion_normal * projected_length;
real_t recovery_length = recovery.length();
// Fixes cases where canceling slide causes the motion to go too deep into the ground,
// Becauses we're only taking rest information into account and not general recovery.
// because we're only taking rest information into account and not general recovery.
if (recovery_length < (real_t)p_margin + precision) {
// Apply adjustment to motion.
r_result.motion = motion_normal * dot;
r_result.motion = motion_normal * projected_length;
r_result.remainder = p_motion - r_result.motion;
}
}
@ -1012,8 +1017,9 @@ void CharacterBody3D::move_and_slide() {
floor_normal = Vector3();
floor_velocity = Vector3();
// No sliding on first attempt to keep motion stable when possible.
bool sliding_enabled = false;
// No sliding on first attempt to keep floor motion stable when possible,
// when stop on slope is enabled.
bool sliding_enabled = !stop_on_slope;
for (int iteration = 0; iteration < max_slides; ++iteration) {
PhysicsServer3D::MotionResult result;
bool found_collision = false;
@ -1052,7 +1058,11 @@ void CharacterBody3D::move_and_slide() {
if (stop_on_slope) {
if ((body_velocity_normal + up_direction).length() < 0.01) {
Transform3D gt = get_global_transform();
gt.origin -= result.motion.slide(up_direction);
if (result.motion.length() > margin) {
gt.origin -= result.motion.slide(up_direction);
} else {
gt.origin -= result.motion;
}
set_global_transform(gt);
linear_velocity = Vector3();
return;
@ -1094,7 +1104,7 @@ void CharacterBody3D::move_and_slide() {
// Apply snap.
Transform3D gt = get_global_transform();
PhysicsServer3D::MotionResult result;
if (move_and_collide(snap, infinite_inertia, result, margin, false, true)) {
if (move_and_collide(snap, infinite_inertia, result, margin, false, true, false)) {
bool apply = true;
if (up_direction != Vector3()) {
if (Math::acos(result.collision_normal.dot(up_direction)) <= floor_max_angle + FLOOR_ANGLE_THRESHOLD) {
@ -1105,7 +1115,11 @@ void CharacterBody3D::move_and_slide() {
if (stop_on_slope) {
// move and collide may stray the object a bit because of pre un-stucking,
// so only ensure that motion happens on floor direction in this case.
result.motion = result.motion.project(up_direction);
if (result.motion.length() > margin) {
result.motion = result.motion.project(up_direction);
} else {
result.motion = Vector3();
}
}
} else {
apply = false; //snapped with floor direction, but did not snap to a floor, do not snap.

63
servers/physics_2d/space_2d_sw.cpp
View File

@ -283,22 +283,38 @@ bool PhysicsDirectSpaceState2DSW::cast_motion(const RID &p_shape, const Transfor
continue;
}
//just do kinematic solving
real_t low = 0;
real_t hi = 1;
Vector2 mnormal = p_motion.normalized();
//just do kinematic solving
real_t low = 0.0;
real_t hi = 1.0;
real_t fraction_coeff = 0.5;
for (int j = 0; j < 8; j++) { //steps should be customizable..
real_t ofs = (low + hi) * 0.5;
real_t fraction = low + (hi - low) * fraction_coeff;
Vector2 sep = mnormal; //important optimization for this to work fast enough
bool collided = CollisionSolver2DSW::solve(shape, p_xform, p_motion * ofs, col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), nullptr, nullptr, &sep, p_margin);
bool collided = CollisionSolver2DSW::solve(shape, p_xform, p_motion * fraction, col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), nullptr, nullptr, &sep, p_margin);
if (collided) {
hi = ofs;
hi = fraction;
if ((j == 0) || (low > 0.0)) { // Did it not collide before?
// When alternating or first iteration, use dichotomy.
fraction_coeff = 0.5;
} else {
// When colliding again, converge faster towards low fraction
// for more accurate results with long motions that collide near the start.
fraction_coeff = 0.25;
}
} else {
low = ofs;
low = fraction;
if ((j == 0) || (hi < 1.0)) { // Did it collide before?
// When alternating or first iteration, use dichotomy.
fraction_coeff = 0.5;
} else {
// When not colliding again, converge faster towards high fraction
// for more accurate results with long motions that collide near the end.
fraction_coeff = 0.75;
}
}
}
@ -957,20 +973,35 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
}
//just do kinematic solving
real_t low = 0;
real_t hi = 1;
real_t low = 0.0;
real_t hi = 1.0;
real_t fraction_coeff = 0.5;
for (int k = 0; k < 8; k++) { //steps should be customizable..
real_t ofs = (low + hi) * 0.5;
real_t fraction = low + (hi - low) * fraction_coeff;
Vector2 sep = motion_normal; //important optimization for this to work fast enough
bool collided = CollisionSolver2DSW::solve(body_shape, body_shape_xform, p_motion * ofs, against_shape, col_obj_shape_xform, Vector2(), nullptr, nullptr, &sep, 0);
bool collided = CollisionSolver2DSW::solve(body_shape, body_shape_xform, p_motion * fraction, against_shape, col_obj_shape_xform, Vector2(), nullptr, nullptr, &sep, 0);
if (collided) {
hi = ofs;
hi = fraction;
if ((k == 0) || (low > 0.0)) { // Did it not collide before?
// When alternating or first iteration, use dichotomy.
fraction_coeff = 0.5;
} else {
// When colliding again, converge faster towards low fraction
// for more accurate results with long motions that collide near the start.
fraction_coeff = 0.25;
}
} else {
low = ofs;
low = fraction;
if ((k == 0) || (hi < 1.0)) { // Did it collide before?
// When alternating or first iteration, use dichotomy.
fraction_coeff = 0.5;
} else {
// When not colliding again, converge faster towards high fraction
// for more accurate results with long motions that collide near the end.
fraction_coeff = 0.75;
}
}
}

77
servers/physics_3d/space_3d_sw.cpp
View File

@ -255,6 +255,8 @@ bool PhysicsDirectSpaceState3DSW::cast_motion(const RID &p_shape, const Transfor
bool best_first = true;
Vector3 motion_normal = p_motion.normalized();
Vector3 closest_A, closest_B;
for (int i = 0; i < amount; i++) {
@ -270,7 +272,7 @@ bool PhysicsDirectSpaceState3DSW::cast_motion(const RID &p_shape, const Transfor
int shape_idx = space->intersection_query_subindex_results[i];
Vector3 point_A, point_B;
Vector3 sep_axis = p_motion.normalized();
Vector3 sep_axis = motion_normal;
Transform3D col_obj_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx);
//test initial overlap, does it collide if going all the way?
@ -279,35 +281,47 @@ bool PhysicsDirectSpaceState3DSW::cast_motion(const RID &p_shape, const Transfor
}
//test initial overlap, ignore objects it's inside of.
sep_axis = p_motion.normalized();
sep_axis = motion_normal;
if (!CollisionSolver3DSW::solve_distance(shape, p_xform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, aabb, &sep_axis)) {
continue;
}
//just do kinematic solving
real_t low = 0;
real_t hi = 1;
Vector3 mnormal = p_motion.normalized();
real_t low = 0.0;
real_t hi = 1.0;
real_t fraction_coeff = 0.5;
for (int j = 0; j < 8; j++) { //steps should be customizable..
real_t fraction = low + (hi - low) * fraction_coeff;
real_t ofs = (low + hi) * 0.5;
Vector3 sep = mnormal; //important optimization for this to work fast enough
mshape.motion = xform_inv.basis.xform(p_motion * ofs);
mshape.motion = xform_inv.basis.xform(p_motion * fraction);
Vector3 lA, lB;
Vector3 sep = motion_normal; //important optimization for this to work fast enough
bool collided = !CollisionSolver3DSW::solve_distance(&mshape, p_xform, col_obj->get_shape(shape_idx), col_obj_xform, lA, lB, aabb, &sep);
if (collided) {
hi = ofs;
hi = fraction;
if ((j == 0) || (low > 0.0)) { // Did it not collide before?
// When alternating or first iteration, use dichotomy.
fraction_coeff = 0.5;
} else {
// When colliding again, converge faster towards low fraction
// for more accurate results with long motions that collide near the start.
fraction_coeff = 0.25;
}
} else {
point_A = lA;
point_B = lB;
low = ofs;
low = fraction;
if ((j == 0) || (hi < 1.0)) { // Did it collide before?
// When alternating or first iteration, use dichotomy.
fraction_coeff = 0.5;
} else {
// When not colliding again, converge faster towards high fraction
// for more accurate results with long motions that collide near the end.
fraction_coeff = 0.75;
}
}
}
@ -902,27 +916,40 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform3D &p_from, co
}
//just do kinematic solving
real_t low = 0;
real_t hi = 1;
real_t low = 0.0;
real_t hi = 1.0;
real_t fraction_coeff = 0.5;
for (int k = 0; k < 8; k++) { //steps should be customizable..
real_t fraction = low + (hi - low) * fraction_coeff;
real_t ofs = (low + hi) * 0.5;
Vector3 sep = motion_normal; //important optimization for this to work fast enough
mshape.motion = body_shape_xform_inv.basis.xform(p_motion * ofs);
mshape.motion = body_shape_xform_inv.basis.xform(p_motion * fraction);
Vector3 lA, lB;
Vector3 sep = motion_normal; //important optimization for this to work fast enough
bool collided = !CollisionSolver3DSW::solve_distance(&mshape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj_xform, lA, lB, motion_aabb, &sep);
if (collided) {
hi = ofs;
hi = fraction;
if ((k == 0) || (low > 0.0)) { // Did it not collide before?
// When alternating or first iteration, use dichotomy.
fraction_coeff = 0.5;
} else {
// When colliding again, converge faster towards low fraction
// for more accurate results with long motions that collide near the start.
fraction_coeff = 0.25;
}
} else {
point_A = lA;
point_B = lB;
low = ofs;
low = fraction;
if ((k == 0) || (hi < 1.0)) { // Did it collide before?
// When alternating or first iteration, use dichotomy.
fraction_coeff = 0.5;
} else {
// When not colliding again, converge faster towards high fraction
// for more accurate results with long motions that collide near the end.
fraction_coeff = 0.75;
}
}
}