diff --git a/scene/2d/physics_body_2d.cpp b/scene/2d/physics_body_2d.cpp index 3fd0bef7e94..90663bd6759 100644 --- a/scene/2d/physics_body_2d.cpp +++ b/scene/2d/physics_body_2d.cpp @@ -1212,7 +1212,7 @@ bool KinematicBody2D::move_and_collide(const Vector2 &p_motion, bool p_infinite_ //so, if you pass 45 as limit, avoid numerical precision errors when angle is 45. #define FLOOR_ANGLE_THRESHOLD 0.01 -Vector2 KinematicBody2D::move_and_slide(const Vector2 &p_linear_velocity, const Vector2 &p_floor_direction, bool p_stop_on_slope, int p_max_slides, float p_floor_max_angle, bool p_infinite_inertia) { +Vector2 KinematicBody2D::move_and_slide(const Vector2 &p_linear_velocity, const Vector2 &p_up_direction, bool p_stop_on_slope, int p_max_slides, float p_floor_max_angle, bool p_infinite_inertia) { Vector2 body_velocity = p_linear_velocity; Vector2 body_velocity_normal = body_velocity.normalized(); @@ -1234,6 +1234,7 @@ Vector2 KinematicBody2D::move_and_slide(const Vector2 &p_linear_velocity, const on_ceiling = false; on_wall = false; colliders.clear(); + floor_normal = p_up_direction; floor_velocity = Vector2(); while (p_max_slides) { @@ -1262,25 +1263,26 @@ Vector2 KinematicBody2D::move_and_slide(const Vector2 &p_linear_velocity, const colliders.push_back(collision); motion = collision.remainder; - if (p_floor_direction == Vector2()) { + if (p_up_direction == Vector2()) { //all is a wall on_wall = true; } else { - if (Math::acos(collision.normal.dot(p_floor_direction)) <= p_floor_max_angle + FLOOR_ANGLE_THRESHOLD) { //floor + if (Math::acos(collision.normal.dot(p_up_direction)) <= p_floor_max_angle + FLOOR_ANGLE_THRESHOLD) { //floor on_floor = true; + floor_normal = collision.normal; on_floor_body = collision.collider_rid; floor_velocity = collision.collider_vel; if (p_stop_on_slope) { - if ((body_velocity_normal + p_floor_direction).length() < 0.01 && collision.travel.length() < 1) { + if ((body_velocity_normal + p_up_direction).length() < 0.01 && collision.travel.length() < 1) { Transform2D gt = get_global_transform(); - gt.elements[2] -= collision.travel.slide(p_floor_direction); + gt.elements[2] -= collision.travel.slide(p_up_direction); set_global_transform(gt); return Vector2(); } } - } else if (Math::acos(collision.normal.dot(-p_floor_direction)) <= p_floor_max_angle + FLOOR_ANGLE_THRESHOLD) { //ceiling + } else if (Math::acos(collision.normal.dot(-p_up_direction)) <= p_floor_max_angle + FLOOR_ANGLE_THRESHOLD) { //ceiling on_ceiling = true; } else { on_wall = true; @@ -1301,11 +1303,11 @@ Vector2 KinematicBody2D::move_and_slide(const Vector2 &p_linear_velocity, const return body_velocity; } -Vector2 KinematicBody2D::move_and_slide_with_snap(const Vector2 &p_linear_velocity, const Vector2 &p_snap, const Vector2 &p_floor_direction, bool p_stop_on_slope, int p_max_slides, float p_floor_max_angle, bool p_infinite_inertia) { +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) { bool was_on_floor = on_floor; - Vector2 ret = move_and_slide(p_linear_velocity, p_floor_direction, p_stop_on_slope, p_max_slides, p_floor_max_angle, p_infinite_inertia); + 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); if (!was_on_floor || p_snap == Vector2()) { return ret; } @@ -1315,15 +1317,16 @@ 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_floor_direction != Vector2()) { - if (Math::acos(p_floor_direction.normalized().dot(col.normal)) < p_floor_max_angle) { + if (p_up_direction != Vector2()) { + if (Math::acos(p_up_direction.normalized().dot(col.normal)) < p_floor_max_angle) { on_floor = true; + floor_normal = col.normal; on_floor_body = col.collider_rid; floor_velocity = col.collider_vel; 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_floor_direction * p_floor_direction.dot(col.travel); + col.travel = p_up_direction * p_up_direction.dot(col.travel); } } else { @@ -1353,6 +1356,11 @@ bool KinematicBody2D::is_on_ceiling() const { return on_ceiling; } +Vector2 KinematicBody2D::get_floor_normal() const { + + return floor_normal; +} + Vector2 KinematicBody2D::get_floor_velocity() const { return floor_velocity; @@ -1473,6 +1481,7 @@ void KinematicBody2D::_bind_methods() { ClassDB::bind_method(D_METHOD("is_on_floor"), &KinematicBody2D::is_on_floor); ClassDB::bind_method(D_METHOD("is_on_ceiling"), &KinematicBody2D::is_on_ceiling); ClassDB::bind_method(D_METHOD("is_on_wall"), &KinematicBody2D::is_on_wall); + ClassDB::bind_method(D_METHOD("get_floor_normal"), &KinematicBody2D::get_floor_normal); ClassDB::bind_method(D_METHOD("get_floor_velocity"), &KinematicBody2D::get_floor_velocity); ClassDB::bind_method(D_METHOD("set_safe_margin", "pixels"), &KinematicBody2D::set_safe_margin); diff --git a/scene/2d/physics_body_2d.h b/scene/2d/physics_body_2d.h index 7f2b42b554c..6766bafde3f 100644 --- a/scene/2d/physics_body_2d.h +++ b/scene/2d/physics_body_2d.h @@ -306,6 +306,7 @@ public: private: float margin; + Vector2 floor_normal; Vector2 floor_velocity; RID on_floor_body; bool on_floor; @@ -339,11 +340,12 @@ public: void set_safe_margin(float p_margin); float get_safe_margin() const; - Vector2 move_and_slide(const Vector2 &p_linear_velocity, const Vector2 &p_floor_direction = Vector2(0, 0), bool p_stop_on_slope = false, int p_max_slides = 4, float p_floor_max_angle = Math::deg2rad((float)45), bool p_infinite_inertia = true); - Vector2 move_and_slide_with_snap(const Vector2 &p_linear_velocity, const Vector2 &p_snap, const Vector2 &p_floor_direction = Vector2(0, 0), bool p_stop_on_slope = false, int p_max_slides = 4, float p_floor_max_angle = Math::deg2rad((float)45), bool p_infinite_inertia = true); + Vector2 move_and_slide(const Vector2 &p_linear_velocity, const Vector2 &p_up_direction = Vector2(0, 0), bool p_stop_on_slope = false, int p_max_slides = 4, float p_floor_max_angle = Math::deg2rad((float)45), bool p_infinite_inertia = true); + Vector2 move_and_slide_with_snap(const Vector2 &p_linear_velocity, const Vector2 &p_snap, const Vector2 &p_up_direction = Vector2(0, 0), bool p_stop_on_slope = false, int p_max_slides = 4, float p_floor_max_angle = Math::deg2rad((float)45), bool p_infinite_inertia = true); bool is_on_floor() const; bool is_on_wall() const; bool is_on_ceiling() const; + Vector2 get_floor_normal() const; Vector2 get_floor_velocity() const; int get_slide_count() const; diff --git a/scene/3d/physics_body.cpp b/scene/3d/physics_body.cpp index c2860c25d8f..999f39c8411 100644 --- a/scene/3d/physics_body.cpp +++ b/scene/3d/physics_body.cpp @@ -1140,7 +1140,7 @@ bool KinematicBody::move_and_collide(const Vector3 &p_motion, bool p_infinite_in //so, if you pass 45 as limit, avoid numerical precision errors when angle is 45. #define FLOOR_ANGLE_THRESHOLD 0.01 -Vector3 KinematicBody::move_and_slide(const Vector3 &p_linear_velocity, const Vector3 &p_floor_direction, bool p_stop_on_slope, int p_max_slides, float p_floor_max_angle, bool p_infinite_inertia) { +Vector3 KinematicBody::move_and_slide(const Vector3 &p_linear_velocity, const Vector3 &p_up_direction, bool p_stop_on_slope, int p_max_slides, float p_floor_max_angle, bool p_infinite_inertia) { Vector3 body_velocity = p_linear_velocity; Vector3 body_velocity_normal = body_velocity.normalized(); @@ -1159,6 +1159,7 @@ Vector3 KinematicBody::move_and_slide(const Vector3 &p_linear_velocity, const Ve on_ceiling = false; on_wall = false; colliders.clear(); + floor_normal = p_up_direction; floor_velocity = Vector3(); while (p_max_slides) { @@ -1187,25 +1188,26 @@ Vector3 KinematicBody::move_and_slide(const Vector3 &p_linear_velocity, const Ve colliders.push_back(collision); motion = collision.remainder; - if (p_floor_direction == Vector3()) { + if (p_up_direction == Vector3()) { //all is a wall on_wall = true; } else { - if (Math::acos(collision.normal.dot(p_floor_direction)) <= p_floor_max_angle + FLOOR_ANGLE_THRESHOLD) { //floor + if (Math::acos(collision.normal.dot(p_up_direction)) <= p_floor_max_angle + FLOOR_ANGLE_THRESHOLD) { //floor on_floor = true; + floor_normal = collision.normal; on_floor_body = collision.collider_rid; floor_velocity = collision.collider_vel; if (p_stop_on_slope) { - if ((body_velocity_normal + p_floor_direction).length() < 0.01 && collision.travel.length() < 1) { + if ((body_velocity_normal + p_up_direction).length() < 0.01 && collision.travel.length() < 1) { Transform gt = get_global_transform(); - gt.origin -= collision.travel.slide(p_floor_direction); + gt.origin -= collision.travel.slide(p_up_direction); set_global_transform(gt); return Vector3(); } } - } else if (Math::acos(collision.normal.dot(-p_floor_direction)) <= p_floor_max_angle + FLOOR_ANGLE_THRESHOLD) { //ceiling + } else if (Math::acos(collision.normal.dot(-p_up_direction)) <= p_floor_max_angle + FLOOR_ANGLE_THRESHOLD) { //ceiling on_ceiling = true; } else { on_wall = true; @@ -1232,11 +1234,11 @@ Vector3 KinematicBody::move_and_slide(const Vector3 &p_linear_velocity, const Ve return body_velocity; } -Vector3 KinematicBody::move_and_slide_with_snap(const Vector3 &p_linear_velocity, const Vector3 &p_snap, const Vector3 &p_floor_direction, bool p_stop_on_slope, int p_max_slides, float p_floor_max_angle, bool p_infinite_inertia) { +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) { bool was_on_floor = on_floor; - Vector3 ret = move_and_slide(p_linear_velocity, p_floor_direction, p_stop_on_slope, p_max_slides, p_floor_max_angle, p_infinite_inertia); + 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); if (!was_on_floor || p_snap == Vector3()) { return ret; } @@ -1247,15 +1249,16 @@ 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_floor_direction != Vector3()) { - if (Math::acos(p_floor_direction.normalized().dot(col.normal)) < p_floor_max_angle) { + if (p_up_direction != Vector3()) { + if (Math::acos(p_up_direction.normalized().dot(col.normal)) < p_floor_max_angle) { on_floor = true; + floor_normal = col.normal; on_floor_body = col.collider_rid; floor_velocity = col.collider_vel; 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_floor_direction); + col.travel = col.travel.project(p_up_direction); } } else { apply = false; //snapped with floor direction, but did not snap to a floor, do not snap. @@ -1284,6 +1287,11 @@ bool KinematicBody::is_on_ceiling() const { return on_ceiling; } +Vector3 KinematicBody::get_floor_normal() const { + + return floor_normal; +} + Vector3 KinematicBody::get_floor_velocity() const { return floor_velocity; @@ -1392,7 +1400,7 @@ void KinematicBody::_notification(int p_what) { void KinematicBody::_bind_methods() { ClassDB::bind_method(D_METHOD("move_and_collide", "rel_vec", "infinite_inertia", "exclude_raycast_shapes", "test_only"), &KinematicBody::_move, DEFVAL(true), DEFVAL(true), DEFVAL(false)); - ClassDB::bind_method(D_METHOD("move_and_slide", "linear_velocity", "floor_normal", "stop_on_slope", "max_slides", "floor_max_angle", "infinite_inertia"), &KinematicBody::move_and_slide, DEFVAL(Vector3(0, 0, 0)), DEFVAL(false), DEFVAL(4), DEFVAL(Math::deg2rad((float)45)), DEFVAL(true)); + ClassDB::bind_method(D_METHOD("move_and_slide", "linear_velocity", "up_direction", "stop_on_slope", "max_slides", "floor_max_angle", "infinite_inertia"), &KinematicBody::move_and_slide, DEFVAL(Vector3(0, 0, 0)), DEFVAL(false), DEFVAL(4), DEFVAL(Math::deg2rad((float)45)), DEFVAL(true)); ClassDB::bind_method(D_METHOD("move_and_slide_with_snap", "linear_velocity", "snap", "floor_normal", "stop_on_slope", "max_slides", "floor_max_angle", "infinite_inertia"), &KinematicBody::move_and_slide_with_snap, DEFVAL(Vector3(0, 0, 0)), DEFVAL(false), DEFVAL(4), DEFVAL(Math::deg2rad((float)45)), DEFVAL(true)); ClassDB::bind_method(D_METHOD("test_move", "from", "rel_vec", "infinite_inertia"), &KinematicBody::test_move, DEFVAL(true)); @@ -1400,6 +1408,7 @@ void KinematicBody::_bind_methods() { ClassDB::bind_method(D_METHOD("is_on_floor"), &KinematicBody::is_on_floor); ClassDB::bind_method(D_METHOD("is_on_ceiling"), &KinematicBody::is_on_ceiling); ClassDB::bind_method(D_METHOD("is_on_wall"), &KinematicBody::is_on_wall); + ClassDB::bind_method(D_METHOD("get_floor_normal"), &KinematicBody::get_floor_normal); ClassDB::bind_method(D_METHOD("get_floor_velocity"), &KinematicBody::get_floor_velocity); ClassDB::bind_method(D_METHOD("set_axis_lock", "axis", "lock"), &KinematicBody::set_axis_lock); diff --git a/scene/3d/physics_body.h b/scene/3d/physics_body.h index 06b31804b2d..05bcbe22f00 100644 --- a/scene/3d/physics_body.h +++ b/scene/3d/physics_body.h @@ -300,6 +300,7 @@ private: float margin; + Vector3 floor_normal; Vector3 floor_velocity; RID on_floor_body; bool on_floor; @@ -330,11 +331,12 @@ public: void set_safe_margin(float p_margin); float get_safe_margin() const; - Vector3 move_and_slide(const Vector3 &p_linear_velocity, const Vector3 &p_floor_direction = Vector3(0, 0, 0), bool p_stop_on_slope = false, int p_max_slides = 4, float p_floor_max_angle = Math::deg2rad((float)45), bool p_infinite_inertia = true); - Vector3 move_and_slide_with_snap(const Vector3 &p_linear_velocity, const Vector3 &p_snap, const Vector3 &p_floor_direction = Vector3(0, 0, 0), bool p_stop_on_slope = false, int p_max_slides = 4, float p_floor_max_angle = Math::deg2rad((float)45), bool p_infinite_inertia = true); + Vector3 move_and_slide(const Vector3 &p_linear_velocity, const Vector3 &p_up_direction = Vector3(0, 0, 0), bool p_stop_on_slope = false, int p_max_slides = 4, float p_floor_max_angle = Math::deg2rad((float)45), bool p_infinite_inertia = true); + Vector3 move_and_slide_with_snap(const Vector3 &p_linear_velocity, const Vector3 &p_snap, const Vector3 &p_up_direction = Vector3(0, 0, 0), bool p_stop_on_slope = false, int p_max_slides = 4, float p_floor_max_angle = Math::deg2rad((float)45), bool p_infinite_inertia = true); bool is_on_floor() const; bool is_on_wall() const; bool is_on_ceiling() const; + Vector3 get_floor_normal() const; Vector3 get_floor_velocity() const; int get_slide_count() const;