diff --git a/data/multibody.bullet b/data/multibody.bullet index 4cb146f2f..f71d00473 100644 Binary files a/data/multibody.bullet and b/data/multibody.bullet differ diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index 29df1e06e..27262685f 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -1097,6 +1097,11 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { int bodyUniqueId = serverCmd.m_removeObjectArgs.m_bodyUniqueIds[i]; removeCachedBody(bodyUniqueId); } + for (int i=0;im_userConstraintInfoMap.remove(key); + } } if (serverCmd.m_type == CMD_USER_CONSTRAINT_INFO_COMPLETED) diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index e3384d871..8a5ec6c8d 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/examples/SharedMemory/PhysicsDirect.cpp @@ -736,6 +736,12 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd int bodyUniqueId = serverCmd.m_removeObjectArgs.m_bodyUniqueIds[i]; removeCachedBody(bodyUniqueId); } + for (int i=0;im_userConstraintInfoMap.remove(key); + } + break; } case CMD_CHANGE_USER_CONSTRAINT_COMPLETED: diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 6f43c99ec..96ab8fd29 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -4941,19 +4941,48 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm if (bodyHandle->m_multiBody) { serverCmd.m_removeObjectArgs.m_bodyUniqueIds[serverCmd.m_removeObjectArgs.m_numBodies++] = bodyUniqueId; + + //also remove user constraints... + for (int i=m_data->m_dynamicsWorld->getNumMultiBodyConstraints()-1;i>=0;i--) + { + btMultiBodyConstraint* mbc = m_data->m_dynamicsWorld->getMultiBodyConstraint(i); + if ((mbc->getMultiBodyA() == bodyHandle->m_multiBody)||(mbc->getMultiBodyB()==bodyHandle->m_multiBody)) + { + m_data->m_dynamicsWorld->removeMultiBodyConstraint(mbc); + + //also remove user constraint and submit it as removed + for (int c=m_data->m_userConstraints.size()-1;c>=0;c--) + { + InteralUserConstraintData* userConstraintPtr = m_data->m_userConstraints.getAtIndex(c); + int userConstraintKey = m_data->m_userConstraints.getKeyAtIndex(c).getUid1(); + + if (userConstraintPtr->m_mbConstraint == mbc) + { + m_data->m_userConstraints.remove(userConstraintKey); + serverCmd.m_removeObjectArgs.m_userConstraintUniqueIds[serverCmd.m_removeObjectArgs.m_numUserConstraints++]=userConstraintKey; + } + } + + delete mbc; + + + } + } + if (bodyHandle->m_multiBody->getBaseCollider()) { + m_data->m_dynamicsWorld->removeCollisionObject(bodyHandle->m_multiBody->getBaseCollider()); int graphicsIndex = bodyHandle->m_multiBody->getBaseCollider()->getUserIndex(); m_data->m_guiHelper->removeGraphicsInstance(graphicsIndex); - m_data->m_dynamicsWorld->removeCollisionObject(bodyHandle->m_multiBody->getBaseCollider()); } for (int link=0;linkm_multiBody->getNumLinks();link++) { + if (bodyHandle->m_multiBody->getLink(link).m_collider) { + m_data->m_dynamicsWorld->removeCollisionObject(bodyHandle->m_multiBody->getLink(link).m_collider); int graphicsIndex = bodyHandle->m_multiBody->getLink(link).m_collider->getUserIndex(); m_data->m_guiHelper->removeGraphicsInstance(graphicsIndex); - m_data->m_dynamicsWorld->removeCollisionObject(bodyHandle->m_multiBody->getLink(link).m_collider); } } int numCollisionObjects = m_data->m_dynamicsWorld->getNumCollisionObjects(); @@ -4961,6 +4990,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm numCollisionObjects = m_data->m_dynamicsWorld->getNumCollisionObjects(); //todo: clear all other remaining data, release memory etc + delete bodyHandle->m_multiBody; + bodyHandle->m_multiBody=0; serverCmd.m_type = CMD_REMOVE_BODY_COMPLETED; } if (bodyHandle->m_rigidBody) @@ -4970,6 +5001,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm m_data->m_dynamicsWorld->removeRigidBody(bodyHandle->m_rigidBody); int graphicsInstance = bodyHandle->m_rigidBody->getUserIndex2(); m_data->m_guiHelper->removeGraphicsInstance(graphicsInstance); + delete bodyHandle->m_rigidBody; + bodyHandle->m_rigidBody=0; serverCmd.m_type = CMD_REMOVE_BODY_COMPLETED; } } diff --git a/src/Bullet3Common/b3ResizablePool.h b/src/Bullet3Common/b3ResizablePool.h index ff28c2218..06ad8a778 100644 --- a/src/Bullet3Common/b3ResizablePool.h +++ b/src/Bullet3Common/b3ResizablePool.h @@ -167,11 +167,13 @@ public: { b3Assert(handle >= 0); - getHandleInternal(handle)->clear(); - getHandleInternal(handle)->setNextFree(m_firstFreeHandle); - m_firstFreeHandle = handle; - - m_numUsedHandles--; + if (m_bodyHandles[handle].getNextFree()==B3_POOL_HANDLE_TERMINAL_USED) + { + getHandleInternal(handle)->clear(); + getHandleInternal(handle)->setNextFree(m_firstFreeHandle); + m_firstFreeHandle = handle; + m_numUsedHandles--; + } } }; ///end handle management