diff --git a/scene/2d/physics_body_2d.cpp b/scene/2d/physics_body_2d.cpp index c7889e2bd2e..e0227c32e23 100644 --- a/scene/2d/physics_body_2d.cpp +++ b/scene/2d/physics_body_2d.cpp @@ -1042,11 +1042,11 @@ bool KinematicBody2D::move_and_collide(const Vector2 &p_motion, bool p_infinite_ // 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 * (result.collision_unsafe_fraction - result.collision_safe_fraction); @@ -1057,16 +1057,21 @@ bool KinematicBody2D::move_and_collide(const Vector2 &p_motion, bool p_infinite_ } 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 = result.motion.dot(motion_normal); - Vector2 recovery = result.motion - motion_normal * dot; + real_t projected_length = result.motion.dot(motion_normal); + Vector2 recovery = 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)margin + precision) { // Apply adjustment to motion. - result.motion = motion_normal * dot; + result.motion = motion_normal * projected_length; result.remainder = p_motion - result.motion; } } @@ -1134,8 +1139,9 @@ Vector2 KinematicBody2D::move_and_slide(const Vector2 &p_linear_velocity, const on_floor_body = RID(); Vector2 motion = body_velocity * delta; - // 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 = !p_stop_on_slope; for (int iteration = 0; iteration < p_max_slides; ++iteration) { Collision collision; bool found_collision = false; @@ -1165,7 +1171,11 @@ Vector2 KinematicBody2D::move_and_slide(const Vector2 &p_linear_velocity, const if (on_floor && p_stop_on_slope) { if ((body_velocity_normal + up_direction).length() < 0.01) { Transform2D gt = get_global_transform(); - gt.elements[2] -= collision.travel.slide(up_direction); + if (collision.travel.length() > margin) { + gt.elements[2] -= collision.travel.slide(up_direction); + } else { + gt.elements[2] -= collision.travel; + } set_global_transform(gt); return Vector2(); } @@ -1207,7 +1217,7 @@ Vector2 KinematicBody2D::move_and_slide_with_snap(const Vector2 &p_linear_veloci Collision col; Transform2D gt = get_global_transform(); - if (move_and_collide(p_snap, p_infinite_inertia, col, false, true)) { + if (move_and_collide(p_snap, p_infinite_inertia, col, false, true, false)) { bool apply = true; if (up_direction != Vector2()) { if (Math::acos(col.normal.dot(up_direction)) <= p_floor_max_angle + FLOOR_ANGLE_THRESHOLD) { @@ -1218,9 +1228,12 @@ Vector2 KinematicBody2D::move_and_slide_with_snap(const Vector2 &p_linear_veloci if (p_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. - col.travel = up_direction * up_direction.dot(col.travel); + if (col.travel.length() > margin) { + col.travel = up_direction * up_direction.dot(col.travel); + } else { + col.travel = Vector2(); + } } - } else { apply = false; } diff --git a/scene/3d/physics_body.cpp b/scene/3d/physics_body.cpp index 70c84512f9f..c6edaac2aaf 100644 --- a/scene/3d/physics_body.cpp +++ b/scene/3d/physics_body.cpp @@ -985,11 +985,11 @@ bool KinematicBody::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 * (result.collision_unsafe_fraction - result.collision_safe_fraction); @@ -1000,16 +1000,21 @@ bool KinematicBody::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 = result.motion.dot(motion_normal); - Vector3 recovery = result.motion - motion_normal * dot; + real_t projected_length = result.motion.dot(motion_normal); + Vector3 recovery = 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)margin + precision) { // Apply adjustment to motion. - result.motion = motion_normal * dot; + result.motion = motion_normal * projected_length; result.remainder = p_motion - result.motion; } } @@ -1076,8 +1081,9 @@ Vector3 KinematicBody::move_and_slide(const Vector3 &p_linear_velocity, const Ve 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 = !p_stop_on_slope; for (int iteration = 0; iteration < p_max_slides; ++iteration) { Collision collision; bool found_collision = false; @@ -1116,7 +1122,11 @@ Vector3 KinematicBody::move_and_slide(const Vector3 &p_linear_velocity, const Ve if (p_stop_on_slope) { if ((body_velocity_normal + up_direction).length() < 0.01) { Transform gt = get_global_transform(); - gt.origin -= collision.travel.slide(up_direction); + if (collision.travel.length() > margin) { + gt.origin -= collision.travel.slide(up_direction); + } else { + gt.origin -= collision.travel; + } set_global_transform(gt); return Vector3(); } @@ -1165,7 +1175,7 @@ Vector3 KinematicBody::move_and_slide_with_snap(const Vector3 &p_linear_velocity Collision col; Transform gt = get_global_transform(); - if (move_and_collide(p_snap, p_infinite_inertia, col, false, true)) { + if (move_and_collide(p_snap, p_infinite_inertia, col, false, true, false)) { bool apply = true; if (up_direction != Vector3()) { if (Math::acos(col.normal.dot(up_direction)) <= p_floor_max_angle + FLOOR_ANGLE_THRESHOLD) { @@ -1176,7 +1186,11 @@ Vector3 KinematicBody::move_and_slide_with_snap(const Vector3 &p_linear_velocity if (p_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. - col.travel = col.travel.project(up_direction); + if (col.travel.length() > margin) { + col.travel = col.travel.project(up_direction); + } else { + col.travel = Vector3(); + } } } else { apply = false; //snapped with floor direction, but did not snap to a floor, do not snap. diff --git a/servers/physics/space_sw.cpp b/servers/physics/space_sw.cpp index 075a5f4d880..bc57f342835 100644 --- a/servers/physics/space_sw.cpp +++ b/servers/physics/space_sw.cpp @@ -251,6 +251,8 @@ bool PhysicsDirectSpaceStateSW::cast_motion(const RID &p_shape, const Transform bool best_first = true; + Vector3 motion_normal = p_motion.normalized(); + Vector3 closest_A, closest_B; for (int i = 0; i < amount; i++) { @@ -266,7 +268,7 @@ bool PhysicsDirectSpaceStateSW::cast_motion(const RID &p_shape, const Transform 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; Transform 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? @@ -275,35 +277,47 @@ bool PhysicsDirectSpaceStateSW::cast_motion(const RID &p_shape, const Transform } //test initial overlap, ignore objects it's inside of. - sep_axis = p_motion.normalized(); + sep_axis = motion_normal; if (!CollisionSolverSW::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 = !CollisionSolverSW::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; + } } } @@ -894,27 +908,40 @@ bool SpaceSW::test_body_motion(BodySW *p_body, const Transform &p_from, const Ve } //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 = !CollisionSolverSW::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; + } } } diff --git a/servers/physics_2d/space_2d_sw.cpp b/servers/physics_2d/space_2d_sw.cpp index 8efbf41e8fc..8756477350c 100644 --- a/servers/physics_2d/space_2d_sw.cpp +++ b/servers/physics_2d/space_2d_sw.cpp @@ -283,22 +283,38 @@ bool Physics2DDirectSpaceStateSW::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; + } } } @@ -962,20 +978,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; + } } }