From 27c47e09a183359706ef2497da9278b46f9190b6 Mon Sep 17 00:00:00 2001
From: Juan Linietsky <reduzio@gmail.com>
Date: Sat, 2 Jan 2016 13:28:18 -0300
Subject: [PATCH] skip the first integration in physics bodies, fixes #2213

---
 servers/physics/body_sw.cpp       | 6 +++++-
 servers/physics/body_sw.h         | 2 ++
 servers/physics_2d/body_2d_sw.cpp | 5 ++++-
 servers/physics_2d/body_2d_sw.h   | 1 +
 4 files changed, 12 insertions(+), 2 deletions(-)

diff --git a/servers/physics/body_sw.cpp b/servers/physics/body_sw.cpp
index bfac852481c..b0ed99cb481 100644
--- a/servers/physics/body_sw.cpp
+++ b/servers/physics/body_sw.cpp
@@ -380,6 +380,8 @@ void BodySW::set_space(SpaceSW *p_space){
 
 	}
 
+	first_integration=true;
+
 }
 
 void BodySW::_compute_area_gravity_and_dampenings(const AreaSW *p_area) {
@@ -479,7 +481,7 @@ void BodySW::integrate_forces(real_t p_step) {
 		do_motion=true;
 
 	} else {
-		if (!omit_force_integration) {
+		if (!omit_force_integration && !first_integration) {
 			//overriden by direct state query
 
 			Vector3 force=gravity*mass;
@@ -512,6 +514,7 @@ void BodySW::integrate_forces(real_t p_step) {
 
 	applied_force=Vector3();
 	applied_torque=Vector3();
+	first_integration=false;
 
 	//motion=linear_velocity*p_step;
 
@@ -749,6 +752,7 @@ BodySW::BodySW() : CollisionObjectSW(TYPE_BODY), active_list(this), inertia_upda
 	island_next=NULL;
 	island_list_next=NULL;
 	first_time_kinematic=false;
+	first_integration=false;
 	_set_static(false);
 
 	contact_count=0;
diff --git a/servers/physics/body_sw.h b/servers/physics/body_sw.h
index 7369b4cecf2..c958177a19c 100644
--- a/servers/physics/body_sw.h
+++ b/servers/physics/body_sw.h
@@ -79,6 +79,8 @@ class BodySW : public CollisionObjectSW {
 	bool omit_force_integration;
 	bool active;
 
+	bool first_integration;
+
 	bool continuous_cd;
 	bool can_sleep;
 	bool first_time_kinematic;
diff --git a/servers/physics_2d/body_2d_sw.cpp b/servers/physics_2d/body_2d_sw.cpp
index 129ce708d74..6721e188d22 100644
--- a/servers/physics_2d/body_2d_sw.cpp
+++ b/servers/physics_2d/body_2d_sw.cpp
@@ -378,6 +378,7 @@ void Body2DSW::set_space(Space2DSW *p_space){
 
 	}
 
+	first_integration=false;
 }
 
 void Body2DSW::_compute_area_gravity_and_dampenings(const Area2DSW *p_area) {
@@ -471,7 +472,7 @@ void Body2DSW::integrate_forces(real_t p_step) {
 		//}
 
 	} else {
-		if (!omit_force_integration) {
+		if (!omit_force_integration && !first_integration) {
 			//overriden by direct state query
 
 			Vector2 force=gravity*mass;
@@ -506,6 +507,7 @@ void Body2DSW::integrate_forces(real_t p_step) {
 
 	//motion=linear_velocity*p_step;
 
+	first_integration=false;
 	biased_angular_velocity=0;
 	biased_linear_velocity=Vector2();
 
@@ -681,6 +683,7 @@ Body2DSW::Body2DSW() : CollisionObject2DSW(TYPE_BODY), active_list(this), inerti
 	gravity_scale=1.0;
 	using_one_way_cache=false;
 	one_way_collision_max_depth=0.1;
+	first_integration=false;
 
 	still_time=0;
 	continuous_cd_mode=Physics2DServer::CCD_MODE_DISABLED;
diff --git a/servers/physics_2d/body_2d_sw.h b/servers/physics_2d/body_2d_sw.h
index ae64875567c..97a0a312e70 100644
--- a/servers/physics_2d/body_2d_sw.h
+++ b/servers/physics_2d/body_2d_sw.h
@@ -81,6 +81,7 @@ class Body2DSW : public CollisionObject2DSW {
 	bool active;
 	bool can_sleep;
 	bool first_time_kinematic;
+	bool first_integration;
 	bool using_one_way_cache;
 	void _update_inertia();
 	virtual void _shapes_changed();