Skip to content

Clean up associated user constraints when removing a body (#4808) #4813

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
Tyagiquamar wants to merge 1 commit into
base: master
Choose a base branch
from
Open

Clean up associated user constraints when removing a body (#4808) #4813

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 4 additions & 1 deletion examples/SharedMemory/PhysicsClientSharedMemory.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1744,7 +1744,10 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus()
for (int i = 0; i < serverCmd.m_removeObjectArgs.m_numUserConstraints; i++)
{
int key = serverCmd.m_removeObjectArgs.m_userConstraintUniqueIds[i];
m_data->m_userConstraintInfoMap.remove(key);
if (key >= 0)
{
m_data->m_userConstraintInfoMap.remove(key);
}
}
}

Expand Down
5 changes: 4 additions & 1 deletion examples/SharedMemory/PhysicsDirect.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -876,7 +876,10 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd
for (int i = 0; i < serverCmd.m_removeObjectArgs.m_numUserConstraints; i++)
{
int key = serverCmd.m_removeObjectArgs.m_userConstraintUniqueIds[i];
m_data->m_userConstraintInfoMap.remove(key);
if (key >= 0)
{
m_data->m_userConstraintInfoMap.remove(key);
}
}

break;
Expand Down
140 changes: 114 additions & 26 deletions examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -12736,6 +12736,117 @@ bool PhysicsServerCommandProcessor::processApplyExternalForceCommand(const struc
return hasStatus;
}

void PhysicsServerCommandProcessor::removeUserConstraintsForBody(int bodyUniqueId, struct SharedMemoryStatus& serverCmd)
{
InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(bodyUniqueId);
if (!bodyHandle)
{
return;
}

#ifndef USE_DISCRETE_DYNAMICS_WORLD
if (bodyHandle->m_multiBody)
{
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);
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);
if (serverCmd.m_removeObjectArgs.m_numUserConstraints < MAX_SDF_BODIES)
{
serverCmd.m_removeObjectArgs.m_userConstraintUniqueIds[serverCmd.m_removeObjectArgs.m_numUserConstraints++] = userConstraintKey;
}
}
}
delete mbc;
}
}
}
#endif

if (bodyHandle->m_rigidBody)
{
for (int i = m_data->m_dynamicsWorld->getNumConstraints() - 1; i >= 0; i--)
{
btTypedConstraint* constraint = m_data->m_dynamicsWorld->getConstraint(i);
if ((&constraint->getRigidBodyA() == bodyHandle->m_rigidBody) || (&constraint->getRigidBodyB() == bodyHandle->m_rigidBody))
{
m_data->m_dynamicsWorld->removeConstraint(constraint);
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_rbConstraint == constraint)
{
m_data->m_userConstraints.remove(userConstraintKey);
if (serverCmd.m_removeObjectArgs.m_numUserConstraints < MAX_SDF_BODIES)
{
serverCmd.m_removeObjectArgs.m_userConstraintUniqueIds[serverCmd.m_removeObjectArgs.m_numUserConstraints++] = userConstraintKey;
}
}
}
delete constraint;
}
}
}

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_userConstraintData.m_parentBodyIndex == bodyUniqueId ||
userConstraintPtr->m_userConstraintData.m_childBodyIndex == bodyUniqueId ||
userConstraintPtr->m_sbHandle == bodyUniqueId)
{
if (userConstraintPtr->m_rbConstraint)
{
m_data->m_dynamicsWorld->removeConstraint(userConstraintPtr->m_rbConstraint);
delete userConstraintPtr->m_rbConstraint;
userConstraintPtr->m_rbConstraint = 0;
}
#ifndef USE_DISCRETE_DYNAMICS_WORLD
if (userConstraintPtr->m_mbConstraint)
{
m_data->m_dynamicsWorld->removeMultiBodyConstraint(userConstraintPtr->m_mbConstraint);
delete userConstraintPtr->m_mbConstraint;
userConstraintPtr->m_mbConstraint = 0;
}
#endif
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
if (userConstraintPtr->m_sbHandle >= 0)
{
InternalBodyHandle* sbodyHandle = m_data->m_bodyHandles.getHandle(userConstraintPtr->m_sbHandle);
if (sbodyHandle && sbodyHandle->m_softBody)
{
if (userConstraintPtr->m_sbNodeMass >= 0)
{
sbodyHandle->m_softBody->setMass(userConstraintPtr->m_sbNodeIndex, userConstraintPtr->m_sbNodeMass);
}
else
{
sbodyHandle->m_softBody->removeAnchor(userConstraintPtr->m_sbNodeIndex);
}
}
}
#endif
m_data->m_userConstraints.remove(userConstraintKey);
if (serverCmd.m_removeObjectArgs.m_numUserConstraints < MAX_SDF_BODIES)
{
serverCmd.m_removeObjectArgs.m_userConstraintUniqueIds[serverCmd.m_removeObjectArgs.m_numUserConstraints++] = userConstraintKey;
}
}
}
}

bool PhysicsServerCommandProcessor::processRemoveBodyCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
bool hasStatus = true;
Expand All @@ -12753,6 +12864,8 @@ bool PhysicsServerCommandProcessor::processRemoveBodyCommand(const struct Shared
InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(bodyUniqueId);
if (bodyHandle)
{
removeUserConstraintsForBody(bodyUniqueId, serverCmd);

#ifndef USE_DISCRETE_DYNAMICS_WORLD
if (bodyHandle->m_multiBody)
{
Expand All @@ -12764,31 +12877,6 @@ bool PhysicsServerCommandProcessor::processRemoveBodyCommand(const struct Shared
m_data->m_pickingMultiBodyPoint2Point = 0;
}

//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())
{
if (m_data->m_pluginManager.getRenderInterface())
Expand Down Expand Up @@ -12838,7 +12926,6 @@ bool PhysicsServerCommandProcessor::processRemoveBodyCommand(const struct Shared
m_data->m_pickedBody = 0;
}

//todo: clear all other remaining data...
m_data->m_dynamicsWorld->removeRigidBody(bodyHandle->m_rigidBody);
int graphicsInstance = bodyHandle->m_rigidBody->getUserIndex2();
m_data->m_guiHelper->removeGraphicsInstance(graphicsInstance);
Expand Down Expand Up @@ -12868,6 +12955,7 @@ bool PhysicsServerCommandProcessor::processRemoveBodyCommand(const struct Shared

int graphicsInstance = psb->getUserIndex2();
m_data->m_guiHelper->removeGraphicsInstance(graphicsInstance);

delete psb;
psb = 0;
serverCmd.m_type = CMD_REMOVE_BODY_COMPLETED;
Expand Down
1 change: 1 addition & 0 deletions examples/SharedMemory/PhysicsServerCommandProcessor.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -119,6 +119,7 @@ class PhysicsServerCommandProcessor : public CommandProcessorInterface
void deleteCachedInverseDynamicsBodies();
void deleteCachedInverseKinematicsBodies();
void deleteStateLoggers();
void removeUserConstraintsForBody(int bodyUniqueId, struct SharedMemoryStatus& serverCmd);

public:
PhysicsServerCommandProcessor();
Expand Down
2 changes: 1 addition & 1 deletion examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1429,7 +1429,7 @@ bool b3RobotSimulatorClientAPI_NoDirect::removeBody(int bodyUniqueId)
}
else
{
b3Warning("getDynamicsInfo did not complete");
b3Warning("removeBody did not complete");
return false;
}
}
Expand Down
20 changes: 20 additions & 0 deletions test/SharedMemory/test.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -282,6 +282,26 @@ void testSharedMemory(b3PhysicsClientHandle sm)
sensorState.m_jointForceTorque[2]);
}

{
b3SharedMemoryStatusHandle statusHandle;
int statusType;
int bodyIndicesOut[10];
int numBodies;
b3SharedMemoryCommandHandle command = b3LoadSdfCommandInit(sm, sdfFileName);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
statusType = b3GetStatusType(statusHandle);
ASSERT_EQ(statusType, CMD_SDF_LOADING_COMPLETED);
numBodies = b3GetStatusBodyIndices(statusHandle, bodyIndicesOut, 10);
if (numBodies > 0)
{
int testBody = bodyIndicesOut[0];
b3SharedMemoryCommandHandle removeCmd = b3InitRemoveBodyCommand(sm, testBody);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, removeCmd);
statusType = b3GetStatusType(statusHandle);
ASSERT_EQ(statusType, CMD_REMOVE_BODY_COMPLETED);
}
}

{
b3SharedMemoryStatusHandle statusHandle;
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, b3InitResetSimulationCommand(sm));
Expand Down