From ea4ee986b5916c07c1d13c798b26cb9cb22c6316 Mon Sep 17 00:00:00 2001 From: Marcel Admiraal Date: Tue, 16 Jun 2020 10:11:54 +0100 Subject: [PATCH] Normalise p_up_direction vector in move_and_slide() and move_and_slide_with_snap() and fix tolerance in move_and_slide_with_snap() max floor angle. --- scene/2d/physics_body_2d.cpp | 20 +++++++++++--------- scene/3d/physics_body.cpp | 20 +++++++++++--------- 2 files changed, 22 insertions(+), 18 deletions(-) diff --git a/scene/2d/physics_body_2d.cpp b/scene/2d/physics_body_2d.cpp index d42bd6adaf1..2bf6dbdd27b 100644 --- a/scene/2d/physics_body_2d.cpp +++ b/scene/2d/physics_body_2d.cpp @@ -1216,6 +1216,7 @@ Vector2 KinematicBody2D::move_and_slide(const Vector2 &p_linear_velocity, const Vector2 body_velocity = p_linear_velocity; Vector2 body_velocity_normal = body_velocity.normalized(); + Vector2 up_direction = p_up_direction.normalized(); Vector2 current_floor_velocity = floor_velocity; if (on_floor && on_floor_body.is_valid()) { @@ -1263,11 +1264,11 @@ Vector2 KinematicBody2D::move_and_slide(const Vector2 &p_linear_velocity, const colliders.push_back(collision); motion = collision.remainder; - if (p_up_direction == Vector2()) { + if (up_direction == Vector2()) { //all is a wall on_wall = true; } else { - if (Math::acos(collision.normal.dot(p_up_direction)) <= p_floor_max_angle + FLOOR_ANGLE_THRESHOLD) { //floor + if (Math::acos(collision.normal.dot(up_direction)) <= p_floor_max_angle + FLOOR_ANGLE_THRESHOLD) { //floor on_floor = true; floor_normal = collision.normal; @@ -1275,14 +1276,14 @@ Vector2 KinematicBody2D::move_and_slide(const Vector2 &p_linear_velocity, const floor_velocity = collision.collider_vel; if (p_stop_on_slope) { - if ((body_velocity_normal + p_up_direction).length() < 0.01 && collision.travel.length() < 1) { + if ((body_velocity_normal + up_direction).length() < 0.01 && collision.travel.length() < 1) { Transform2D gt = get_global_transform(); - gt.elements[2] -= collision.travel.slide(p_up_direction); + gt.elements[2] -= collision.travel.slide(up_direction); set_global_transform(gt); return Vector2(); } } - } else if (Math::acos(collision.normal.dot(-p_up_direction)) <= p_floor_max_angle + FLOOR_ANGLE_THRESHOLD) { //ceiling + } else if (Math::acos(collision.normal.dot(-up_direction)) <= p_floor_max_angle + FLOOR_ANGLE_THRESHOLD) { //ceiling on_ceiling = true; } else { on_wall = true; @@ -1305,9 +1306,10 @@ Vector2 KinematicBody2D::move_and_slide(const Vector2 &p_linear_velocity, const Vector2 KinematicBody2D::move_and_slide_with_snap(const Vector2 &p_linear_velocity, const Vector2 &p_snap, const Vector2 &p_up_direction, bool p_stop_on_slope, int p_max_slides, float p_floor_max_angle, bool p_infinite_inertia) { + Vector2 up_direction = p_up_direction.normalized(); bool was_on_floor = on_floor; - Vector2 ret = move_and_slide(p_linear_velocity, p_up_direction, p_stop_on_slope, p_max_slides, p_floor_max_angle, p_infinite_inertia); + Vector2 ret = move_and_slide(p_linear_velocity, up_direction, p_stop_on_slope, p_max_slides, p_floor_max_angle, p_infinite_inertia); if (!was_on_floor || p_snap == Vector2()) { return ret; } @@ -1317,8 +1319,8 @@ Vector2 KinematicBody2D::move_and_slide_with_snap(const Vector2 &p_linear_veloci if (move_and_collide(p_snap, p_infinite_inertia, col, false, true)) { bool apply = true; - if (p_up_direction != Vector2()) { - if (Math::acos(p_up_direction.normalized().dot(col.normal)) < p_floor_max_angle) { + if (up_direction != Vector2()) { + if (Math::acos(col.normal.dot(up_direction)) <= p_floor_max_angle + FLOOR_ANGLE_THRESHOLD) { on_floor = true; floor_normal = col.normal; on_floor_body = col.collider_rid; @@ -1326,7 +1328,7 @@ 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 = p_up_direction * p_up_direction.dot(col.travel); + col.travel = up_direction * up_direction.dot(col.travel); } } else { diff --git a/scene/3d/physics_body.cpp b/scene/3d/physics_body.cpp index 1f95560c7a7..1e9e6672262 100644 --- a/scene/3d/physics_body.cpp +++ b/scene/3d/physics_body.cpp @@ -1145,6 +1145,7 @@ Vector3 KinematicBody::move_and_slide(const Vector3 &p_linear_velocity, const Ve Vector3 body_velocity = p_linear_velocity; Vector3 body_velocity_normal = body_velocity.normalized(); + Vector3 up_direction = p_up_direction.normalized(); for (int i = 0; i < 3; i++) { if (locked_axis & (1 << i)) { @@ -1189,11 +1190,11 @@ Vector3 KinematicBody::move_and_slide(const Vector3 &p_linear_velocity, const Ve colliders.push_back(collision); motion = collision.remainder; - if (p_up_direction == Vector3()) { + if (up_direction == Vector3()) { //all is a wall on_wall = true; } else { - if (Math::acos(collision.normal.dot(p_up_direction)) <= p_floor_max_angle + FLOOR_ANGLE_THRESHOLD) { //floor + if (Math::acos(collision.normal.dot(up_direction)) <= p_floor_max_angle + FLOOR_ANGLE_THRESHOLD) { //floor on_floor = true; floor_normal = collision.normal; @@ -1201,14 +1202,14 @@ Vector3 KinematicBody::move_and_slide(const Vector3 &p_linear_velocity, const Ve floor_velocity = collision.collider_vel; if (p_stop_on_slope) { - if ((body_velocity_normal + p_up_direction).length() < 0.01 && collision.travel.length() < 1) { + if ((body_velocity_normal + up_direction).length() < 0.01 && collision.travel.length() < 1) { Transform gt = get_global_transform(); - gt.origin -= collision.travel.slide(p_up_direction); + gt.origin -= collision.travel.slide(up_direction); set_global_transform(gt); return Vector3(); } } - } else if (Math::acos(collision.normal.dot(-p_up_direction)) <= p_floor_max_angle + FLOOR_ANGLE_THRESHOLD) { //ceiling + } else if (Math::acos(collision.normal.dot(-up_direction)) <= p_floor_max_angle + FLOOR_ANGLE_THRESHOLD) { //ceiling on_ceiling = true; } else { on_wall = true; @@ -1237,9 +1238,10 @@ Vector3 KinematicBody::move_and_slide(const Vector3 &p_linear_velocity, const Ve Vector3 KinematicBody::move_and_slide_with_snap(const Vector3 &p_linear_velocity, const Vector3 &p_snap, const Vector3 &p_up_direction, bool p_stop_on_slope, int p_max_slides, float p_floor_max_angle, bool p_infinite_inertia) { + Vector3 up_direction = p_up_direction.normalized(); bool was_on_floor = on_floor; - Vector3 ret = move_and_slide(p_linear_velocity, p_up_direction, p_stop_on_slope, p_max_slides, p_floor_max_angle, p_infinite_inertia); + Vector3 ret = move_and_slide(p_linear_velocity, up_direction, p_stop_on_slope, p_max_slides, p_floor_max_angle, p_infinite_inertia); if (!was_on_floor || p_snap == Vector3()) { return ret; } @@ -1250,8 +1252,8 @@ Vector3 KinematicBody::move_and_slide_with_snap(const Vector3 &p_linear_velocity if (move_and_collide(p_snap, p_infinite_inertia, col, false, true)) { bool apply = true; - if (p_up_direction != Vector3()) { - if (Math::acos(p_up_direction.normalized().dot(col.normal)) < p_floor_max_angle) { + if (up_direction != Vector3()) { + if (Math::acos(col.normal.dot(up_direction)) <= p_floor_max_angle + FLOOR_ANGLE_THRESHOLD) { on_floor = true; floor_normal = col.normal; on_floor_body = col.collider_rid; @@ -1259,7 +1261,7 @@ 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(p_up_direction); + col.travel = col.travel.project(up_direction); } } else { apply = false; //snapped with floor direction, but did not snap to a floor, do not snap.