5 files changed, 22 insertions, 7 deletions
diff --git a/servers/physics/body_pair_sw.cpp b/servers/physics/body_pair_sw.cpp
index d112afa8e2..bca6a9fa72 100644
--- a/servers/physics/body_pair_sw.cpp
+++ b/servers/physics/body_pair_sw.cpp
@@ -175,7 +175,7 @@ void BodyPairSW::validate_contacts() {
 bool BodyPairSW::setup(float p_step) {
 
 	//cannot collide
-	if (A->has_exception(B->get_self()) || B->has_exception(A->get_self()) || (A->get_mode()<=PhysicsServer::BODY_MODE_KINEMATIC && B->get_mode()<=PhysicsServer::BODY_MODE_KINEMATIC)) {
+	if (A->has_exception(B->get_self()) || B->has_exception(A->get_self()) || (A->get_mode()<=PhysicsServer::BODY_MODE_KINEMATIC && B->get_mode()<=PhysicsServer::BODY_MODE_KINEMATIC && A->get_max_contacts_reported()==0 && B->get_max_contacts_reported()==0)) {
 		collided=false;
 		return false;
 	}
@@ -267,6 +267,14 @@ bool BodyPairSW::setup(float p_step) {
 			B->add_contact(global_B,c.normal,depth,shape_B,global_A,shape_A,A->get_instance_id(),A->get_self(),crA);
 		}
 
+		if (A->is_shape_set_as_trigger(shape_A) || B->is_shape_set_as_trigger(shape_B) || (A->get_mode()<=PhysicsServer::BODY_MODE_KINEMATIC && B->get_mode()<=PhysicsServer::BODY_MODE_KINEMATIC)) {
+			c.active=false;
+			collided=false;
+			continue;
+
+		}
+
+
 		c.active=true;
 
 		// Precompute normal mass, tangent mass, and bias.
diff --git a/servers/physics/body_sw.cpp b/servers/physics/body_sw.cpp
index 0fd754ba25..73441882fb 100644
--- a/servers/physics/body_sw.cpp
+++ b/servers/physics/body_sw.cpp
@@ -196,6 +196,7 @@ void BodySW::set_mode(PhysicsServer::BodyMode p_mode) {
 			_inv_mass=0;
 			_set_static(p_mode==PhysicsServer::BODY_MODE_STATIC);
 			//set_active(p_mode==PhysicsServer::BODY_MODE_KINEMATIC);
+			set_active(p_mode==PhysicsServer::BODY_MODE_KINEMATIC && contacts.size());
 			linear_velocity=Vector3();
 			angular_velocity=Vector3();
 		} break;
@@ -460,8 +461,8 @@ void BodySW::integrate_velocities(real_t p_step) {
 	if (mode==PhysicsServer::BODY_MODE_KINEMATIC) {
 
 		_set_transform(new_transform,false);
-		_set_inv_transform(new_transform.affine_inverse());				;
-		if (linear_velocity==Vector3() && angular_velocity==Vector3())
+		_set_inv_transform(new_transform.affine_inverse());
+		if (contacts.size()==0 && linear_velocity==Vector3() && angular_velocity==Vector3())
 			set_active(false); //stopped moving, deactivate
 
 		return;
diff --git a/servers/physics/body_sw.h b/servers/physics/body_sw.h
index 6317186d5f..f152c4754a 100644
--- a/servers/physics/body_sw.h
+++ b/servers/physics/body_sw.h
@@ -137,7 +137,7 @@ public:
 	_FORCE_INLINE_ void add_area(AreaSW *p_area) { areas.insert(AreaCMP(p_area)); }
 	_FORCE_INLINE_ void remove_area(AreaSW *p_area) { areas.erase(AreaCMP(p_area)); }
 
-	_FORCE_INLINE_ void set_max_contacts_reported(int p_size) { contacts.resize(p_size); contact_count=0; }
+	_FORCE_INLINE_ void set_max_contacts_reported(int p_size) { contacts.resize(p_size); contact_count=0; if (mode==PhysicsServer::BODY_MODE_KINEMATIC && p_size) set_active(true);}
 	_FORCE_INLINE_ int get_max_contacts_reported() const { return contacts.size(); }
 
 	_FORCE_INLINE_ bool can_report_contacts() const { return !contacts.empty(); }
diff --git a/servers/physics/collision_object_sw.h b/servers/physics/collision_object_sw.h
index 98d05538a8..788292ad2a 100644
--- a/servers/physics/collision_object_sw.h
+++ b/servers/physics/collision_object_sw.h
@@ -59,6 +59,9 @@ private:
 		BroadPhaseSW::ID bpid;
 		AABB aabb_cache; //for rayqueries
 		ShapeSW *shape;
+		bool trigger;
+
+		Shape() { trigger=false; }
 	};
 
 	Vector<Shape> shapes;
@@ -125,6 +128,8 @@ public:
 	_FORCE_INLINE_ void set_ray_pickable(bool p_enable) { ray_pickable=p_enable; }
 	_FORCE_INLINE_ bool is_ray_pickable() const { return ray_pickable; }
 
+	_FORCE_INLINE_ void set_shape_as_trigger(int p_idx,bool p_enable) { shapes[p_idx].trigger=p_enable; }
+	_FORCE_INLINE_ bool is_shape_set_as_trigger(int p_idx) const { return shapes[p_idx].trigger; }
 
 	_FORCE_INLINE_ void set_layer_mask(uint32_t p_mask) { layer_mask=p_mask; }
 	_FORCE_INLINE_ uint32_t get_layer_mask() const { return layer_mask; }
diff --git a/servers/physics/physics_server_sw.cpp b/servers/physics/physics_server_sw.cpp
index 31c4d8f121..510a6ea93f 100644
--- a/servers/physics/physics_server_sw.cpp
+++ b/servers/physics/physics_server_sw.cpp
@@ -540,6 +540,8 @@ void PhysicsServerSW::body_set_shape_as_trigger(RID p_body, int p_shape_idx,bool
 
 	BodySW *body = body_owner.get(p_body);
 	ERR_FAIL_COND(!body);
+	ERR_FAIL_INDEX(p_shape_idx,body->get_shape_count());
+	body->set_shape_as_trigger(p_shape_idx,p_enable);
 
 }
 
@@ -547,10 +549,9 @@ bool PhysicsServerSW::body_is_shape_set_as_trigger(RID p_body, int p_shape_idx)
 
 	BodySW *body = body_owner.get(p_body);
 	ERR_FAIL_COND_V(!body,false);
+	ERR_FAIL_INDEX_V(p_shape_idx,body->get_shape_count(),false);
 
-	// todo ?
-
-	return false;
+	body->is_shape_set_as_trigger(p_shape_idx);
 }