6 files changed, 24 insertions, 20 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);
 		}
 
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);
 		}
 	}