4 files changed, 16 insertions, 8 deletions
diff --git a/servers/physics_2d/body_2d_sw.cpp b/servers/physics_2d/body_2d_sw.cpp
index 64f71fd25c..d86943e658 100644
--- a/servers/physics_2d/body_2d_sw.cpp
+++ b/servers/physics_2d/body_2d_sw.cpp
@@ -269,11 +269,13 @@ void Body2DSW::set_state(PhysicsServer2D::BodyState p_state, const Variant &p_va
 		} break;
 		case PhysicsServer2D::BODY_STATE_LINEAR_VELOCITY: {
 			linear_velocity = p_variant;
+			constant_linear_velocity = linear_velocity;
 			wakeup();
 
 		} break;
 		case PhysicsServer2D::BODY_STATE_ANGULAR_VELOCITY: {
 			angular_velocity = p_variant;
+			constant_angular_velocity = angular_velocity;
 			wakeup();
 
 		} break;
@@ -429,10 +431,10 @@ void Body2DSW::integrate_forces(real_t p_step) {
 	if (mode == PhysicsServer2D::BODY_MODE_KINEMATIC) {
 		//compute motion, angular and etc. velocities from prev transform
 		motion = new_transform.get_origin() - get_transform().get_origin();
-		linear_velocity = motion / p_step;
+		linear_velocity = constant_linear_velocity + motion / p_step;
 
 		real_t rot = new_transform.get_rotation() - get_transform().get_rotation();
-		angular_velocity = remainder(rot, 2.0 * Math_PI) / p_step;
+		angular_velocity = constant_angular_velocity + remainder(rot, 2.0 * Math_PI) / p_step;
 
 		do_motion = true;
 
@@ -621,8 +623,6 @@ Body2DSW::Body2DSW() :
 		direct_state_query_list(this) {
 	mode = PhysicsServer2D::BODY_MODE_DYNAMIC;
 	active = true;
-	angular_velocity = 0;
-	biased_angular_velocity = 0;
 	mass = 1;
 	inertia = 0;
 	user_inertia = false;
diff --git a/servers/physics_2d/body_2d_sw.h b/servers/physics_2d/body_2d_sw.h
index 9cd53ceca1..b83afdbec6 100644
--- a/servers/physics_2d/body_2d_sw.h
+++ b/servers/physics_2d/body_2d_sw.h
@@ -44,10 +44,13 @@ class Body2DSW : public CollisionObject2DSW {
 	PhysicsServer2D::BodyMode mode;
 
 	Vector2 biased_linear_velocity;
-	real_t biased_angular_velocity;
+	real_t biased_angular_velocity = 0.0;
 
 	Vector2 linear_velocity;
-	real_t angular_velocity;
+	real_t angular_velocity = 0.0;
+
+	Vector2 constant_linear_velocity;
+	real_t constant_angular_velocity = 0.0;
 
 	real_t linear_damp;
 	real_t angular_damp;
diff --git a/servers/physics_3d/body_3d_sw.cpp b/servers/physics_3d/body_3d_sw.cpp
index 7f62e4eb85..6640245d91 100644
--- a/servers/physics_3d/body_3d_sw.cpp
+++ b/servers/physics_3d/body_3d_sw.cpp
@@ -302,10 +302,12 @@ void Body3DSW::set_state(PhysicsServer3D::BodyState p_state, const Variant &p_va
 		} break;
 		case PhysicsServer3D::BODY_STATE_LINEAR_VELOCITY: {
 			linear_velocity = p_variant;
+			constant_linear_velocity = linear_velocity;
 			wakeup();
 		} break;
 		case PhysicsServer3D::BODY_STATE_ANGULAR_VELOCITY: {
 			angular_velocity = p_variant;
+			constant_angular_velocity = angular_velocity;
 			wakeup();
 
 		} break;
@@ -473,7 +475,7 @@ void Body3DSW::integrate_forces(real_t p_step) {
 		//compute motion, angular and etc. velocities from prev transform
 		motion = new_transform.origin - get_transform().origin;
 		do_motion = true;
-		linear_velocity = motion / p_step;
+		linear_velocity = constant_linear_velocity + motion / p_step;
 
 		//compute a FAKE angular velocity, not so easy
 		Basis rot = new_transform.basis.orthonormalized() * get_transform().basis.orthonormalized().transposed();
@@ -482,7 +484,7 @@ void Body3DSW::integrate_forces(real_t p_step) {
 
 		rot.get_axis_angle(axis, angle);
 		axis.normalize();
-		angular_velocity = axis * (angle / p_step);
+		angular_velocity = constant_angular_velocity + axis * (angle / p_step);
 	} else {
 		if (!omit_force_integration && !first_integration) {
 			//overridden by direct state query
diff --git a/servers/physics_3d/body_3d_sw.h b/servers/physics_3d/body_3d_sw.h
index f58f40652b..38b7c4f94c 100644
--- a/servers/physics_3d/body_3d_sw.h
+++ b/servers/physics_3d/body_3d_sw.h
@@ -44,6 +44,9 @@ class Body3DSW : public CollisionObject3DSW {
 	Vector3 linear_velocity;
 	Vector3 angular_velocity;
 
+	Vector3 constant_linear_velocity;
+	Vector3 constant_angular_velocity;
+
 	Vector3 biased_linear_velocity;
 	Vector3 biased_angular_velocity;
 	real_t mass;