BGE: Fix T43215: Rigidbody constraints deletion.

Deleting an object was deleting all rigidbody constraints in the scene.
Bug was introduced with D701.

Reviewers: agoose77, sergof, moguri, lordloki, sybren

Reviewed By: lordloki, sybren

Subscribers: sybren, hbar, blueprintrandom, sergof, agoose77

Differential Revision: https://developer.blender.org/D1007

3 files changed, 41 insertions, 3 deletions

diff --git a/source/gameengine/Physics/Bullet/CcdPhysicsController.cpp b/source/gameengine/Physics/Bullet/CcdPhysicsController.cpp
index 08fcb243445..44c4e284e7c 100644
--- a/source/gameengine/Physics/Bullet/CcdPhysicsController.cpp
+++ b/source/gameengine/Physics/Bullet/CcdPhysicsController.cpp
@@ -154,6 +154,28 @@ CcdPhysicsController::CcdPhysicsController (const CcdConstructionInfo& ci)
 	CreateRigidbody();
 }
 
+void CcdPhysicsController::addCcdConstraintRef(btTypedConstraint* c)
+{
+	int index = m_ccdConstraintRefs.findLinearSearch(c);
+	if (index == m_ccdConstraintRefs.size())
+		m_ccdConstraintRefs.push_back(c);
+}
+
+void CcdPhysicsController::removeCcdConstraintRef(btTypedConstraint* c)
+{
+	m_ccdConstraintRefs.remove(c);
+}
+
+btTypedConstraint* CcdPhysicsController::getCcdConstraintRef(int index)
+{
+	return m_ccdConstraintRefs[index];
+}
+
+int CcdPhysicsController::getNumCcdConstraintRefs() const
+{
+	return m_ccdConstraintRefs.size();
+}
+
 btTransform&	CcdPhysicsController::GetTransformFromMotionState(PHY_IMotionState* motionState)
 {
 	static btTransform trans;
diff --git a/source/gameengine/Physics/Bullet/CcdPhysicsController.h b/source/gameengine/Physics/Bullet/CcdPhysicsController.h
index be393bbfa7f..de40624d7bb 100644
--- a/source/gameengine/Physics/Bullet/CcdPhysicsController.h
+++ b/source/gameengine/Physics/Bullet/CcdPhysicsController.h
@@ -461,6 +461,7 @@ protected:
 	class CcdShapeConstructionInfo* m_shapeInfo;
 	btCollisionShape* m_bulletChildShape;
 
+	btAlignedObjectArray<btTypedConstraint*> m_ccdConstraintRefs; // keep track of typed constraints referencing this rigid body
 	friend class CcdPhysicsEnvironment;	// needed when updating the controller
 
 	//some book keeping for replication
@@ -497,6 +498,11 @@ protected:
 		return (--m_registerCount == 0) ? true : false;
 	}
 
+	void addCcdConstraintRef(btTypedConstraint* c);
+	void removeCcdConstraintRef(btTypedConstraint* c);
+	btTypedConstraint* getCcdConstraintRef(int index);
+	int getNumCcdConstraintRefs() const;
+
 	void SetWorldOrientation(const btMatrix3x3& mat);
 	void ForceWorldTransform(const btMatrix3x3& mat, const btVector3& pos);
 
diff --git a/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp b/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp
index ac132a509c1..557b52978ef 100644
--- a/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp
+++ b/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp
@@ -509,11 +509,13 @@ bool	CcdPhysicsEnvironment::RemoveCcdPhysicsController(CcdPhysicsController* ctr
 	btRigidBody* body = ctrl->GetRigidBody();
 	if (body)
 	{
-		for (int i = m_dynamicsWorld->getNumConstraints()-1;i>=0;i--)
+		for (int i = ctrl->getNumCcdConstraintRefs() - 1; i >= 0; i--)
 		{
-			btTypedConstraint *con = m_dynamicsWorld->getConstraint(i);
+			btTypedConstraint* con = ctrl->getCcdConstraintRef(i);
+			con->getRigidBodyA().activate();
+			con->getRigidBodyB().activate();
 			m_dynamicsWorld->removeConstraint(con);
-			body->removeConstraintRef(con);
+			ctrl->removeCcdConstraintRef(con);
 			//delete con; //might be kept by python KX_ConstraintWrapper
 		}
 		m_dynamicsWorld->removeRigidBody(ctrl->GetRigidBody());
@@ -2667,6 +2669,8 @@ int			CcdPhysicsEnvironment::CreateConstraint(class PHY_IPhysicsController* ctrl
 					pivotInA);
 			}
 
+			c0->addCcdConstraintRef(p2p);
+			c1->addCcdConstraintRef(p2p);
 			m_dynamicsWorld->addConstraint(p2p,disableCollisionBetweenLinkedBodies);
 //			m_constraints.push_back(p2p);
 
@@ -2737,6 +2741,8 @@ int			CcdPhysicsEnvironment::CreateConstraint(class PHY_IPhysicsController* ctrl
 			if (genericConstraint)
 			{
 				//m_constraints.push_back(genericConstraint);
+				c0->addCcdConstraintRef(genericConstraint);
+				c1->addCcdConstraintRef(genericConstraint);
 				m_dynamicsWorld->addConstraint(genericConstraint,disableCollisionBetweenLinkedBodies);
 				genericConstraint->setUserConstraintId(gConstraintUid++);
 				genericConstraint->setUserConstraintType(type);
@@ -2803,6 +2809,8 @@ int			CcdPhysicsEnvironment::CreateConstraint(class PHY_IPhysicsController* ctrl
 			if (coneTwistContraint)
 			{
 				//m_constraints.push_back(genericConstraint);
+				c0->addCcdConstraintRef(coneTwistContraint);
+				c1->addCcdConstraintRef(coneTwistContraint);
 				m_dynamicsWorld->addConstraint(coneTwistContraint,disableCollisionBetweenLinkedBodies);
 				coneTwistContraint->setUserConstraintId(gConstraintUid++);
 				coneTwistContraint->setUserConstraintType(type);
@@ -2876,6 +2884,8 @@ int			CcdPhysicsEnvironment::CreateConstraint(class PHY_IPhysicsController* ctrl
 			hinge->setAngularOnly(angularOnly);
 
 			//m_constraints.push_back(hinge);
+			c0->addCcdConstraintRef(hinge);
+			c1->addCcdConstraintRef(hinge);
 			m_dynamicsWorld->addConstraint(hinge,disableCollisionBetweenLinkedBodies);
 			hinge->setUserConstraintId(gConstraintUid++);
 			hinge->setUserConstraintType(type);