From f1013492254cfccf88a19e4b6dab692bc0ea23a4 Mon Sep 17 00:00:00 2001 From: PouleyKetchoupp Date: Mon, 9 Aug 2021 11:57:17 -0700 Subject: [PATCH] Fix applied rotation from moving platforms in move_and_slide When synchronizing KinematicBody 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/Physics2DDirectBodyState.xml | 7 +++++++ doc/classes/PhysicsDirectBodyState.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.cpp | 4 +++- servers/physics/body_sw.h | 2 ++ servers/physics_2d/body_2d_sw.h | 6 ++++++ servers/physics_2d_server.cpp | 2 ++ servers/physics_2d_server.h | 2 ++ servers/physics_server.cpp | 2 ++ servers/physics_server.h | 2 ++ 12 files changed, 48 insertions(+), 2 deletions(-) diff --git a/doc/classes/Physics2DDirectBodyState.xml b/doc/classes/Physics2DDirectBodyState.xml index d2a51f39fcc..01b36f4d772 100644 --- a/doc/classes/Physics2DDirectBodyState.xml +++ b/doc/classes/Physics2DDirectBodyState.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/PhysicsDirectBodyState.xml b/doc/classes/PhysicsDirectBodyState.xml index 1c7ce46605f..4d3f77bb496 100644 --- a/doc/classes/PhysicsDirectBodyState.xml +++ b/doc/classes/PhysicsDirectBodyState.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 260407deaab..db1d7bc98b1 100644 --- a/modules/bullet/rigid_body_bullet.cpp +++ b/modules/bullet/rigid_body_bullet.cpp @@ -114,6 +114,16 @@ Transform BulletPhysicsDirectBodyState::get_transform() const { return body->get_transform(); } +Vector3 BulletPhysicsDirectBodyState::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 BulletPhysicsDirectBodyState::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 512c5d0dd5f..cab32cdc680 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 Transform &p_transform); virtual Transform get_transform() const; + virtual Vector3 get_velocity_at_local_position(const Vector3 &p_position) const; + virtual void add_central_force(const Vector3 &p_force); virtual void add_force(const Vector3 &p_force, const Vector3 &p_pos); virtual void add_torque(const Vector3 &p_torque); diff --git a/scene/2d/physics_body_2d.cpp b/scene/2d/physics_body_2d.cpp index e49ca76fc4a..42770a2e4dc 100644 --- a/scene/2d/physics_body_2d.cpp +++ b/scene/2d/physics_body_2d.cpp @@ -1115,7 +1115,9 @@ Vector2 KinematicBody2D::move_and_slide(const Vector2 &p_linear_velocity, const //this approach makes sure there is less delay between the actual body velocity and the one we saved Physics2DDirectBodyState *bs = Physics2DServer::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.cpp b/scene/3d/physics_body.cpp index c6edaac2aaf..96ea7cc2e30 100644 --- a/scene/3d/physics_body.cpp +++ b/scene/3d/physics_body.cpp @@ -1066,7 +1066,9 @@ Vector3 KinematicBody::move_and_slide(const Vector3 &p_linear_velocity, const Ve // This approach makes sure there is less delay between the actual body velocity and the one we saved. PhysicsDirectBodyState *bs = PhysicsServer::get_singleton()->body_get_direct_state(on_floor_body); if (bs) { - current_floor_velocity = bs->get_linear_velocity(); + Transform 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/body_sw.h b/servers/physics/body_sw.h index 4a837130e1d..70f4a8ec4c8 100644 --- a/servers/physics/body_sw.h +++ b/servers/physics/body_sw.h @@ -402,6 +402,8 @@ public: virtual void set_transform(const Transform &p_transform) { body->set_state(PhysicsServer::BODY_STATE_TRANSFORM, p_transform); } virtual Transform get_transform() const { return body->get_transform(); } + virtual Vector3 get_velocity_at_local_position(const Vector3 &p_position) const { return body->get_velocity_in_local_point(p_position); } + virtual void add_central_force(const Vector3 &p_force) { body->add_central_force(p_force); } virtual void add_force(const Vector3 &p_force, const Vector3 &p_pos) { body->add_force(p_force, p_pos); } virtual void add_torque(const Vector3 &p_torque) { body->add_torque(p_torque); } diff --git a/servers/physics_2d/body_2d_sw.h b/servers/physics_2d/body_2d_sw.h index bd996c7615c..b9e78326888 100644 --- a/servers/physics_2d/body_2d_sw.h +++ b/servers/physics_2d/body_2d_sw.h @@ -273,6 +273,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 > Physics2DServer::BODY_MODE_KINEMATIC) { return new_transform.get_origin() - get_transform().get_origin(); @@ -359,6 +363,8 @@ public: virtual void set_transform(const Transform2D &p_transform) { body->set_state(Physics2DServer::BODY_STATE_TRANSFORM, p_transform); } virtual Transform2D get_transform() const { return body->get_transform(); } + virtual Vector2 get_velocity_at_local_position(const Vector2 &p_position) const { return body->get_velocity_in_local_point(p_position); } + virtual void add_central_force(const Vector2 &p_force) { body->add_central_force(p_force); } virtual void add_force(const Vector2 &p_offset, const Vector2 &p_force) { body->add_force(p_offset, p_force); } virtual void add_torque(real_t p_torque) { body->add_torque(p_torque); } diff --git a/servers/physics_2d_server.cpp b/servers/physics_2d_server.cpp index 0425923f15e..b8c6d0c9bac 100644 --- a/servers/physics_2d_server.cpp +++ b/servers/physics_2d_server.cpp @@ -90,6 +90,8 @@ void Physics2DDirectBodyState::_bind_methods() { ClassDB::bind_method(D_METHOD("set_transform", "transform"), &Physics2DDirectBodyState::set_transform); ClassDB::bind_method(D_METHOD("get_transform"), &Physics2DDirectBodyState::get_transform); + ClassDB::bind_method(D_METHOD("get_velocity_at_local_position", "local_position"), &Physics2DDirectBodyState::get_velocity_at_local_position); + ClassDB::bind_method(D_METHOD("add_central_force", "force"), &Physics2DDirectBodyState::add_central_force); ClassDB::bind_method(D_METHOD("add_force", "offset", "force"), &Physics2DDirectBodyState::add_force); ClassDB::bind_method(D_METHOD("add_torque", "torque"), &Physics2DDirectBodyState::add_torque); diff --git a/servers/physics_2d_server.h b/servers/physics_2d_server.h index bba68981f4a..9a17521597b 100644 --- a/servers/physics_2d_server.h +++ b/servers/physics_2d_server.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_offset, const Vector2 &p_force) = 0; virtual void add_torque(real_t p_torque) = 0; diff --git a/servers/physics_server.cpp b/servers/physics_server.cpp index 717c5b01b7d..591652e7380 100644 --- a/servers/physics_server.cpp +++ b/servers/physics_server.cpp @@ -92,6 +92,8 @@ void PhysicsDirectBodyState::_bind_methods() { ClassDB::bind_method(D_METHOD("set_transform", "transform"), &PhysicsDirectBodyState::set_transform); ClassDB::bind_method(D_METHOD("get_transform"), &PhysicsDirectBodyState::get_transform); + ClassDB::bind_method(D_METHOD("get_velocity_at_local_position", "local_position"), &PhysicsDirectBodyState::get_velocity_at_local_position); + ClassDB::bind_method(D_METHOD("add_central_force", "force"), &PhysicsDirectBodyState::add_central_force); ClassDB::bind_method(D_METHOD("add_force", "force", "position"), &PhysicsDirectBodyState::add_force); ClassDB::bind_method(D_METHOD("add_torque", "torque"), &PhysicsDirectBodyState::add_torque); diff --git a/servers/physics_server.h b/servers/physics_server.h index 264a16fdd66..2580b313a3f 100644 --- a/servers/physics_server.h +++ b/servers/physics_server.h @@ -62,6 +62,8 @@ public: virtual void set_transform(const Transform &p_transform) = 0; virtual Transform 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_pos) = 0; virtual void add_torque(const Vector3 &p_torque) = 0;