properly wake up bodies when a parameter changes, fixes #1740

6 files changed, 37 insertions, 2 deletions

diff --git a/servers/physics/body_sw.cpp b/servers/physics/body_sw.cpp
index 05b3fa727e..43c20d211f 100644
--- a/servers/physics/body_sw.cpp
+++ b/servers/physics/body_sw.cpp
@@ -261,6 +261,7 @@ void BodySW::set_state(PhysicsServer::BodyState p_state, const Variant& p_varian
 				_set_inv_transform(get_transform().inverse());
 
 			}
+			wakeup();
 
 		} break;
 		case PhysicsServer::BODY_STATE_LINEAR_VELOCITY: {
@@ -268,11 +269,13 @@ void BodySW::set_state(PhysicsServer::BodyState p_state, const Variant& p_varian
 			//if (mode==PhysicsServer::BODY_MODE_STATIC)
 			//	break;
 			linear_velocity=p_variant;
+			wakeup();
 		} break;
 		case PhysicsServer::BODY_STATE_ANGULAR_VELOCITY: {
 			//if (mode!=PhysicsServer::BODY_MODE_RIGID)
 			//	break;
 			angular_velocity=p_variant;
+			wakeup();
 
 		} break;
 		case PhysicsServer::BODY_STATE_SLEEPING: {
diff --git a/servers/physics/body_sw.h b/servers/physics/body_sw.h
index 63dd3e6762..f15fd9b831 100644
--- a/servers/physics/body_sw.h
+++ b/servers/physics/body_sw.h
@@ -200,6 +200,12 @@ public:
 	void set_active(bool p_active);
 	_FORCE_INLINE_ bool is_active() const { return active; }
 
+	_FORCE_INLINE_ void wakeup() {
+		if ((get_space() && active) || mode==PhysicsServer::BODY_MODE_STATIC || mode==PhysicsServer::BODY_MODE_KINEMATIC)
+			return;
+		set_active(true);
+	}
+
 	void set_param(PhysicsServer::BodyParameter p_param, float);
 	float get_param(PhysicsServer::BodyParameter p_param) const;
 
diff --git a/servers/physics/physics_server_sw.cpp b/servers/physics/physics_server_sw.cpp
index 53335f2e19..2b4a137e11 100644
--- a/servers/physics/physics_server_sw.cpp
+++ b/servers/physics/physics_server_sw.cpp
@@ -604,6 +604,7 @@ void PhysicsServerSW::body_set_layer_mask(RID p_body, uint32_t p_mask) {
 	ERR_FAIL_COND(!body);
 
 	body->set_layer_mask(p_mask);
+	body->wakeup();
 
 }
 
@@ -674,6 +675,7 @@ void PhysicsServerSW::body_set_state(RID p_body, BodyState p_state, const Varian
 	ERR_FAIL_COND(!body);
 
 	body->set_state(p_state,p_variant);
+
 };
 
 Variant PhysicsServerSW::body_get_state(RID p_body, BodyState p_state) const {
@@ -691,6 +693,7 @@ void PhysicsServerSW::body_set_applied_force(RID p_body, const Vector3& p_force)
 	ERR_FAIL_COND(!body);
 
 	body->set_applied_force(p_force);
+	body->wakeup();
 };
 
 Vector3 PhysicsServerSW::body_get_applied_force(RID p_body) const {
@@ -706,6 +709,7 @@ void PhysicsServerSW::body_set_applied_torque(RID p_body, const Vector3& p_torqu
 	ERR_FAIL_COND(!body);
 
 	body->set_applied_torque(p_torque);
+	body->wakeup();
 };
 
 Vector3 PhysicsServerSW::body_get_applied_torque(RID p_body) const {
@@ -722,6 +726,7 @@ void PhysicsServerSW::body_apply_impulse(RID p_body, const Vector3& p_pos, const
 	ERR_FAIL_COND(!body);
 
 	body->apply_impulse(p_pos,p_impulse);
+	body->wakeup();
 };
 
 void PhysicsServerSW::body_set_axis_velocity(RID p_body, const Vector3& p_axis_velocity) {
@@ -734,6 +739,7 @@ void PhysicsServerSW::body_set_axis_velocity(RID p_body, const Vector3& p_axis_v
 	v-=axis*axis.dot(v);
 	v+=p_axis_velocity;
 	body->set_linear_velocity(v);
+	body->wakeup();
 
 };
 
@@ -743,6 +749,7 @@ void PhysicsServerSW::body_set_axis_lock(RID p_body,BodyAxisLock p_lock) {
 	BodySW *body = body_owner.get(p_body);
 	ERR_FAIL_COND(!body);
 	body->set_axis_lock(p_lock);
+	body->wakeup();
 
 }
 
@@ -762,6 +769,7 @@ void PhysicsServerSW::body_add_collision_exception(RID p_body, RID p_body_b) {
 	ERR_FAIL_COND(!body);
 
 	body->add_exception(p_body_b);
+	body->wakeup();
 
 };
 
@@ -771,6 +779,7 @@ void PhysicsServerSW::body_remove_collision_exception(RID p_body, RID p_body_b)
 	ERR_FAIL_COND(!body);
 
 	body->remove_exception(p_body_b);
+	body->wakeup();
 
 };
 
diff --git a/servers/physics_2d/body_2d_sw.cpp b/servers/physics_2d/body_2d_sw.cpp
index 98dabb4d3c..464b818384 100644
--- a/servers/physics_2d/body_2d_sw.cpp
+++ b/servers/physics_2d/body_2d_sw.cpp
@@ -279,6 +279,7 @@ void Body2DSW::set_state(Physics2DServer::BodyState p_state, const Variant& p_va
 				_set_inv_transform(get_transform().inverse());
 
 			}
+			wakeup();
 
 		} break;
 		case Physics2DServer::BODY_STATE_LINEAR_VELOCITY: {
@@ -286,12 +287,14 @@ void Body2DSW::set_state(Physics2DServer::BodyState p_state, const Variant& p_va
 			//if (mode==Physics2DServer::BODY_MODE_STATIC)
 			//	break;
 			linear_velocity=p_variant;
+			wakeup();
 
 		} break;
 		case Physics2DServer::BODY_STATE_ANGULAR_VELOCITY: {
 			//if (mode!=Physics2DServer::BODY_MODE_RIGID)
 			//	break;
 			angular_velocity=p_variant;
+			wakeup();
 
 		} break;
 		case Physics2DServer::BODY_STATE_SLEEPING: {
diff --git a/servers/physics_2d/body_2d_sw.h b/servers/physics_2d/body_2d_sw.h
index 3b07401dd7..ca4d80a15b 100644
--- a/servers/physics_2d/body_2d_sw.h
+++ b/servers/physics_2d/body_2d_sw.h
@@ -201,6 +201,15 @@ public:
 	void set_active(bool p_active);
 	_FORCE_INLINE_ bool is_active() const { return active; }
 
+	_FORCE_INLINE_ void wakeup() {
+		if ((get_space() && active) || mode==Physics2DServer::BODY_MODE_STATIC || mode==Physics2DServer::BODY_MODE_KINEMATIC)
+			return;
+		set_active(true);
+	}
+
+
+
+
 	void set_param(Physics2DServer::BodyParameter p_param, float);
 	float get_param(Physics2DServer::BodyParameter p_param) const;
 
diff --git a/servers/physics_2d/physics_2d_server_sw.cpp b/servers/physics_2d/physics_2d_server_sw.cpp
index d0a0ff67d7..d31606acfb 100644
--- a/servers/physics_2d/physics_2d_server_sw.cpp
+++ b/servers/physics_2d/physics_2d_server_sw.cpp
@@ -783,6 +783,8 @@ void Physics2DServerSW::body_set_applied_force(RID p_body, const Vector2& p_forc
 	ERR_FAIL_COND(!body);
 
 	body->set_applied_force(p_force);
+	body->wakeup();
+
 };
 
 Vector2 Physics2DServerSW::body_get_applied_force(RID p_body) const {
@@ -798,6 +800,7 @@ void Physics2DServerSW::body_set_applied_torque(RID p_body, float p_torque) {
 	ERR_FAIL_COND(!body);
 
 	body->set_applied_torque(p_torque);
+	body->wakeup();
 };
 
 float Physics2DServerSW::body_get_applied_torque(RID p_body) const {
@@ -814,6 +817,7 @@ void Physics2DServerSW::body_apply_impulse(RID p_body, const Vector2& p_pos, con
 	ERR_FAIL_COND(!body);
 
 	body->apply_impulse(p_pos,p_impulse);
+	body->wakeup();
 };
 
 void Physics2DServerSW::body_set_axis_velocity(RID p_body, const Vector2& p_axis_velocity) {
@@ -826,7 +830,7 @@ void Physics2DServerSW::body_set_axis_velocity(RID p_body, const Vector2& p_axis
 	v-=axis*axis.dot(v);
 	v+=p_axis_velocity;
 	body->set_linear_velocity(v);
-
+	body->wakeup();
 };
 
 void Physics2DServerSW::body_add_collision_exception(RID p_body, RID p_body_b) {
@@ -835,7 +839,7 @@ void Physics2DServerSW::body_add_collision_exception(RID p_body, RID p_body_b) {
 	ERR_FAIL_COND(!body);
 
 	body->add_exception(p_body_b);
-
+	body->wakeup();
 };
 
 void Physics2DServerSW::body_remove_collision_exception(RID p_body, RID p_body_b) {
@@ -844,6 +848,7 @@ void Physics2DServerSW::body_remove_collision_exception(RID p_body, RID p_body_b
 	ERR_FAIL_COND(!body);
 
 	body->remove_exception(p_body_b);
+	body->wakeup();
 
 };