From fb8fba9170f514675ee8ce5ef02c9b53b1579282 Mon Sep 17 00:00:00 2001
From: Otto Naderer <otto.naderer@aec.at>
Date: Thu, 20 Mar 2014 15:28:54 +0100
Subject: [PATCH] added correct constraint removal

---
 include/irrDynamics.h |  1 +
 src/irrDynamics.cpp   | 25 +++++++++++++++++++++++++
 2 files changed, 26 insertions(+)

diff --git a/include/irrDynamics.h b/include/irrDynamics.h
index 8a556c3..fbc5aa6 100644
--- a/include/irrDynamics.h
+++ b/include/irrDynamics.h
@@ -100,6 +100,7 @@ class irrDynamics
     ~irrDynamics();
     static irrDynamics* instance;
     void clearObjects();
+    void removeConstraints(btRigidBody* rigidBody);
 
     //bt-related stuff
     class btDefaultCollisionConfiguration *collisionConfiguration;
diff --git a/src/irrDynamics.cpp b/src/irrDynamics.cpp
index 09eede0..aab6f2f 100644
--- a/src/irrDynamics.cpp
+++ b/src/irrDynamics.cpp
@@ -138,6 +138,7 @@ void irrDynamics::removeObject(scene::ISceneNode* node)
     std::map<scene::ISceneNode*, btRigidBody*>::iterator iter = inst->objects.find(node);
     if (iter != inst->objects.end())
     {
+        inst->removeConstraints(iter->second);
         inst->world->removeRigidBody(iter->second);
 
         // Free memory
@@ -157,6 +158,7 @@ void irrDynamics::clearObjects()
 {
     for (std::map<scene::ISceneNode*, btRigidBody*>::iterator iter = objects.begin(); iter != objects.end(); iter++)
     {
+        removeConstraints(iter->second);
         world->removeRigidBody(iter->second);
 
         // Free memory
@@ -407,3 +409,26 @@ void irrDynamics::setPosition(scene::ISceneNode* node, const core::vector3df& ne
     transform.setOrigin(btVector3(newPos.X, newPos.Y, newPos.Z));
     iter->second->setCenterOfMassTransform(transform);
 }
+
+void irrDynamics::removeConstraints(btRigidBody* rigidBody)
+{
+    const int len = rigidBody->getNumConstraintRefs();
+    //first, remove constraint reference at the other node
+    for (int i = 0; i < len; i++)
+    {
+        btTypedConstraint* constraint = rigidBody->getConstraintRef(i);
+        btRigidBody* bodyB = &(constraint->getRigidBodyB());
+        if (bodyB == rigidBody)
+            bodyB = &(constraint->getRigidBodyA());
+
+        bodyB->removeConstraintRef(constraint);
+    }
+    //then remove/delete at our side
+    while (rigidBody->getNumConstraintRefs() > 0)
+    {
+        btTypedConstraint* constraint = rigidBody->getConstraintRef(0);
+        world->removeConstraint(constraint);
+        rigidBody->removeConstraintRef(constraint);
+        delete constraint;
+    }
+}