Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Make move_and_slide collision detection more accurate #50063

Merged
merged 1 commit into from
Jul 13, 2021
Merged

Make move_and_slide collision detection more accurate #50063

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
41 changes: 27 additions & 14 deletions scene/2d/physics_body_2d.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand All @@ -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;
}
}
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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) {
Expand All @@ -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;
}
Expand Down
42 changes: 28 additions & 14 deletions scene/3d/physics_body_3d.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand All @@ -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;
}
}
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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) {
Expand All @@ -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.
Expand Down
63 changes: 47 additions & 16 deletions servers/physics_2d/space_2d_sw.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
}
}

Expand Down Expand Up @@ -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;
}
}
}

Expand Down
Loading