Fix RigidDynamicBody gaining momentum with bounce

Bounce calculation now uses the previous frame's velocity, so it's
consistent with the actual motion of the bodies involved and not the
yet-to-be-applied forces.

When bounce is 1, using the current velocity was causing the new forces
(including gravity) to be taken into account, which lead to the bounce
velocity to be higher than the falling velocity at the moment of impact,
adding more and more energy over time.

3 files changed, 12 insertions, 16 deletions

diff --git a/servers/physics_3d/godot_body_3d.cpp b/servers/physics_3d/godot_body_3d.cpp
index 40d946655d..d5098a2a5f 100644
--- a/servers/physics_3d/godot_body_3d.cpp
+++ b/servers/physics_3d/godot_body_3d.cpp
@@ -604,6 +604,9 @@ void GodotBody3D::integrate_forces(real_t p_step) {
 
 	gravity *= gravity_scale;
 
+	prev_linear_velocity = linear_velocity;
+	prev_angular_velocity = angular_velocity;
+
 	Vector3 motion;
 	bool do_motion = false;
 
diff --git a/servers/physics_3d/godot_body_3d.h b/servers/physics_3d/godot_body_3d.h
index bba9ec6c3a..6ea6d1fcaa 100644
--- a/servers/physics_3d/godot_body_3d.h
+++ b/servers/physics_3d/godot_body_3d.h
@@ -45,6 +45,9 @@ class GodotBody3D : public GodotCollisionObject3D {
 	Vector3 linear_velocity;
 	Vector3 angular_velocity;
 
+	Vector3 prev_linear_velocity;
+	Vector3 prev_angular_velocity;
+
 	Vector3 constant_linear_velocity;
 	Vector3 constant_angular_velocity;
 
@@ -207,6 +210,9 @@ public:
 	_FORCE_INLINE_ void set_angular_velocity(const Vector3 &p_velocity) { angular_velocity = p_velocity; }
 	_FORCE_INLINE_ Vector3 get_angular_velocity() const { return angular_velocity; }
 
+	_FORCE_INLINE_ Vector3 get_prev_linear_velocity() const { return prev_linear_velocity; }
+	_FORCE_INLINE_ Vector3 get_prev_angular_velocity() const { return prev_angular_velocity; }
+
 	_FORCE_INLINE_ const Vector3 &get_biased_linear_velocity() const { return biased_linear_velocity; }
 	_FORCE_INLINE_ const Vector3 &get_biased_angular_velocity() const { return biased_angular_velocity; }
 
diff --git a/servers/physics_3d/godot_body_pair_3d.cpp b/servers/physics_3d/godot_body_pair_3d.cpp
index f0002870ae..8a701466ed 100644
--- a/servers/physics_3d/godot_body_pair_3d.cpp
+++ b/servers/physics_3d/godot_body_pair_3d.cpp
@@ -35,18 +35,6 @@
 
 #include "core/os/os.h"
 
-/*
-#define NO_ACCUMULATE_IMPULSES
-#define NO_SPLIT_IMPULSES
-
-#define NO_FRICTION
-*/
-
-#define NO_TANGENTIALS
-/* BODY PAIR */
-
-//#define ALLOWED_PENETRATION 0.01
-#define RELAXATION_TIMESTEPS 3
 #define MIN_VELOCITY 0.0001
 #define MAX_BIAS_ROTATION (Math_PI / 8)
 
@@ -370,10 +358,9 @@ bool GodotBodyPair3D::pre_solve(real_t p_step) {
 
 		c.bounce = combine_bounce(A, B);
 		if (c.bounce) {
-			Vector3 crA = A->get_angular_velocity().cross(c.rA);
-			Vector3 crB = B->get_angular_velocity().cross(c.rB);
-			Vector3 dv = B->get_linear_velocity() + crB - A->get_linear_velocity() - crA;
-			//normal impule
+			Vector3 crA = A->get_prev_angular_velocity().cross(c.rA);
+			Vector3 crB = B->get_prev_angular_velocity().cross(c.rB);
+			Vector3 dv = B->get_prev_linear_velocity() + crB - A->get_prev_linear_velocity() - crA;
 			c.bounce = c.bounce * dv.dot(c.normal);
 		}
 	}