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, 4 deletions

diff --git a/servers/physics_2d/godot_body_2d.cpp b/servers/physics_2d/godot_body_2d.cpp
index 6d1c5539aa..44da5d4f3b 100644
--- a/servers/physics_2d/godot_body_2d.cpp
+++ b/servers/physics_2d/godot_body_2d.cpp
@@ -549,6 +549,9 @@ void GodotBody2D::integrate_forces(real_t p_step) {
 
 	gravity *= gravity_scale;
 
+	prev_linear_velocity = linear_velocity;
+	prev_angular_velocity = angular_velocity;
+
 	Vector2 motion;
 	bool do_motion = false;
 
diff --git a/servers/physics_2d/godot_body_2d.h b/servers/physics_2d/godot_body_2d.h
index 0df93482dc..ba4c39737a 100644
--- a/servers/physics_2d/godot_body_2d.h
+++ b/servers/physics_2d/godot_body_2d.h
@@ -50,6 +50,9 @@ class GodotBody2D : public GodotCollisionObject2D {
 	Vector2 linear_velocity;
 	real_t angular_velocity = 0.0;
 
+	Vector2 prev_linear_velocity;
+	real_t prev_angular_velocity = 0.0;
+
 	Vector2 constant_linear_velocity;
 	real_t constant_angular_velocity = 0.0;
 
@@ -209,6 +212,9 @@ public:
 	_FORCE_INLINE_ void set_angular_velocity(real_t p_velocity) { angular_velocity = p_velocity; }
 	_FORCE_INLINE_ real_t get_angular_velocity() const { return angular_velocity; }
 
+	_FORCE_INLINE_ Vector2 get_prev_linear_velocity() const { return prev_linear_velocity; }
+	_FORCE_INLINE_ real_t get_prev_angular_velocity() const { return prev_angular_velocity; }
+
 	_FORCE_INLINE_ void set_biased_linear_velocity(const Vector2 &p_velocity) { biased_linear_velocity = p_velocity; }
 	_FORCE_INLINE_ Vector2 get_biased_linear_velocity() const { return biased_linear_velocity; }
 
diff --git a/servers/physics_2d/godot_body_pair_2d.cpp b/servers/physics_2d/godot_body_pair_2d.cpp
index 97eeefbfe6..67b0f21456 100644
--- a/servers/physics_2d/godot_body_pair_2d.cpp
+++ b/servers/physics_2d/godot_body_pair_2d.cpp
@@ -32,7 +32,6 @@
 #include "godot_collision_solver_2d.h"
 #include "godot_space_2d.h"
 
-#define POSITION_CORRECTION
 #define ACCUMULATE_IMPULSES
 
 void GodotBodyPair2D::_add_contact(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_self) {
@@ -453,9 +452,9 @@ bool GodotBodyPair2D::pre_solve(real_t p_step) {
 
 		c.bounce = combine_bounce(A, B);
 		if (c.bounce) {
-			Vector2 crA(-A->get_angular_velocity() * c.rA.y, A->get_angular_velocity() * c.rA.x);
-			Vector2 crB(-B->get_angular_velocity() * c.rB.y, B->get_angular_velocity() * c.rB.x);
-			Vector2 dv = B->get_linear_velocity() + crB - A->get_linear_velocity() - crA;
+			Vector2 crA(-A->get_prev_angular_velocity() * c.rA.y, A->get_prev_angular_velocity() * c.rA.x);
+			Vector2 crB(-B->get_prev_angular_velocity() * c.rB.y, B->get_prev_angular_velocity() * c.rB.x);
+			Vector2 dv = B->get_prev_linear_velocity() + crB - A->get_prev_linear_velocity() - crA;
 			c.bounce = c.bounce * dv.dot(c.normal);
 		}