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.

6 files changed, 16 insertions, 0 deletions

diff --git a/servers/physics_2d/body_2d_sw.h b/servers/physics_2d/body_2d_sw.h
index b4a95651cb..74bef433dc 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 0fa31c5037..bdc615ab6c 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 e8272f377e..2e44a1a10b 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 8542b54838..fafde62599 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 3ed8841119..63a51dd9d6 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 17bae9a057..3fb01b301b 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;