From 5650c83e4b7f669066685a9f5526d7ec56e9b43d Mon Sep 17 00:00:00 2001 From: PouleyKetchoupp Date: Mon, 9 Aug 2021 12:14:24 -0700 Subject: [PATCH] Fix applied rotation from moving platforms in move_and_slide When synchronizing CharacterBody motion with moving the platform using direct body state, only the linear velocity was taken into account. This change exposes velocity at local point in direct body state and uses it in move_and_slide to get the proper velocity that includes rotations. --- doc/classes/PhysicsDirectBodyState2D.xml | 7 +++++++ doc/classes/PhysicsDirectBodyState3D.xml | 7 +++++++ modules/bullet/rigid_body_bullet.cpp | 10 ++++++++++ modules/bullet/rigid_body_bullet.h | 2 ++ scene/2d/physics_body_2d.cpp | 4 +++- scene/3d/physics_body_3d.cpp | 4 +++- servers/physics_2d/body_2d_sw.h | 6 ++++++ servers/physics_3d/body_3d_sw.h | 2 ++ servers/physics_server_2d.cpp | 2 ++ servers/physics_server_2d.h | 2 ++ servers/physics_server_3d.cpp | 2 ++ servers/physics_server_3d.h | 2 ++ 12 files changed, 48 insertions(+), 2 deletions(-) diff --git a/doc/classes/PhysicsDirectBodyState2D.xml b/doc/classes/PhysicsDirectBodyState2D.xml index ab3a04698f08..4c6adfca322b 100644 --- a/doc/classes/PhysicsDirectBodyState2D.xml +++ b/doc/classes/PhysicsDirectBodyState2D.xml @@ -137,6 +137,13 @@ Returns the current state of the space, useful for queries. + + + + + Returns the body's velocity at the given relative position, including both translation and rotation. + + diff --git a/doc/classes/PhysicsDirectBodyState3D.xml b/doc/classes/PhysicsDirectBodyState3D.xml index 6af62c2bcc31..271668e339b7 100644 --- a/doc/classes/PhysicsDirectBodyState3D.xml +++ b/doc/classes/PhysicsDirectBodyState3D.xml @@ -138,6 +138,13 @@ Returns the current state of the space, useful for queries. + + + + + Returns the body's velocity at the given relative position, including both translation and rotation. + + diff --git a/modules/bullet/rigid_body_bullet.cpp b/modules/bullet/rigid_body_bullet.cpp index ce39d4f0dfed..0d2cd1f5a062 100644 --- a/modules/bullet/rigid_body_bullet.cpp +++ b/modules/bullet/rigid_body_bullet.cpp @@ -114,6 +114,16 @@ Transform3D BulletPhysicsDirectBodyState3D::get_transform() const { return body->get_transform(); } +Vector3 BulletPhysicsDirectBodyState3D::get_velocity_at_local_position(const Vector3 &p_position) const { + btVector3 local_position; + G_TO_B(p_position, local_position); + + Vector3 velocity; + B_TO_G(body->btBody->getVelocityInLocalPoint(local_position), velocity); + + return velocity; +} + void BulletPhysicsDirectBodyState3D::add_central_force(const Vector3 &p_force) { body->apply_central_force(p_force); } diff --git a/modules/bullet/rigid_body_bullet.h b/modules/bullet/rigid_body_bullet.h index 606df7134b2b..5e102d8b054d 100644 --- a/modules/bullet/rigid_body_bullet.h +++ b/modules/bullet/rigid_body_bullet.h @@ -110,6 +110,8 @@ public: virtual void set_transform(const Transform3D &p_transform) override; virtual Transform3D get_transform() const override; + virtual Vector3 get_velocity_at_local_position(const Vector3 &p_position) const override; + virtual void add_central_force(const Vector3 &p_force) override; virtual void add_force(const Vector3 &p_force, const Vector3 &p_position = Vector3()) override; virtual void add_torque(const Vector3 &p_torque) override; diff --git a/scene/2d/physics_body_2d.cpp b/scene/2d/physics_body_2d.cpp index a382fb2f1e25..00eeb601922a 100644 --- a/scene/2d/physics_body_2d.cpp +++ b/scene/2d/physics_body_2d.cpp @@ -1062,7 +1062,9 @@ void CharacterBody2D::move_and_slide() { //this approach makes sure there is less delay between the actual body velocity and the one we saved PhysicsDirectBodyState2D *bs = PhysicsServer2D::get_singleton()->body_get_direct_state(on_floor_body); if (bs) { - current_floor_velocity = bs->get_linear_velocity(); + Transform2D gt = get_global_transform(); + Vector2 local_position = gt.elements[2] - bs->get_transform().elements[2]; + current_floor_velocity = bs->get_velocity_at_local_position(local_position); } } diff --git a/scene/3d/physics_body_3d.cpp b/scene/3d/physics_body_3d.cpp index 100e3563a3b7..38104a83657c 100644 --- a/scene/3d/physics_body_3d.cpp +++ b/scene/3d/physics_body_3d.cpp @@ -1101,7 +1101,9 @@ void CharacterBody3D::move_and_slide() { //this approach makes sure there is less delay between the actual body velocity and the one we saved PhysicsDirectBodyState3D *bs = PhysicsServer3D::get_singleton()->body_get_direct_state(on_floor_body); if (bs) { - current_floor_velocity = bs->get_linear_velocity(); + Transform3D gt = get_global_transform(); + Vector3 local_position = gt.origin - bs->get_transform().origin; + current_floor_velocity = bs->get_velocity_at_local_position(local_position); } } diff --git a/servers/physics_2d/body_2d_sw.h b/servers/physics_2d/body_2d_sw.h index b4a95651cb78..74bef433dc56 100644 --- a/servers/physics_2d/body_2d_sw.h +++ b/servers/physics_2d/body_2d_sw.h @@ -266,6 +266,10 @@ public: void integrate_forces(real_t p_step); void integrate_velocities(real_t p_step); + _FORCE_INLINE_ Vector2 get_velocity_in_local_point(const Vector2 &rel_pos) const { + return linear_velocity + Vector2(-angular_velocity * rel_pos.y, angular_velocity * rel_pos.x); + } + _FORCE_INLINE_ Vector2 get_motion() const { if (mode > PhysicsServer2D::BODY_MODE_KINEMATIC) { return new_transform.get_origin() - get_transform().get_origin(); @@ -352,6 +356,8 @@ public: virtual void set_transform(const Transform2D &p_transform) override { body->set_state(PhysicsServer2D::BODY_STATE_TRANSFORM, p_transform); } virtual Transform2D get_transform() const override { return body->get_transform(); } + virtual Vector2 get_velocity_at_local_position(const Vector2 &p_position) const override { return body->get_velocity_in_local_point(p_position); } + virtual void add_central_force(const Vector2 &p_force) override { body->add_central_force(p_force); } virtual void add_force(const Vector2 &p_force, const Vector2 &p_position = Vector2()) override { body->add_force(p_force, p_position); } virtual void add_torque(real_t p_torque) override { body->add_torque(p_torque); } diff --git a/servers/physics_3d/body_3d_sw.h b/servers/physics_3d/body_3d_sw.h index 0fa31c503759..bdc615ab6c69 100644 --- a/servers/physics_3d/body_3d_sw.h +++ b/servers/physics_3d/body_3d_sw.h @@ -389,6 +389,8 @@ public: virtual void set_transform(const Transform3D &p_transform) override { body->set_state(PhysicsServer3D::BODY_STATE_TRANSFORM, p_transform); } virtual Transform3D get_transform() const override { return body->get_transform(); } + virtual Vector3 get_velocity_at_local_position(const Vector3 &p_position) const override { return body->get_velocity_in_local_point(p_position); } + virtual void add_central_force(const Vector3 &p_force) override { body->add_central_force(p_force); } virtual void add_force(const Vector3 &p_force, const Vector3 &p_position = Vector3()) override { body->add_force(p_force, p_position); diff --git a/servers/physics_server_2d.cpp b/servers/physics_server_2d.cpp index e8272f377ecd..2e44a1a10baa 100644 --- a/servers/physics_server_2d.cpp +++ b/servers/physics_server_2d.cpp @@ -89,6 +89,8 @@ void PhysicsDirectBodyState2D::_bind_methods() { ClassDB::bind_method(D_METHOD("set_transform", "transform"), &PhysicsDirectBodyState2D::set_transform); ClassDB::bind_method(D_METHOD("get_transform"), &PhysicsDirectBodyState2D::get_transform); + ClassDB::bind_method(D_METHOD("get_velocity_at_local_position", "local_position"), &PhysicsDirectBodyState2D::get_velocity_at_local_position); + ClassDB::bind_method(D_METHOD("add_central_force", "force"), &PhysicsDirectBodyState2D::add_central_force); ClassDB::bind_method(D_METHOD("add_force", "force", "position"), &PhysicsDirectBodyState2D::add_force, Vector2()); ClassDB::bind_method(D_METHOD("add_torque", "torque"), &PhysicsDirectBodyState2D::add_torque); diff --git a/servers/physics_server_2d.h b/servers/physics_server_2d.h index 8542b548382d..fafde62599df 100644 --- a/servers/physics_server_2d.h +++ b/servers/physics_server_2d.h @@ -60,6 +60,8 @@ public: virtual void set_transform(const Transform2D &p_transform) = 0; virtual Transform2D get_transform() const = 0; + virtual Vector2 get_velocity_at_local_position(const Vector2 &p_position) const = 0; + virtual void add_central_force(const Vector2 &p_force) = 0; virtual void add_force(const Vector2 &p_force, const Vector2 &p_position = Vector2()) = 0; virtual void add_torque(real_t p_torque) = 0; diff --git a/servers/physics_server_3d.cpp b/servers/physics_server_3d.cpp index 3ed884111944..63a51dd9d6b2 100644 --- a/servers/physics_server_3d.cpp +++ b/servers/physics_server_3d.cpp @@ -91,6 +91,8 @@ void PhysicsDirectBodyState3D::_bind_methods() { ClassDB::bind_method(D_METHOD("set_transform", "transform"), &PhysicsDirectBodyState3D::set_transform); ClassDB::bind_method(D_METHOD("get_transform"), &PhysicsDirectBodyState3D::get_transform); + ClassDB::bind_method(D_METHOD("get_velocity_at_local_position", "local_position"), &PhysicsDirectBodyState3D::get_velocity_at_local_position); + ClassDB::bind_method(D_METHOD("add_central_force", "force"), &PhysicsDirectBodyState3D::add_central_force, Vector3()); ClassDB::bind_method(D_METHOD("add_force", "force", "position"), &PhysicsDirectBodyState3D::add_force, Vector3()); ClassDB::bind_method(D_METHOD("add_torque", "torque"), &PhysicsDirectBodyState3D::add_torque); diff --git a/servers/physics_server_3d.h b/servers/physics_server_3d.h index 17bae9a057b5..3fb01b301b12 100644 --- a/servers/physics_server_3d.h +++ b/servers/physics_server_3d.h @@ -62,6 +62,8 @@ public: virtual void set_transform(const Transform3D &p_transform) = 0; virtual Transform3D get_transform() const = 0; + virtual Vector3 get_velocity_at_local_position(const Vector3 &p_position) const = 0; + virtual void add_central_force(const Vector3 &p_force) = 0; virtual void add_force(const Vector3 &p_force, const Vector3 &p_position = Vector3()) = 0; virtual void add_torque(const Vector3 &p_torque) = 0;