From 26019322496936df5fc76c1bb6ff1dd824d001d5 Mon Sep 17 00:00:00 2001
From: erwincoumans <erwin.coumans@gmail.com>
Date: Sat, 17 May 2014 14:24:22 -0700
Subject: [PATCH] fix issue related to addConstraint/removeConstraint,
 introduced by filtering collision between two particular bodies
 https://github.com/bulletphysics/bullet3/issues/173

---
 .../Dynamics/btDiscreteDynamicsWorld.cpp      |  3 ++
 src/BulletDynamics/Dynamics/btRigidBody.cpp   | 30 ++++++++++---------
 2 files changed, 19 insertions(+), 14 deletions(-)

diff --git a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
index fb8a4068e..df57f893c 100644
--- a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
+++ b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
@@ -653,6 +653,9 @@ void	btDiscreteDynamicsWorld::updateActivationState(btScalar timeStep)
 void	btDiscreteDynamicsWorld::addConstraint(btTypedConstraint* constraint,bool disableCollisionsBetweenLinkedBodies)
 {
 	m_constraints.push_back(constraint);
+    //Make sure the two bodies of a type constraint are different (possibly add this to the btTypedConstraint constructor?)
+    btAssert(&constraint->getRigidBodyA()!=&constraint->getRigidBodyB());
+    
 	if (disableCollisionsBetweenLinkedBodies)
 	{
 		constraint->getRigidBodyA().addConstraintRef(constraint);
diff --git a/src/BulletDynamics/Dynamics/btRigidBody.cpp b/src/BulletDynamics/Dynamics/btRigidBody.cpp
index aad5089ed..732d75963 100644
--- a/src/BulletDynamics/Dynamics/btRigidBody.cpp
+++ b/src/BulletDynamics/Dynamics/btRigidBody.cpp
@@ -326,7 +326,7 @@ void btRigidBody::addConstraintRef(btTypedConstraint* c)
 
 	int index = m_constraintRefs.findLinearSearch(c);
 	//don't add constraints that are already referenced
-	btAssert(index == m_constraintRefs.size());
+	//btAssert(index == m_constraintRefs.size());
 	if (index == m_constraintRefs.size())
 	{
 		m_constraintRefs.push_back(c);
@@ -340,25 +340,27 @@ void btRigidBody::addConstraintRef(btTypedConstraint* c)
 		{
 			colObjB->setIgnoreCollisionCheck(colObjA, true);
 		}
-	}
+	} 
 }
 
 void btRigidBody::removeConstraintRef(btTypedConstraint* c)
 {
 	int index = m_constraintRefs.findLinearSearch(c);
 	//don't remove constraints that are not referenced
-	btAssert(index < m_constraintRefs.size());
-	m_constraintRefs.remove(c);
-	btCollisionObject* colObjA = &c->getRigidBodyA();
-	btCollisionObject* colObjB = &c->getRigidBodyB();
-	if (colObjA == this)
-	{
-		colObjA->setIgnoreCollisionCheck(colObjB, false);
-	}
-	else
-	{
-		colObjB->setIgnoreCollisionCheck(colObjA, false);
-	}
+	if(index < m_constraintRefs.size())
+    {
+        m_constraintRefs.remove(c);
+        btCollisionObject* colObjA = &c->getRigidBodyA();
+        btCollisionObject* colObjB = &c->getRigidBodyB();
+        if (colObjA == this)
+        {
+            colObjA->setIgnoreCollisionCheck(colObjB, false);
+        }
+        else
+        {
+            colObjB->setIgnoreCollisionCheck(colObjA, false);
+        }
+    }
 }
 
 int	btRigidBody::calculateSerializeBufferSize()	const