diff --git a/scene/2d/physics_body_2d.cpp b/scene/2d/physics_body_2d.cpp index 4c9071e40d8..69fd1438d3f 100644 --- a/scene/2d/physics_body_2d.cpp +++ b/scene/2d/physics_body_2d.cpp @@ -1101,10 +1101,11 @@ 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_up_direction, bool p_stop_on_slope, int p_max_slides, float p_floor_max_angle, bool p_infinite_inertia) { +Vector2 KinematicBody2D::_move_and_slide_internal(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 body_velocity = p_linear_velocity; Vector2 body_velocity_normal = body_velocity.normalized(); Vector2 up_direction = p_up_direction.normalized(); + bool was_on_floor = on_floor; // Hack in order to work with calling from _process as well as from _physics_process; calling from thread is risky float delta = Engine::get_singleton()->is_in_physics_frame() ? get_physics_process_delta_time() : get_process_delta_time(); @@ -1199,6 +1200,40 @@ Vector2 KinematicBody2D::move_and_slide(const Vector2 &p_linear_velocity, const } } + if (was_on_floor && p_snap != Vector2() && !on_floor) { + // Apply snap. + Collision col; + Transform2D gt = get_global_transform(); + + 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) { + 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. + if (col.travel.length() > margin) { + col.travel = up_direction * up_direction.dot(col.travel); + } else { + col.travel = Vector2(); + } + } + } else { + apply = false; + } + } + + if (apply) { + gt.elements[2] += col.travel; + set_global_transform(gt); + } + } + } + if (!on_floor && !on_wall) { // Add last platform velocity when just left a moving platform. return body_velocity + current_floor_velocity; @@ -1207,47 +1242,12 @@ Vector2 KinematicBody2D::move_and_slide(const Vector2 &p_linear_velocity, const return body_velocity; } +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) { + return _move_and_slide_internal(p_linear_velocity, Vector2(), p_up_direction, p_stop_on_slope, p_max_slides, p_floor_max_angle, 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) { - Vector2 up_direction = p_up_direction.normalized(); - bool was_on_floor = on_floor; - - 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() || on_floor) { - return ret; - } - - Collision col; - Transform2D gt = get_global_transform(); - - 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) { - 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. - if (col.travel.length() > margin) { - col.travel = up_direction * up_direction.dot(col.travel); - } else { - col.travel = Vector2(); - } - } - } else { - apply = false; - } - } - - if (apply) { - gt.elements[2] += col.travel; - set_global_transform(gt); - } - } - - return ret; + return _move_and_slide_internal(p_linear_velocity, p_snap, p_up_direction, p_stop_on_slope, p_max_slides, p_floor_max_angle, p_infinite_inertia); } void KinematicBody2D::_set_collision_direction(const Collision &p_collision, const Vector2 &p_up_direction, float p_floor_max_angle) { diff --git a/scene/2d/physics_body_2d.h b/scene/2d/physics_body_2d.h index 2c403bb0ace..a4daefb1e77 100644 --- a/scene/2d/physics_body_2d.h +++ b/scene/2d/physics_body_2d.h @@ -297,13 +297,12 @@ private: Vector> slide_colliders; Ref motion_cache; - _FORCE_INLINE_ bool _ignores_mode(Physics2DServer::BodyMode) const; - Ref _move(const Vector2 &p_motion, bool p_infinite_inertia = true, bool p_exclude_raycast_shapes = true, bool p_test_only = false); Ref _get_slide_collision(int p_bounce); Transform2D last_valid_transform; void _direct_state_changed(Object *p_state); + Vector2 _move_and_slide_internal(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); void _set_collision_direction(const Collision &p_collision, const Vector2 &p_up_direction, float p_floor_max_angle); protected: diff --git a/scene/3d/physics_body.cpp b/scene/3d/physics_body.cpp index ff7a651f45e..62655ad0efc 100644 --- a/scene/3d/physics_body.cpp +++ b/scene/3d/physics_body.cpp @@ -1050,10 +1050,11 @@ 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_up_direction, bool p_stop_on_slope, int p_max_slides, float p_floor_max_angle, bool p_infinite_inertia) { +Vector3 KinematicBody::_move_and_slide_internal(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 body_velocity = p_linear_velocity; Vector3 body_velocity_normal = body_velocity.normalized(); Vector3 up_direction = p_up_direction.normalized(); + bool was_on_floor = on_floor; for (int i = 0; i < 3; i++) { if (locked_axis & (1 << i)) { @@ -1159,6 +1160,39 @@ Vector3 KinematicBody::move_and_slide(const Vector3 &p_linear_velocity, const Ve } } + if (was_on_floor && p_snap != Vector3() && !on_floor) { + // Apply snap. + Collision col; + Transform gt = get_global_transform(); + + 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) { + 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. + 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. + } + } + if (apply) { + gt.origin += col.travel; + set_global_transform(gt); + } + } + } + if (!on_floor && !on_wall) { // Add last platform velocity when just left a moving platform. return body_velocity + current_floor_velocity; @@ -1167,46 +1201,12 @@ Vector3 KinematicBody::move_and_slide(const Vector3 &p_linear_velocity, const Ve return body_velocity; } +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) { + return _move_and_slide_internal(p_linear_velocity, Vector3(), p_up_direction, p_stop_on_slope, p_max_slides, p_floor_max_angle, 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) { - Vector3 up_direction = p_up_direction.normalized(); - bool was_on_floor = on_floor; - - 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() || on_floor) { - return ret; - } - - Collision col; - Transform gt = get_global_transform(); - - 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) { - 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. - 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. - } - } - if (apply) { - gt.origin += col.travel; - set_global_transform(gt); - } - } - - return ret; + return _move_and_slide_internal(p_linear_velocity, p_snap, p_up_direction, p_stop_on_slope, p_max_slides, p_floor_max_angle, p_infinite_inertia); } void KinematicBody::_set_collision_direction(const Collision &p_collision, const Vector3 &p_up_direction, float p_floor_max_angle) { diff --git a/scene/3d/physics_body.h b/scene/3d/physics_body.h index 02200f96a0f..40e8e9d3629 100644 --- a/scene/3d/physics_body.h +++ b/scene/3d/physics_body.h @@ -293,13 +293,13 @@ private: Vector> slide_colliders; Ref motion_cache; - _FORCE_INLINE_ bool _ignores_mode(PhysicsServer::BodyMode) const; - Ref _move(const Vector3 &p_motion, bool p_infinite_inertia = true, bool p_exclude_raycast_shapes = true, bool p_test_only = false); Ref _get_slide_collision(int p_bounce); Transform last_valid_transform; void _direct_state_changed(Object *p_state); + + Vector3 _move_and_slide_internal(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); void _set_collision_direction(const Collision &p_collision, const Vector3 &p_up_direction, float p_floor_max_angle); protected: