From e6a0206d47adc8abca9110ca8af88fb1befb6c17 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Thu, 4 Jun 2020 00:00:19 -0700 Subject: [PATCH 1/3] Allow to remove soft body anchors, using pybullet.removeConstraint (untested). Usage example: anchors = [] anchors.append(p.createSoftBodyAnchor(clothId ,0,-1,-1)) anchors.append(p.createSoftBodyAnchor(clothId ,3,boxId,-1, [0.5,-0.5,0])) for a in anchors: p.removeConstraint(a) --- .../PhysicsServerCommandProcessor.cpp | 36 ++++++++++++++++++- src/BulletSoftBody/btSoftBody.cpp | 19 ++++++++++ src/BulletSoftBody/btSoftBody.h | 1 + 3 files changed, 55 insertions(+), 1 deletion(-) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 4c2d92e35..b3e1387ec 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -310,9 +310,16 @@ struct InteralUserConstraintData b3UserConstraint m_userConstraintData; + int m_sbHandle; + int m_sbNodeIndex; + btScalar m_sbNodeMass; + InteralUserConstraintData() : m_rbConstraint(0), - m_mbConstraint(0) + m_mbConstraint(0), + m_sbHandle(-1), + m_sbNodeIndex(-1), + m_sbNodeMass(-1) { } }; @@ -11226,8 +11233,13 @@ bool PhysicsServerCommandProcessor::processCreateUserConstraintCommand(const str if (bodyUniqueId<=0) { //fixed anchor (mass = 0) + InteralUserConstraintData userConstraintData; + userConstraintData.m_sbHandle = clientCmd.m_userConstraintArguments.m_parentBodyIndex; + userConstraintData.m_sbNodeIndex = nodeIndex; + userConstraintData.m_sbNodeMass = sbodyHandle->m_softBody->getMass(nodeIndex); sbodyHandle->m_softBody->setMass(nodeIndex,0.0); int uid = m_data->m_userConstraintUIDGenerator++; + m_data->m_userConstraints.insert(uid, userConstraintData); serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid; serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; } else @@ -11278,6 +11290,10 @@ bool PhysicsServerCommandProcessor::processCreateUserConstraintCommand(const str } int uid = m_data->m_userConstraintUIDGenerator++; serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid; + InteralUserConstraintData userConstraintData; + userConstraintData.m_sbHandle = clientCmd.m_userConstraintArguments.m_parentBodyIndex; + userConstraintData.m_sbNodeIndex = nodeIndex; + m_data->m_userConstraints.insert(uid, userConstraintData); serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; } @@ -11740,6 +11756,24 @@ bool PhysicsServerCommandProcessor::processCreateUserConstraintCommand(const str delete userConstraintPtr->m_rbConstraint; m_data->m_userConstraints.remove(userConstraintUidRemove); } + if (userConstraintPtr->m_sbHandle >= 0) + { + InternalBodyHandle* sbodyHandle = m_data->m_bodyHandles.getHandle(clientCmd.m_userConstraintArguments.m_parentBodyIndex); + if (sbodyHandle) + { + if (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); + } + } + } + } serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = userConstraintUidRemove; serverCmd.m_type = CMD_REMOVE_USER_CONSTRAINT_COMPLETED; } diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index f1ab3460f..f28f89ea5 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -558,6 +558,25 @@ void btSoftBody::appendDeformableAnchor(int node, btRigidBody* body) m_deformableAnchors.push_back(c); } +void btSoftBody::removeAnchor(int node) +{ + const btSoftBody::Node& n = m_nodes[node]; + for (int i = 0; i < m_deformableAnchors.size(); ) + { + const DeformableNodeRigidAnchor& c = m_deformableAnchors[i]; + if (c.m_node == &n) + { + m_deformableAnchors.removeAtIndex(i); + } + else + { + i++; + } + } +} + + + // void btSoftBody::appendDeformableAnchor(int node, btMultiBodyLinkCollider* link) { diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h index 954adf4d1..264d71279 100644 --- a/src/BulletSoftBody/btSoftBody.h +++ b/src/BulletSoftBody/btSoftBody.h @@ -927,6 +927,7 @@ public: void appendAnchor(int node, btRigidBody* body, bool disableCollisionBetweenLinkedBodies = false, btScalar influence = 1); void appendAnchor(int node, btRigidBody* body, const btVector3& localPivot, bool disableCollisionBetweenLinkedBodies = false, btScalar influence = 1); + void removeAnchor(int node); /* Append linear joint */ void appendLinearJoint(const LJoint::Specs& specs, Cluster* body0, Body body1); void appendLinearJoint(const LJoint::Specs& specs, Body body = Body()); From e82a4ecc6d18e9567899896c1e4daf3114d179ee Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Thu, 4 Jun 2020 00:09:11 -0700 Subject: [PATCH 2/3] Apply patch to apply force at softbody nodes, thanks to Robert Lee, see https://github.com/bulletphysics/bullet3/issues/2699#issuecomment-610734583 --- .../PhysicsServerCommandProcessor.cpp | 20 +++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index b98f742d4..c03074c7d 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -11005,6 +11005,26 @@ bool PhysicsServerCommandProcessor::processApplyExternalForceCommand(const struc rb->applyTorque(torqueWorld); } } + + if (body && body->m_softBody) + { + btSoftBody* sb = body->m_softBody; + int link = clientCmd.m_externalForceArguments.m_linkIds[i]; + if ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_FORCE) != 0) + { + btVector3 forceLocal(clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 0], + clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 1], + clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 2]); + btVector3 positionLocal( + clientCmd.m_externalForceArguments.m_positions[i * 3 + 0], + clientCmd.m_externalForceArguments.m_positions[i * 3 + 1], + clientCmd.m_externalForceArguments.m_positions[i * 3 + 2]); + + btVector3 forceWorld = isLinkFrame ? forceLocal : sb->getWorldTransform().getBasis() * forceLocal; + btVector3 relPosWorld = isLinkFrame ? positionLocal : sb->getWorldTransform().getBasis() * positionLocal; + sb->addForce(forceWorld, link); + } + } } SharedMemoryStatus& serverCmd = serverStatusOut; From a2cdadb9b48a24b1fc4c07b23226bbeda82b9705 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Thu, 4 Jun 2020 00:24:51 -0700 Subject: [PATCH 3/3] add check to avoid crash, if link is out of bounds. --- examples/SharedMemory/PhysicsServerCommandProcessor.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index c03074c7d..13e6e3ab1 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -11022,7 +11022,10 @@ bool PhysicsServerCommandProcessor::processApplyExternalForceCommand(const struc btVector3 forceWorld = isLinkFrame ? forceLocal : sb->getWorldTransform().getBasis() * forceLocal; btVector3 relPosWorld = isLinkFrame ? positionLocal : sb->getWorldTransform().getBasis() * positionLocal; - sb->addForce(forceWorld, link); + if (link >= 0 && link < sb->m_nodes.size()) + { + sb->addForce(forceWorld, link); + } } } }