From d38ea87027d4d28a36fc294d47b5a395383bd2f2 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Fri, 13 Dec 2019 14:33:54 -0800 Subject: [PATCH 1/4] add gripper with deformable cloth demo --- examples/ExampleBrowser/ExampleEntries.cpp | 1 + .../RoboticsLearning/GripperGraspExample.cpp | 129 +++++++++++++++++- .../RoboticsLearning/GripperGraspExample.h | 1 + .../b3RobotSimulatorClientAPI_NoDirect.cpp | 36 +++++ .../b3RobotSimulatorClientAPI_NoDirect.h | 53 +++++++ .../btDeformableMultiBodyConstraintSolver.cpp | 24 ++++ 6 files changed, 239 insertions(+), 5 deletions(-) diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index 6bc8b589b..e59641d18 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -303,6 +303,7 @@ static ExampleEntry gDefaultExamples[] = ExampleEntry(1, "Rolling friction", "Experiment on multibody rolling friction", R2D2GraspExampleCreateFunc, eROBOTIC_LEARN_ROLLING_FRICTION), ExampleEntry(1, "Gripper Grasp", "Grasp experiment with a gripper to improve contact model", GripperGraspExampleCreateFunc, eGRIPPER_GRASP), ExampleEntry(1, "Two Point Grasp", "Grasp experiment with two point contact to test rolling friction", GripperGraspExampleCreateFunc, eTWO_POINT_GRASP), + ExampleEntry(1, "Grasp Deformable Cloth", "Grasp experiment with deformable cloth", GripperGraspExampleCreateFunc, eGRASP_DEFORMABLE_CLOTH), ExampleEntry(1, "One Motor Gripper Grasp", "Grasp experiment with a gripper with one motor to test slider constraint for closed loop structure", GripperGraspExampleCreateFunc, eONE_MOTOR_GRASP), #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD ExampleEntry(1, "Grasp Soft Body", "Grasp soft body experiment", GripperGraspExampleCreateFunc, eGRASP_SOFT_BODY), diff --git a/examples/RoboticsLearning/GripperGraspExample.cpp b/examples/RoboticsLearning/GripperGraspExample.cpp index 8f5ec1a8e..c3afaee88 100644 --- a/examples/RoboticsLearning/GripperGraspExample.cpp +++ b/examples/RoboticsLearning/GripperGraspExample.cpp @@ -294,6 +294,102 @@ public: slider.m_maxVal = 1; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } + { + b3RobotSimulatorLoadFileResults results; + m_robotSim.loadSDF("gripper/wsg50_one_motor_gripper_new.sdf", results); + + if (results.m_uniqueObjectIds.size() == 1) + { + m_gripperIndex = results.m_uniqueObjectIds[0]; + int numJoints = m_robotSim.getNumJoints(m_gripperIndex); + b3Printf("numJoints = %d", numJoints); + + for (int i = 0; i < numJoints; i++) + { + b3JointInfo jointInfo; + m_robotSim.getJointInfo(m_gripperIndex, i, &jointInfo); + b3Printf("joint[%d].m_jointName=%s", i, jointInfo.m_jointName); + } + + for (int i = 0; i < 8; i++) + { + b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_VELOCITY); + controlArgs.m_maxTorqueValue = 0.0; + m_robotSim.setJointMotorControl(m_gripperIndex, i, controlArgs); + } + } + } + { + b3RobotSimulatorLoadUrdfFileArgs args; + args.m_startPosition.setValue(0, 0, -0.2); + args.m_startOrientation.setEulerZYX(0, 0, 0); + m_robotSim.loadURDF("plane.urdf", args); + } + m_robotSim.setGravity(btVector3(0, 0, -10)); + b3RobotSimulatorLoadSoftBodyArgs args(0.1, 1, 0.02); + args.m_startPosition.setValue(0, 0, 5); + args.m_startOrientation.setValue(1, 0, 0, 1); + m_robotSim.loadSoftBody("bunny.obj", args); + + b3JointInfo revoluteJoint1; + revoluteJoint1.m_parentFrame[0] = -0.055; + revoluteJoint1.m_parentFrame[1] = 0; + revoluteJoint1.m_parentFrame[2] = 0.02; + revoluteJoint1.m_parentFrame[3] = 0; + revoluteJoint1.m_parentFrame[4] = 0; + revoluteJoint1.m_parentFrame[5] = 0; + revoluteJoint1.m_parentFrame[6] = 1.0; + revoluteJoint1.m_childFrame[0] = 0; + revoluteJoint1.m_childFrame[1] = 0; + revoluteJoint1.m_childFrame[2] = 0; + revoluteJoint1.m_childFrame[3] = 0; + revoluteJoint1.m_childFrame[4] = 0; + revoluteJoint1.m_childFrame[5] = 0; + revoluteJoint1.m_childFrame[6] = 1.0; + revoluteJoint1.m_jointAxis[0] = 1.0; + revoluteJoint1.m_jointAxis[1] = 0.0; + revoluteJoint1.m_jointAxis[2] = 0.0; + revoluteJoint1.m_jointType = ePoint2PointType; + b3JointInfo revoluteJoint2; + revoluteJoint2.m_parentFrame[0] = 0.055; + revoluteJoint2.m_parentFrame[1] = 0; + revoluteJoint2.m_parentFrame[2] = 0.02; + revoluteJoint2.m_parentFrame[3] = 0; + revoluteJoint2.m_parentFrame[4] = 0; + revoluteJoint2.m_parentFrame[5] = 0; + revoluteJoint2.m_parentFrame[6] = 1.0; + revoluteJoint2.m_childFrame[0] = 0; + revoluteJoint2.m_childFrame[1] = 0; + revoluteJoint2.m_childFrame[2] = 0; + revoluteJoint2.m_childFrame[3] = 0; + revoluteJoint2.m_childFrame[4] = 0; + revoluteJoint2.m_childFrame[5] = 0; + revoluteJoint2.m_childFrame[6] = 1.0; + revoluteJoint2.m_jointAxis[0] = 1.0; + revoluteJoint2.m_jointAxis[1] = 0.0; + revoluteJoint2.m_jointAxis[2] = 0.0; + revoluteJoint2.m_jointType = ePoint2PointType; + m_robotSim.createConstraint(0, 2, 0, 4, &revoluteJoint1); + m_robotSim.createConstraint(0, 3, 0, 6, &revoluteJoint2); + } + + if ((m_options & eGRASP_DEFORMABLE_CLOTH) != 0) + { + m_robotSim.resetSimulation(RESET_USE_DEFORMABLE_WORLD); + { + SliderParams slider("Vertical velocity", &sGripperVerticalVelocity); + slider.m_minVal = -2; + slider.m_maxVal = 2; + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + } + + { + SliderParams slider("Closing velocity", &sGripperClosingTargetVelocity); + slider.m_minVal = -1; + slider.m_maxVal = 1; + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + } + { b3RobotSimulatorLoadFileResults results; m_robotSim.loadSDF("gripper/wsg50_one_motor_gripper_new.sdf", results); @@ -326,10 +422,18 @@ public: m_robotSim.loadURDF("plane.urdf", args); } m_robotSim.setGravity(btVector3(0, 0, -10)); - b3RobotSimulatorLoadSoftBodyArgs args(0.1, 1, 0.02); - args.m_startPosition.setValue(0, 0, 5); - args.m_startOrientation.setValue(1, 0, 0, 1); - m_robotSim.loadSoftBody("bunny.obj", args); + + m_robotSim.setGravity(btVector3(0, 0, -10)); + b3RobotSimulatorLoadDeformableBodyArgs args(1, .01, 0.02); + args.m_springElasticStiffness = 1; + args.m_springDampingStiffness = .001; + args.m_springBendingStiffness = .2; + args.m_frictionCoeff = 2; + args.m_useSelfCollision = false; + args.m_useBendingSprings = true; + args.m_startPosition.setValue(0, 0, 0); + args.m_startOrientation.setValue(0, 0, 1, 1); + m_robotSim.loadDeformableBody("flat_napkin.obj", args); b3JointInfo revoluteJoint1; revoluteJoint1.m_parentFrame[0] = -0.055; @@ -371,7 +475,7 @@ public: revoluteJoint2.m_jointType = ePoint2PointType; m_robotSim.createConstraint(0, 2, 0, 4, &revoluteJoint1); m_robotSim.createConstraint(0, 3, 0, 6, &revoluteJoint2); - } + } if ((m_options & eSOFTBODY_MULTIBODY_COUPLING) != 0) { @@ -462,6 +566,21 @@ public: m_robotSim.setJointMotorControl(m_gripperIndex, fingerJointIndices[i], controlArgs); } } + + if ((m_options & eGRASP_DEFORMABLE_CLOTH) != 0) + { + int fingerJointIndices[2] = {0, 1}; + double fingerTargetVelocities[2] = {sGripperVerticalVelocity, sGripperClosingTargetVelocity}; + double maxTorqueValues[2] = {250.0, 50.0}; + for (int i = 0; i < 2; i++) + { + b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_VELOCITY); + controlArgs.m_targetVelocity = fingerTargetVelocities[i]; + controlArgs.m_maxTorqueValue = maxTorqueValues[i]; + controlArgs.m_kd = 1.; + m_robotSim.setJointMotorControl(m_gripperIndex, fingerJointIndices[i], controlArgs); + } + } if ((m_options & eSOFTBODY_MULTIBODY_COUPLING) != 0) { diff --git a/examples/RoboticsLearning/GripperGraspExample.h b/examples/RoboticsLearning/GripperGraspExample.h index 96387bc41..110068b8e 100644 --- a/examples/RoboticsLearning/GripperGraspExample.h +++ b/examples/RoboticsLearning/GripperGraspExample.h @@ -23,6 +23,7 @@ enum GripperGraspExampleOptions eONE_MOTOR_GRASP = 4, eGRASP_SOFT_BODY = 8, eSOFTBODY_MULTIBODY_COUPLING = 16, + eGRASP_DEFORMABLE_CLOTH = 32, }; class CommonExampleInterface* GripperGraspExampleCreateFunc(struct CommonExampleOptions& options); diff --git a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp index 27bc23163..d8cd3d495 100644 --- a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp +++ b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp @@ -89,6 +89,19 @@ void b3RobotSimulatorClientAPI_NoDirect::resetSimulation() m_data->m_physicsClientHandle, b3InitResetSimulationCommand(m_data->m_physicsClientHandle)); } +void b3RobotSimulatorClientAPI_NoDirect::resetSimulation(int flag) +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return; + } + b3SharedMemoryStatusHandle statusHandle; + b3SharedMemoryCommandHandle command = b3InitResetSimulationCommand(m_data->m_physicsClientHandle); + b3InitResetSimulationSetFlags(command, flag); + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); +} + bool b3RobotSimulatorClientAPI_NoDirect::canSubmitCommand() const { if (!isConnected()) @@ -1151,6 +1164,29 @@ void b3RobotSimulatorClientAPI_NoDirect::loadSoftBody(const std::string& fileNam b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); } +void b3RobotSimulatorClientAPI_NoDirect::loadDeformableBody(const std::string& fileName, const struct b3RobotSimulatorLoadDeformableBodyArgs& args) +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return; + } + + b3SharedMemoryCommandHandle command = b3LoadSoftBodyCommandInit(m_data->m_physicsClientHandle, fileName.c_str()); + b3LoadSoftBodySetStartPosition(command, args.m_startPosition[0], args.m_startPosition[1], args.m_startPosition[2]); + b3LoadSoftBodySetStartOrientation(command, args.m_startOrientation[0], args.m_startOrientation[1], args.m_startOrientation[2], args.m_startOrientation[3]); + b3LoadSoftBodySetScale(command, args.m_scale); + b3LoadSoftBodySetMass(command, args.m_mass); + b3LoadSoftBodySetCollisionMargin(command, args.m_collisionMargin); + b3LoadSoftBodyAddNeoHookeanForce(command, args.m_NeoHookeanMu, args.m_NeoHookeanLambda, args.m_NeoHookeanDamping); + b3LoadSoftBodyAddMassSpringForce(command, args.m_springElasticStiffness, args.m_springDampingStiffness); + b3LoadSoftBodyUseSelfCollision(command, args.m_useSelfCollision); + b3LoadSoftBodyUseFaceContact(command, args.m_useFaceContact); + b3LoadSoftBodySetFrictionCoefficient(command, args.m_frictionCoeff); + b3LoadSoftBodyUseBendingSprings(command, args.m_useBendingSprings, args.m_springBendingStiffness); + b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); +} + void b3RobotSimulatorClientAPI_NoDirect::getMouseEvents(b3MouseEventsData* mouseEventsData) { mouseEventsData->m_numMouseEvents = 0; diff --git a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h index d8fd179c0..e119a37de 100644 --- a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h +++ b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h @@ -89,6 +89,55 @@ struct b3RobotSimulatorLoadSoftBodyArgs } }; + +struct b3RobotSimulatorLoadDeformableBodyArgs +{ + btVector3 m_startPosition; + btQuaternion m_startOrientation; + double m_scale; + double m_mass; + double m_collisionMargin; + double m_springElasticStiffness; + double m_springDampingStiffness; + double m_springBendingStiffness; + double m_NeoHookeanMu; + double m_NeoHookeanLambda; + double m_NeoHookeanDamping; + bool m_useSelfCollision; + bool m_useFaceContact; + bool m_useBendingSprings; + double m_frictionCoeff; + + b3RobotSimulatorLoadDeformableBodyArgs(const btVector3 &startPos, const btQuaternion &startOrn, const double &scale, const double &mass, const double &collisionMargin) + : m_startPosition(startPos), + m_startOrientation(startOrn), + m_scale(scale), + m_mass(mass), + m_collisionMargin(collisionMargin) + { + } + + b3RobotSimulatorLoadDeformableBodyArgs(const btVector3 &startPos, const btQuaternion &startOrn) + { + b3RobotSimulatorLoadSoftBodyArgs(startPos, startOrn, 1.0, 1.0, 0.02); + } + + b3RobotSimulatorLoadDeformableBodyArgs() + { + b3RobotSimulatorLoadSoftBodyArgs(btVector3(0, 0, 0), btQuaternion(0, 0, 0, 1)); + } + + b3RobotSimulatorLoadDeformableBodyArgs(double scale, double mass, double collisionMargin) + : m_startPosition(btVector3(0, 0, 0)), + m_startOrientation(btQuaternion(0, 0, 0, 1)), + m_scale(scale), + m_mass(mass), + m_collisionMargin(collisionMargin) + { + } +}; + + struct b3RobotSimulatorLoadFileResults { btAlignedObjectArray m_uniqueObjectIds; @@ -534,6 +583,8 @@ public: void syncBodies(); void resetSimulation(); + + void resetSimulation(int flag); btQuaternion getQuaternionFromEuler(const btVector3 &rollPitchYaw); btVector3 getEulerFromQuaternion(const btQuaternion &quat); @@ -685,6 +736,8 @@ public: int getConstraintUniqueId(int serialIndex); void loadSoftBody(const std::string &fileName, const struct b3RobotSimulatorLoadSoftBodyArgs &args); + + void loadDeformableBody(const std::string &fileName, const struct b3RobotSimulatorLoadDeformableBodyArgs &args); virtual void setGuiHelper(struct GUIHelperInterface *guiHelper); virtual struct GUIHelperInterface *getGuiHelper(); diff --git a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp index 9c5390d1f..581d6feae 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp @@ -22,8 +22,32 @@ btScalar btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlyIteration { ///this is a special step to resolve penetrations (just for contacts) solveGroupCacheFriendlySplitImpulseIterations(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); +// +// int maxMotorIterations = 150; +// for (int iteration = 0; iteration < maxMotorIterations; ++iteration) +// { +// btScalar motorResidual = 0; +// for (int j = 0; j < m_multiBodyNonContactConstraints.size(); j++) +// { +// int index = iteration & 1 ? j : m_multiBodyNonContactConstraints.size() - 1 - j; +// +// btMultiBodySolverConstraint& constraint = m_multiBodyNonContactConstraints[index]; +// +// btScalar residual = resolveSingleConstraintRowGeneric(constraint); +// motorResidual = btMax(motorResidual, residual * residual); +// if (constraint.m_multiBodyA) +// constraint.m_multiBodyA->setPosUpdated(false); +// if (constraint.m_multiBodyB) +// constraint.m_multiBodyB->setPosUpdated(false); +// } +// if (motorResidual < infoGlobal.m_leastSquaresResidualThreshold) +// { +// break; +// } +// } int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations ? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations; + maxIterations = 500; for (int iteration = 0; iteration < maxIterations; iteration++) { // rigid bodies are solved using solver body velocity, but rigid/deformable contact directly uses the velocity of the actual rigid body. So we have to do the following: Solve one iteration of the rigid/rigid contact, get the updated velocity in the solver body and update the velocity of the underlying rigid body. Then solve the rigid/deformable contact. Finally, grab the (once again) updated rigid velocity and update the velocity of the wrapping solver body From f65a8b03c00447d882e132f45fbfd4b4d68b81c8 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Tue, 17 Dec 2019 18:27:23 -0800 Subject: [PATCH 2/4] separate deformable contact solve by islands WIP --- .../b3RobotSimulatorClientAPI_NoDirect.cpp | 2 +- .../btMultiBodyInplaceSolverIslandCallback.h | 6 +++ ...eformableBodyInplaceSolverIslandCallback.h | 46 +++++++++++++++++++ .../btDeformableMultiBodyConstraintSolver.cpp | 6 +-- .../btDeformableMultiBodyConstraintSolver.h | 5 +- .../btDeformableMultiBodyDynamicsWorld.cpp | 33 +++++++++++-- .../btDeformableMultiBodyDynamicsWorld.h | 27 ++--------- 7 files changed, 92 insertions(+), 33 deletions(-) create mode 100644 src/BulletSoftBody/DeformableBodyInplaceSolverIslandCallback.h diff --git a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp index d8cd3d495..68bf2d231 100644 --- a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp +++ b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp @@ -1180,7 +1180,7 @@ void b3RobotSimulatorClientAPI_NoDirect::loadDeformableBody(const std::string& f b3LoadSoftBodySetCollisionMargin(command, args.m_collisionMargin); b3LoadSoftBodyAddNeoHookeanForce(command, args.m_NeoHookeanMu, args.m_NeoHookeanLambda, args.m_NeoHookeanDamping); b3LoadSoftBodyAddMassSpringForce(command, args.m_springElasticStiffness, args.m_springDampingStiffness); - b3LoadSoftBodyUseSelfCollision(command, args.m_useSelfCollision); + b3LoadSoftBodySetSelfCollision(command, args.m_useSelfCollision); b3LoadSoftBodyUseFaceContact(command, args.m_useFaceContact); b3LoadSoftBodySetFrictionCoefficient(command, args.m_frictionCoeff); b3LoadSoftBodyUseBendingSprings(command, args.m_useBendingSprings, args.m_springBendingStiffness); diff --git a/src/BulletDynamics/Featherstone/btMultiBodyInplaceSolverIslandCallback.h b/src/BulletDynamics/Featherstone/btMultiBodyInplaceSolverIslandCallback.h index 593f68d9a..3169b86e6 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyInplaceSolverIslandCallback.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyInplaceSolverIslandCallback.h @@ -76,6 +76,7 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager:: btDispatcher* m_dispatcher; btAlignedObjectArray m_bodies; + btAlignedObjectArray m_softBodies; btAlignedObjectArray m_manifolds; btAlignedObjectArray m_constraints; btAlignedObjectArray m_multiBodyConstraints; @@ -194,6 +195,10 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager:: { m_bodies.push_back(bodies[i]); } + else + { + m_softBodies.push_back(bodies[i]); + } } for (i = 0; i < numManifolds; i++) m_manifolds.push_back(manifolds[i]); @@ -231,6 +236,7 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager:: m_islandAnalyticsData.push_back(m_solver->m_analyticsData); } m_bodies.resize(0); + m_softBodies.resize(0); m_manifolds.resize(0); m_constraints.resize(0); m_multiBodyConstraints.resize(0); diff --git a/src/BulletSoftBody/DeformableBodyInplaceSolverIslandCallback.h b/src/BulletSoftBody/DeformableBodyInplaceSolverIslandCallback.h new file mode 100644 index 000000000..7b225701f --- /dev/null +++ b/src/BulletSoftBody/DeformableBodyInplaceSolverIslandCallback.h @@ -0,0 +1,46 @@ +// +// DeformableBodyInplaceSolverIslandCallback.h +// BulletSoftBody +// +// Created by Xuchen Han on 12/16/19. +// + +#ifndef DeformableBodyInplaceSolverIslandCallback_h +#define DeformableBodyInplaceSolverIslandCallback_h + +struct DeformableBodyInplaceSolverIslandCallback : public MultiBodyInplaceSolverIslandCallback +{ + btDeformableMultiBodyConstraintSolver* m_deformableSolver; + + DeformableBodyInplaceSolverIslandCallback(btDeformableMultiBodyConstraintSolver* solver, + btDispatcher* dispatcher) + : MultiBodyInplaceSolverIslandCallback(solver, dispatcher), m_deformableSolver(solver) + { + } + + + virtual void processConstraints(int islandId=-1) + { + btCollisionObject** bodies = m_bodies.size() ? &m_bodies[0] : 0; + btCollisionObject** softBodies = m_softBodies.size() ? &m_softBodies[0] : 0; + btPersistentManifold** manifold = m_manifolds.size() ? &m_manifolds[0] : 0; + btTypedConstraint** constraints = m_constraints.size() ? &m_constraints[0] : 0; + btMultiBodyConstraint** multiBodyConstraints = m_multiBodyConstraints.size() ? &m_multiBodyConstraints[0] : 0; + + //printf("mb contacts = %d, mb constraints = %d\n", mbContacts, m_multiBodyConstraints.size()); + + m_deformableSolver->solveDeformableBodyGroup(bodies, m_bodies.size(), softBodies, m_softBodies.size(), manifold, m_manifolds.size(), constraints, m_constraints.size(), multiBodyConstraints, m_multiBodyConstraints.size(), *m_solverInfo, m_debugDrawer, m_dispatcher); + if (m_bodies.size() && (m_solverInfo->m_reportSolverAnalytics&1)) + { + m_deformableSolver->m_analyticsData.m_islandId = islandId; + m_islandAnalyticsData.push_back(m_solver->m_analyticsData); + } + m_bodies.resize(0); + m_softBodies.resize(0); + m_manifolds.resize(0); + m_constraints.resize(0); + m_multiBodyConstraints.resize(0); + } +}; + +#endif /* DeformableBodyInplaceSolverIslandCallback_h */ diff --git a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp index 581d6feae..d2c810d0b 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp @@ -17,7 +17,7 @@ #include "btDeformableMultiBodyConstraintSolver.h" #include // override the iterations method to include deformable/multibody contact -btScalar btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) +btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(btCollisionObject** bodies,int numBodies,btCollisionObject** deformableBodies,int numDeformableBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) { { ///this is a special step to resolve penetrations (just for contacts) @@ -82,7 +82,7 @@ btScalar btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlyIteration return 0.f; } -void btDeformableMultiBodyConstraintSolver::solveMultiBodyGroup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher) +void btDeformableMultiBodyConstraintSolver::solveDeformableBodyGroup(btCollisionObject * *bodies, int numBodies, btCollisionObject * *deformableBodies, int numDeformableBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher) { m_tmpMultiBodyConstraints = multiBodyConstraints; m_tmpNumMultiBodyConstraints = numMultiBodyConstraints; @@ -91,7 +91,7 @@ void btDeformableMultiBodyConstraintSolver::solveMultiBodyGroup(btCollisionObjec solveGroupCacheFriendlySetup(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer); // overriden - solveGroupCacheFriendlyIterations(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer); + solveDeformableGroupIterations(bodies, numBodies, deformableBodies, numDeformableBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer); // inherited from MultiBodyConstraintSolver solveGroupCacheFriendlyFinish(bodies, numBodies, info); diff --git a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h index 10f1d3f4b..0c7cc26a8 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h +++ b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h @@ -36,7 +36,7 @@ btDeformableMultiBodyConstraintSolver : public btMultiBodyConstraintSolver protected: // override the iterations method to include deformable/multibody contact - virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); +// virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); // write the velocity of the the solver body to the underlying rigid body void solverBodyWriteBack(const btContactSolverInfo& infoGlobal); @@ -46,6 +46,7 @@ protected: virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer); + virtual btScalar solveDeformableGroupIterations(btCollisionObject** bodies,int numBodies,btCollisionObject** deformableBodies,int numDeformableBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); public: BT_DECLARE_ALIGNED_ALLOCATOR(); @@ -54,7 +55,7 @@ public: m_deformableSolver = deformableSolver; } - virtual void solveMultiBodyGroup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher); + virtual void solveDeformableBodyGroup(btCollisionObject * *bodies, int numBodies, btCollisionObject * *deformableBodies, int numDeformableBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher); }; #endif /* BT_DEFORMABLE_MULTIBODY_CONSTRAINT_SOLVER_H */ diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp index 25ee65450..618e5c0d7 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp @@ -36,9 +36,36 @@ The algorithm also closely resembles the one in http://physbam.stanford.edu/~fed #include #include "btDeformableMultiBodyDynamicsWorld.h" +#include "DeformableBodyInplaceSolverIslandCallback.h" #include "btDeformableBodySolver.h" #include "LinearMath/btQuickprof.h" #include "btSoftBodyInternals.h" +btDeformableMultiBodyDynamicsWorld::btDeformableMultiBodyDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btDeformableMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration, btDeformableBodySolver* deformableBodySolver) +: btMultiBodyDynamicsWorld(dispatcher, pairCache, (btMultiBodyConstraintSolver*)constraintSolver, collisionConfiguration), +m_deformableBodySolver(deformableBodySolver), m_solverCallback(0) +{ + m_drawFlags = fDrawFlags::Std; + m_drawNodeTree = true; + m_drawFaceTree = false; + m_drawClusterTree = false; + m_sbi.m_broadphase = pairCache; + m_sbi.m_dispatcher = dispatcher; + m_sbi.m_sparsesdf.Initialize(); + m_sbi.m_sparsesdf.setDefaultVoxelsz(0.005); + m_sbi.m_sparsesdf.Reset(); + + m_sbi.air_density = (btScalar)1.2; + m_sbi.water_density = 0; + m_sbi.water_offset = 0; + m_sbi.water_normal = btVector3(0, 0, 0); + m_sbi.m_gravity.setValue(0, -10, 0); + m_internalTime = 0.0; + m_implicit = false; + m_lineSearch = false; + m_selfCollision = true; + m_solverDeformableBodyIslandCallback = new DeformableBodyInplaceSolverIslandCallback(constraintSolver, dispatcher); +} + void btDeformableMultiBodyDynamicsWorld::internalSingleStepSimulation(btScalar timeStep) { BT_PROFILE("internalSingleStepSimulation"); @@ -261,7 +288,7 @@ void btDeformableMultiBodyDynamicsWorld::setupConstraints() // setup the solver callback btMultiBodyConstraint** sortedMultiBodyConstraints = m_sortedMultiBodyConstraints.size() ? &m_sortedMultiBodyConstraints[0] : 0; btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0; - m_solverMultiBodyIslandCallback->setup(&m_solverInfo, constraintsPtr, m_sortedConstraints.size(), sortedMultiBodyConstraints, m_sortedMultiBodyConstraints.size(), getDebugDrawer()); + m_solverDeformableBodyIslandCallback->setup(&m_solverInfo, constraintsPtr, m_sortedConstraints.size(), sortedMultiBodyConstraints, m_sortedMultiBodyConstraints.size(), getDebugDrawer()); // build islands m_islandManager->buildIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld()); @@ -290,10 +317,10 @@ void btDeformableMultiBodyDynamicsWorld::sortConstraints() void btDeformableMultiBodyDynamicsWorld::solveContactConstraints() { // process constraints on each island - m_islandManager->processIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_solverMultiBodyIslandCallback); + m_islandManager->processIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_solverDeformableBodyIslandCallback); // process deferred - m_solverMultiBodyIslandCallback->processConstraints(); + m_solverDeformableBodyIslandCallback->processConstraints(); m_constraintSolver->allSolved(m_solverInfo, m_debugDrawer); // write joint feedback diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h index 9fbebd6ed..763038576 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h @@ -29,6 +29,7 @@ typedef btAlignedObjectArray btSoftBodyArray; class btDeformableBodySolver; class btDeformableLagrangianForce; struct MultiBodyInplaceSolverIslandCallback; +struct DeformableBodyInplaceSolverIslandCallback; class btDeformableMultiBodyConstraintSolver; typedef btAlignedObjectArray btSoftBodyArray; @@ -49,6 +50,7 @@ class btDeformableMultiBodyDynamicsWorld : public btMultiBodyDynamicsWorld bool m_implicit; bool m_lineSearch; bool m_selfCollision; + DeformableBodyInplaceSolverIslandCallback* m_solverDeformableBodyIslandCallback; typedef void (*btSolverCallback)(btScalar time, btDeformableMultiBodyDynamicsWorld* world); btSolverCallback m_solverCallback; @@ -67,30 +69,7 @@ protected: void clearGravity(); public: - btDeformableMultiBodyDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btDeformableMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration, btDeformableBodySolver* deformableBodySolver = 0) - : btMultiBodyDynamicsWorld(dispatcher, pairCache, (btMultiBodyConstraintSolver*)constraintSolver, collisionConfiguration), - m_deformableBodySolver(deformableBodySolver), m_solverCallback(0) - { - m_drawFlags = fDrawFlags::Std; - m_drawNodeTree = true; - m_drawFaceTree = false; - m_drawClusterTree = false; - m_sbi.m_broadphase = pairCache; - m_sbi.m_dispatcher = dispatcher; - m_sbi.m_sparsesdf.Initialize(); - m_sbi.m_sparsesdf.setDefaultVoxelsz(0.005); - m_sbi.m_sparsesdf.Reset(); - - m_sbi.air_density = (btScalar)1.2; - m_sbi.water_density = 0; - m_sbi.water_offset = 0; - m_sbi.water_normal = btVector3(0, 0, 0); - m_sbi.m_gravity.setValue(0, -10, 0); - m_internalTime = 0.0; - m_implicit = false; - m_lineSearch = false; - m_selfCollision = true; - } + btDeformableMultiBodyDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btDeformableMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration, btDeformableBodySolver* deformableBodySolver = 0); virtual int stepSimulation(btScalar timeStep, int maxSubSteps = 1, btScalar fixedTimeStep = btScalar(1.) / btScalar(60.)); From 89553c44e7343ebe4f3c62e186c56627f7f663db Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Wed, 18 Dec 2019 23:11:34 -0800 Subject: [PATCH 3/4] solve constraints involving deformable objects according to islands --- src/BulletSoftBody/btDeformableBodySolver.cpp | 4 +- src/BulletSoftBody/btDeformableBodySolver.h | 2 +- .../btDeformableContactProjection.cpp | 945 +++++++++--------- .../btDeformableContactProjection.h | 35 +- .../btDeformableMultiBodyConstraintSolver.cpp | 28 +- 5 files changed, 483 insertions(+), 531 deletions(-) diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index 82f343104..7724a8ec6 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -234,10 +234,10 @@ void btDeformableBodySolver::setConstraints() m_objective->setConstraints(); } -btScalar btDeformableBodySolver::solveContactConstraints() +btScalar btDeformableBodySolver::solveContactConstraints(btCollisionObject** deformableBodies,int numDeformableBodies) { BT_PROFILE("solveContactConstraints"); - btScalar maxSquaredResidual = m_objective->m_projection.update(); + btScalar maxSquaredResidual = m_objective->m_projection.update(deformableBodies,numDeformableBodies); return maxSquaredResidual; } diff --git a/src/BulletSoftBody/btDeformableBodySolver.h b/src/BulletSoftBody/btDeformableBodySolver.h index 64c7dcfbe..f78a8f696 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.h +++ b/src/BulletSoftBody/btDeformableBodySolver.h @@ -65,7 +65,7 @@ public: virtual void solveDeformableConstraints(btScalar solverdt); // solve the contact between deformable and rigid as well as among deformables - btScalar solveContactConstraints(); + btScalar solveContactConstraints(btCollisionObject** deformableBodies,int numDeformableBodies); // solve the position error between deformable and rigid as well as among deformables; btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal); diff --git a/src/BulletSoftBody/btDeformableContactProjection.cpp b/src/BulletSoftBody/btDeformableContactProjection.cpp index 5986e2881..5a4f3241b 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.cpp +++ b/src/BulletSoftBody/btDeformableContactProjection.cpp @@ -17,527 +17,492 @@ #include "btDeformableMultiBodyDynamicsWorld.h" #include #include -btScalar btDeformableContactProjection::update() +btScalar btDeformableContactProjection::update(btCollisionObject** deformableBodies,int numDeformableBodies) { - btScalar residualSquare = 0; - - // node constraints - for (int index = 0; index < m_nodeRigidConstraints.size(); ++index) - { - btAlignedObjectArray& constraints = *m_nodeRigidConstraints.getAtIndex(index); - for (int i = 0; i < constraints.size(); ++i) - { - btScalar localResidualSquare = constraints[i].solveConstraint(); - residualSquare = btMax(residualSquare, localResidualSquare); - } - } - - // anchor constraints - for (int index = 0; index < m_nodeAnchorConstraints.size(); ++index) - { - btDeformableNodeAnchorConstraint& constraint = *m_nodeAnchorConstraints.getAtIndex(index); - btScalar localResidualSquare = constraint.solveConstraint(); - residualSquare = btMax(residualSquare, localResidualSquare); - } - - // face constraints - for (int index = 0; index < m_allFaceConstraints.size(); ++index) - { - btDeformableContactConstraint* constraint = m_allFaceConstraints[index]; - btScalar localResidualSquare = constraint->solveConstraint(); - residualSquare = btMax(residualSquare, localResidualSquare); - } - - return residualSquare; + btScalar residualSquare = 0; + for (int i = 0; i < numDeformableBodies; ++i) + { + for (int j = 0; j < m_softBodies.size(); ++j) + { + btCollisionObject* psb = m_softBodies[j]; + if (psb != deformableBodies[i]) + { + continue; + } + for (int k = 0; k < m_nodeRigidConstraints[j].size(); ++k) + { + btDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[j][k]; + btScalar localResidualSquare = constraint.solveConstraint(); + residualSquare = btMax(residualSquare, localResidualSquare); + } + for (int k = 0; k < m_nodeAnchorConstraints[j].size(); ++k) + { + btDeformableNodeAnchorConstraint& constraint = m_nodeAnchorConstraints[j][k]; + btScalar localResidualSquare = constraint.solveConstraint(); + residualSquare = btMax(residualSquare, localResidualSquare); + } + for (int k = 0; k < m_faceRigidConstraints[j].size(); ++k) + { + btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[j][k]; + btScalar localResidualSquare = constraint.solveConstraint(); + residualSquare = btMax(residualSquare, localResidualSquare); + } + for (int k = 0; k < m_deformableConstraints[j].size(); ++k) + { + btDeformableFaceNodeContactConstraint& constraint = m_deformableConstraints[j][k]; + btScalar localResidualSquare = constraint.solveConstraint(); + residualSquare = btMax(residualSquare, localResidualSquare); + } + } + } + return residualSquare; } void btDeformableContactProjection::splitImpulseSetup(const btContactSolverInfo& infoGlobal) { - // node constraints - for (int index = 0; index < m_nodeRigidConstraints.size(); ++index) - { - btAlignedObjectArray& constraints = *m_nodeRigidConstraints.getAtIndex(index); - for (int i = 0; i < constraints.size(); ++i) - { - constraints[i].setPenetrationScale(infoGlobal.m_deformable_erp); - } - } - - // face constraints - for (int index = 0; index < m_allFaceConstraints.size(); ++index) - { - btDeformableContactConstraint* constraint = m_allFaceConstraints[index]; - constraint->setPenetrationScale(infoGlobal.m_deformable_erp); - } + for (int i = 0; i < m_softBodies.size(); ++i) + { + // node constraints + for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j) + { + btDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[i][j]; + constraint.setPenetrationScale(infoGlobal.m_deformable_erp); + } + // face constraints + for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j) + { + btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[i][j]; + constraint.setPenetrationScale(infoGlobal.m_deformable_erp); + } + } } btScalar btDeformableContactProjection::solveSplitImpulse(const btContactSolverInfo& infoGlobal) { - btScalar residualSquare = 0; - // node constraints - for (int index = 0; index < m_nodeRigidConstraints.size(); ++index) - { - btAlignedObjectArray& constraints = *m_nodeRigidConstraints.getAtIndex(index); - for (int i = 0; i < constraints.size(); ++i) - { - btScalar localResidualSquare = constraints[i].solveSplitImpulse(infoGlobal); - residualSquare = btMax(residualSquare, localResidualSquare); - } - } - - // anchor constraints - for (int index = 0; index < m_nodeAnchorConstraints.size(); ++index) - { - btDeformableNodeAnchorConstraint& constraint = *m_nodeAnchorConstraints.getAtIndex(index); - btScalar localResidualSquare = constraint.solveSplitImpulse(infoGlobal); - residualSquare = btMax(residualSquare, localResidualSquare); - } - - // face constraints - for (int index = 0; index < m_allFaceConstraints.size(); ++index) - { - btDeformableContactConstraint* constraint = m_allFaceConstraints[index]; - btScalar localResidualSquare = constraint->solveSplitImpulse(infoGlobal); - residualSquare = btMax(residualSquare, localResidualSquare); - } - return residualSquare; + btScalar residualSquare = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + // node constraints + for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j) + { + btDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[i][j]; + btScalar localResidualSquare = constraint.solveSplitImpulse(infoGlobal); + residualSquare = btMax(residualSquare, localResidualSquare); + } + // anchor constraints + for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j) + { + btDeformableNodeAnchorConstraint& constraint = m_nodeAnchorConstraints[i][j]; + btScalar localResidualSquare = constraint.solveSplitImpulse(infoGlobal); + residualSquare = btMax(residualSquare, localResidualSquare); + } + // face constraints + for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j) + { + btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[i][j]; + btScalar localResidualSquare = constraint.solveSplitImpulse(infoGlobal); + residualSquare = btMax(residualSquare, localResidualSquare); + } + + } + return residualSquare; } void btDeformableContactProjection::setConstraints() { - BT_PROFILE("setConstraints"); - // set Dirichlet constraint - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - continue; - } - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - if (psb->m_nodes[j].m_im == 0) - { - btDeformableStaticConstraint static_constraint(&psb->m_nodes[j]); - m_staticConstraints.insert(psb->m_nodes[j].index, static_constraint); - } - } - } - - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - continue; - } + BT_PROFILE("setConstraints"); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } - // set up deformable anchors - for (int j = 0; j < psb->m_deformableAnchors.size(); ++j) - { - btSoftBody::DeformableNodeRigidAnchor& anchor = psb->m_deformableAnchors[j]; - // skip fixed points - if (anchor.m_node->m_im == 0) - { - continue; - } - - if (m_nodeAnchorConstraints.find(anchor.m_node->index) == NULL) - { - anchor.m_c1 = anchor.m_cti.m_colObj->getWorldTransform().getBasis() * anchor.m_local; - btDeformableNodeAnchorConstraint constraint(anchor); - m_nodeAnchorConstraints.insert(anchor.m_node->index, constraint); - } - } - - // set Deformable Node vs. Rigid constraint - for (int j = 0; j < psb->m_nodeRigidContacts.size(); ++j) - { - const btSoftBody::DeformableNodeRigidContact& contact = psb->m_nodeRigidContacts[j]; - // skip fixed points - if (contact.m_node->m_im == 0) - { - continue; - } - btDeformableNodeRigidContactConstraint constraint(contact); - btVector3 va = constraint.getVa(); - btVector3 vb = constraint.getVb(); - const btVector3 vr = vb - va; - const btSoftBody::sCti& cti = contact.m_cti; - const btScalar dn = btDot(vr, cti.m_normal); - if (dn < SIMD_EPSILON) - { - if (m_nodeRigidConstraints.find(contact.m_node->index) == NULL) - { - btAlignedObjectArray constraintsList; - constraintsList.push_back(constraint); - m_nodeRigidConstraints.insert(contact.m_node->index, constraintsList); - } - else - { - btAlignedObjectArray& constraintsList = *m_nodeRigidConstraints[contact.m_node->index]; - constraintsList.push_back(constraint); - } - } - } - - // set Deformable Face vs. Rigid constraint - for (int j = 0; j < psb->m_faceRigidContacts.size(); ++j) - { - const btSoftBody::DeformableFaceRigidContact& contact = psb->m_faceRigidContacts[j]; - // skip fixed faces - if (contact.m_c2 == 0) - { - continue; - } - btDeformableFaceRigidContactConstraint* constraint = new btDeformableFaceRigidContactConstraint(contact); - btVector3 va = constraint->getVa(); - btVector3 vb = constraint->getVb(); - const btVector3 vr = vb - va; - const btSoftBody::sCti& cti = contact.m_cti; - const btScalar dn = btDot(vr, cti.m_normal); - if (dn < SIMD_EPSILON) - { - m_allFaceConstraints.push_back(constraint); - // add face constraints to each of the nodes - for (int k = 0; k < 3; ++k) - { - btSoftBody::Node* node = contact.m_face->m_n[k]; - // static node does not need to own face/rigid constraint - if (node->m_im != 0) - { - if (m_faceRigidConstraints.find(node->index) == NULL) - { - btAlignedObjectArray constraintsList; - constraintsList.push_back(constraint); - m_faceRigidConstraints.insert(node->index, constraintsList); - } - else - { - btAlignedObjectArray& constraintsList = *m_faceRigidConstraints[node->index]; - constraintsList.push_back(constraint); - } - } - } - } - else - { - delete constraint; - } - } - - // set Deformable Face vs. Deformable Node constraint - for (int j = 0; j < psb->m_faceNodeContacts.size(); ++j) - { - const btSoftBody::DeformableFaceNodeContact& contact = psb->m_faceNodeContacts[j]; - - btDeformableFaceNodeContactConstraint* constraint = new btDeformableFaceNodeContactConstraint(contact); - btVector3 va = constraint->getVa(); - btVector3 vb = constraint->getVb(); - const btVector3 vr = vb - va; - const btScalar dn = btDot(vr, contact.m_normal); - if (dn > -SIMD_EPSILON) - { - btSoftBody::Node* node = contact.m_node; - btSoftBody::Face* face = contact.m_face; - m_allFaceConstraints.push_back(constraint); - if (node->m_im != 0) - { - if (m_deformableConstraints.find(node->index) == NULL) - { - btAlignedObjectArray constraintsList; - constraintsList.push_back(constraint); - m_deformableConstraints.insert(node->index, constraintsList); - } - else - { - btAlignedObjectArray& constraintsList = *m_deformableConstraints[node->index]; - constraintsList.push_back(constraint); - } - } - - // add face constraints to each of the nodes - for (int k = 0; k < 3; ++k) - { - btSoftBody::Node* node = face->m_n[k]; - // static node does not need to own face/rigid constraint - if (node->m_im != 0) - { - if (m_deformableConstraints.find(node->index) == NULL) - { - btAlignedObjectArray constraintsList; - constraintsList.push_back(constraint); - m_deformableConstraints.insert(node->index, constraintsList); - } - else - { - btAlignedObjectArray& constraintsList = *m_deformableConstraints[node->index]; - constraintsList.push_back(constraint); - } - } - } - } - else - { - delete constraint; - } - } - } + // set Dirichlet constraint + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + if (psb->m_nodes[j].m_im == 0) + { + btDeformableStaticConstraint static_constraint(&psb->m_nodes[j]); + m_staticConstraints[i].push_back(static_constraint); + } + } + + // set up deformable anchors + for (int j = 0; j < psb->m_deformableAnchors.size(); ++j) + { + btSoftBody::DeformableNodeRigidAnchor& anchor = psb->m_deformableAnchors[j]; + // skip fixed points + if (anchor.m_node->m_im == 0) + { + continue; + } + anchor.m_c1 = anchor.m_cti.m_colObj->getWorldTransform().getBasis() * anchor.m_local; + btDeformableNodeAnchorConstraint constraint(anchor); + m_nodeAnchorConstraints[i].push_back(constraint); + } + + // set Deformable Node vs. Rigid constraint + for (int j = 0; j < psb->m_nodeRigidContacts.size(); ++j) + { + const btSoftBody::DeformableNodeRigidContact& contact = psb->m_nodeRigidContacts[j]; + // skip fixed points + if (contact.m_node->m_im == 0) + { + continue; + } + btDeformableNodeRigidContactConstraint constraint(contact); + btVector3 va = constraint.getVa(); + btVector3 vb = constraint.getVb(); + const btVector3 vr = vb - va; + const btSoftBody::sCti& cti = contact.m_cti; + const btScalar dn = btDot(vr, cti.m_normal); + if (dn < SIMD_EPSILON) + { + m_nodeRigidConstraints[i].push_back(constraint); + } + } + + // set Deformable Face vs. Rigid constraint + for (int j = 0; j < psb->m_faceRigidContacts.size(); ++j) + { + const btSoftBody::DeformableFaceRigidContact& contact = psb->m_faceRigidContacts[j]; + // skip fixed faces + if (contact.m_c2 == 0) + { + continue; + } + btDeformableFaceRigidContactConstraint constraint(contact); + btVector3 va = constraint.getVa(); + btVector3 vb = constraint.getVb(); + const btVector3 vr = vb - va; + const btSoftBody::sCti& cti = contact.m_cti; + const btScalar dn = btDot(vr, cti.m_normal); + if (dn < SIMD_EPSILON) + { + m_faceRigidConstraints[i].push_back(constraint); + } + } + + // set Deformable Face vs. Deformable Node constraint + for (int j = 0; j < psb->m_faceNodeContacts.size(); ++j) + { + const btSoftBody::DeformableFaceNodeContact& contact = psb->m_faceNodeContacts[j]; + + btDeformableFaceNodeContactConstraint constraint(contact); + btVector3 va = constraint.getVa(); + btVector3 vb = constraint.getVb(); + const btVector3 vr = vb - va; + const btScalar dn = btDot(vr, contact.m_normal); + if (dn > -SIMD_EPSILON) + { + m_deformableConstraints[i].push_back(constraint); + } + } + } } void btDeformableContactProjection::project(TVStack& x) { - const int dim = 3; - for (int index = 0; index < m_projectionsDict.size(); ++index) - { - btAlignedObjectArray& projectionDirs = *m_projectionsDict.getAtIndex(index); - size_t i = m_projectionsDict.getKeyAtIndex(index).getUid1(); - if (projectionDirs.size() >= dim) - { - // static node - x[i].setZero(); - continue; - } - else if (projectionDirs.size() == 2) - { - btVector3 dir0 = projectionDirs[0]; - btVector3 dir1 = projectionDirs[1]; - btVector3 free_dir = btCross(dir0, dir1); - if (free_dir.norm() < SIMD_EPSILON) - { - x[i] -= x[i].dot(dir0) * dir0; - x[i] -= x[i].dot(dir1) * dir1; - } - else - { - free_dir.normalize(); - x[i] = x[i].dot(free_dir) * free_dir; - } - } - else - { - btAssert(projectionDirs.size() == 1); - btVector3 dir0 = projectionDirs[0]; - x[i] -= x[i].dot(dir0) * dir0; - } - } + const int dim = 3; + for (int index = 0; index < m_projectionsDict.size(); ++index) + { + btAlignedObjectArray& projectionDirs = *m_projectionsDict.getAtIndex(index); + size_t i = m_projectionsDict.getKeyAtIndex(index).getUid1(); + if (projectionDirs.size() >= dim) + { + // static node + x[i].setZero(); + continue; + } + else if (projectionDirs.size() == 2) + { + btVector3 dir0 = projectionDirs[0]; + btVector3 dir1 = projectionDirs[1]; + btVector3 free_dir = btCross(dir0, dir1); + if (free_dir.safeNorm() < SIMD_EPSILON) + { + x[i] -= x[i].dot(dir0) * dir0; + x[i] -= x[i].dot(dir1) * dir1; + } + else + { + free_dir.normalize(); + x[i] = x[i].dot(free_dir) * free_dir; + } + } + else + { + btAssert(projectionDirs.size() == 1); + btVector3 dir0 = projectionDirs[0]; + x[i] -= x[i].dot(dir0) * dir0; + } + } } void btDeformableContactProjection::setProjection() { - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - continue; - } - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - int index = psb->m_nodes[j].index; - bool hasConstraint = false; - bool existStaticConstraint = false; - btVector3 averagedNormal(0,0,0); - btAlignedObjectArray normals; - if (m_staticConstraints.find(index) != NULL || m_nodeAnchorConstraints.find(index) != NULL) - { - existStaticConstraint = true; - hasConstraint = true; - } - - // accumulate normals from Deformable Node vs. Rigid constraints - if (!existStaticConstraint && m_nodeRigidConstraints.find(index) != NULL) - { - hasConstraint = true; - btAlignedObjectArray& constraintsList = *m_nodeRigidConstraints[index]; - for (int k = 0; k < constraintsList.size(); ++k) - { - if (constraintsList[k].m_static) - { - existStaticConstraint = true; - break; - } - const btVector3& local_normal = constraintsList[k].m_normal; - normals.push_back(local_normal); - averagedNormal += local_normal; - } - } - - // accumulate normals from Deformable Face vs. Rigid constraints - if (!existStaticConstraint && m_faceRigidConstraints.find(index) != NULL) - { - hasConstraint = true; - btAlignedObjectArray& constraintsList = *m_faceRigidConstraints[index]; - for (int k = 0; k < constraintsList.size(); ++k) - { - if (constraintsList[k]->m_static) - { - existStaticConstraint = true; - break; - } - const btVector3& local_normal = constraintsList[k]->m_normal; - normals.push_back(local_normal); - averagedNormal += local_normal; - } - } - - // accumulate normals from Deformable Node vs. Deformable Face constraints - if (!existStaticConstraint && m_deformableConstraints.find(index) != NULL) - { - hasConstraint = true; - btAlignedObjectArray& constraintsList = *m_deformableConstraints[index]; - for (int k = 0; k < constraintsList.size(); ++k) - { - if (constraintsList[k]->m_static) - { - existStaticConstraint = true; - break; - } - const btVector3& local_normal = constraintsList[k]->m_normal; - normals.push_back(local_normal); - averagedNormal += local_normal; - } - } - - - // build projections - if (!hasConstraint) - { - continue; - } - btAlignedObjectArray projections; - if (existStaticConstraint) - { - projections.push_back(btVector3(1,0,0)); - projections.push_back(btVector3(0,1,0)); - projections.push_back(btVector3(0,0,1)); - } - else - { - bool averageExists = (averagedNormal.length2() > SIMD_EPSILON); - averagedNormal = averageExists ? averagedNormal.normalized() : btVector3(0,0,0); - if (averageExists) - { - projections.push_back(averagedNormal); - } - for (int k = 0; k < normals.size(); ++k) - { - const btVector3& local_normal = normals[k]; - // add another projection direction if it deviates from the average by more than about 15 degrees - if (!averageExists || btAngle(averagedNormal, local_normal) > 0.25) - { - projections.push_back(local_normal); - } - } - } - m_projectionsDict.insert(index, projections); - } - } + btAlignedObjectArray units; + units.push_back(btVector3(1,0,0)); + units.push_back(btVector3(0,1,0)); + units.push_back(btVector3(0,0,1)); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < m_staticConstraints[i].size(); ++j) + { + int index = m_staticConstraints[i][j].m_node->index; + if (m_projectionsDict.find(index) == NULL) + { + m_projectionsDict.insert(index, units); + } + else + { + btAlignedObjectArray& projections = *m_projectionsDict[index]; + for (int k = 0; k < 3; ++k) + { + projections.push_back(units[k]); + } + } + } + for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j) + { + int index = m_nodeAnchorConstraints[i][j].m_anchor->m_node->index; + if (m_projectionsDict.find(index) == NULL) + { + m_projectionsDict.insert(index, units); + } + else + { + btAlignedObjectArray& projections = *m_projectionsDict[index]; + for (int k = 0; k < 3; ++k) + { + projections.push_back(units[k]); + } + } + } + for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j) + { + int index = m_nodeRigidConstraints[i][j].m_node->index; + if (m_nodeRigidConstraints[i][j].m_static) + { + if (m_projectionsDict.find(index) == NULL) + { + m_projectionsDict.insert(index, units); + } + else + { + btAlignedObjectArray& projections = *m_projectionsDict[index]; + for (int k = 0; k < 3; ++k) + { + projections.push_back(units[k]); + } + } + } + else + { + if (m_projectionsDict.find(index) == NULL) + { + btAlignedObjectArray projections; + projections.push_back(m_nodeRigidConstraints[i][j].m_normal); + m_projectionsDict.insert(index, projections); + } + else + { + btAlignedObjectArray& projections = *m_projectionsDict[index]; + projections.push_back(m_nodeRigidConstraints[i][j].m_normal); + } + } + } + for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j) + { + const btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face; + for (int k = 0; k < 3; ++k) + { + const btSoftBody::Node* node = face->m_n[k]; + int index = node->index; + if (m_faceRigidConstraints[i][j].m_static) + { + if (m_projectionsDict.find(index) == NULL) + { + m_projectionsDict.insert(index, units); + } + else + { + btAlignedObjectArray& projections = *m_projectionsDict[index]; + for (int k = 0; k < 3; ++k) + { + projections.push_back(units[k]); + } + } + } + else + { + if (m_projectionsDict.find(index) == NULL) + { + btAlignedObjectArray projections; + projections.push_back(m_faceRigidConstraints[i][j].m_normal); + m_projectionsDict.insert(index, projections); + } + else + { + btAlignedObjectArray& projections = *m_projectionsDict[index]; + projections.push_back(m_faceRigidConstraints[i][j].m_normal); + } + } + } + } + for (int j = 0; j < m_deformableConstraints[i].size(); ++j) + { + const btSoftBody::Face* face = m_deformableConstraints[i][j].m_face; + for (int k = 0; k < 3; ++k) + { + const btSoftBody::Node* node = face->m_n[k]; + int index = node->index; + if (m_deformableConstraints[i][j].m_static) + { + if (m_projectionsDict.find(index) == NULL) + { + m_projectionsDict.insert(index, units); + } + else + { + btAlignedObjectArray& projections = *m_projectionsDict[index]; + for (int k = 0; k < 3; ++k) + { + projections.push_back(units[k]); + } + } + } + else + { + if (m_projectionsDict.find(index) == NULL) + { + btAlignedObjectArray projections; + projections.push_back(m_deformableConstraints[i][j].m_normal); + m_projectionsDict.insert(index, projections); + } + else + { + btAlignedObjectArray& projections = *m_projectionsDict[index]; + projections.push_back(m_deformableConstraints[i][j].m_normal); + } + } + } + + const btSoftBody::Node* node = m_deformableConstraints[i][j].m_node; + int index = node->index; + if (m_deformableConstraints[i][j].m_static) + { + if (m_projectionsDict.find(index) == NULL) + { + m_projectionsDict.insert(index, units); + } + else + { + btAlignedObjectArray& projections = *m_projectionsDict[index]; + for (int k = 0; k < 3; ++k) + { + projections.push_back(units[k]); + } + } + } + else + { + if (m_projectionsDict.find(index) == NULL) + { + btAlignedObjectArray projections; + projections.push_back(m_deformableConstraints[i][j].m_normal); + m_projectionsDict.insert(index, projections); + } + else + { + btAlignedObjectArray& projections = *m_projectionsDict[index]; + projections.push_back(m_deformableConstraints[i][j].m_normal); + } + } + } + } } void btDeformableContactProjection::applyDynamicFriction(TVStack& f) { - // loop over constraints - for (int i = 0; i < f.size(); ++i) - { - if (m_projectionsDict.find(i) != NULL) - { - // doesn't need to add friction force for fully constrained vertices - btAlignedObjectArray& projectionDirs = *m_projectionsDict[i]; - if (projectionDirs.size() >= 3) - { - continue; - } - } - - // add friction contribution from Face vs. Node - if (m_nodeRigidConstraints.find(i) != NULL) - { - btAlignedObjectArray& constraintsList = *m_nodeRigidConstraints[i]; - for (int j = 0; j < constraintsList.size(); ++j) - { - const btDeformableNodeRigidContactConstraint& constraint = constraintsList[j]; - btSoftBody::Node* node = constraint.getContact()->m_node; - - // it's ok to add the friction force generated by the entire impulse here because the normal component of the residual will be projected out anyway. - f[i] += constraint.getDv(node)* (1./node->m_im); - } - } - - // add friction contribution from Face vs. Rigid - if (m_faceRigidConstraints.find(i) != NULL) - { - btAlignedObjectArray& constraintsList = *m_faceRigidConstraints[i]; - for (int j = 0; j < constraintsList.size(); ++j) - { - const btDeformableFaceRigidContactConstraint* constraint = constraintsList[j]; - btSoftBody::Face* face = constraint->getContact()->m_face; - - // it's ok to add the friction force generated by the entire impulse here because the normal component of the residual will be projected out anyway. - for (int k = 0; k < 3; ++k) - { - if (face->m_n[k]->index == i) - { - if (face->m_n[k]->m_im != 0) - { - f[i] += constraint->getDv(face->m_n[k])* (1./face->m_n[k]->m_im); - } - break; - } - } - } - } - - if (m_deformableConstraints.find(i) != NULL) - { - btAlignedObjectArray& constraintsList = *m_deformableConstraints[i]; - for (int j = 0; j < constraintsList.size(); ++j) - { - const btDeformableFaceNodeContactConstraint* constraint = constraintsList[j]; - btSoftBody::Face* face = constraint->getContact()->m_face; - btSoftBody::Node* node = constraint->getContact()->m_node; - - // it's ok to add the friction force generated by the entire impulse here because the normal component of the residual will be projected out anyway. - if (node->index == i) - { - if (node->m_im != 0) - { - f[i] += constraint->getDv(node)*(1./node->m_im); - } - } - else - { - for (int k = 0; k < 3; ++k) - { - if (face->m_n[k]->index == i) - { - if (face->m_n[k]->m_im != 0) - { - f[i] += constraint->getDv(face->m_n[k])* (1./face->m_n[k]->m_im); - } - break; - } - } - } - } - } - } + for (int i = 0; i < m_softBodies.size(); ++i) + { + for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j) + { + const btDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[i][j]; + const btSoftBody::Node* node = constraint.m_node; + if (node->m_im != 0) + { + int index = node->index; + f[index] += constraint.getDv(node)* (1./node->m_im); + } + } + for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j) + { + const btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[i][j]; + const btSoftBody::Face* face = constraint.getContact()->m_face; + for (int k = 0; k < 3; ++k) + { + const btSoftBody::Node* node = face->m_n[k]; + if (node->m_im != 0) + { + int index = node->index; + f[index] += constraint.getDv(node)* (1./node->m_im); + } + } + } + for (int j = 0; j < m_deformableConstraints[i].size(); ++j) + { + const btDeformableFaceNodeContactConstraint& constraint = m_deformableConstraints[i][j]; + const btSoftBody::Face* face = constraint.getContact()->m_face; + const btSoftBody::Node* node = constraint.getContact()->m_node; + if (node->m_im != 0) + { + int index = node->index; + f[index] += constraint.getDv(node)* (1./node->m_im); + } + for (int k = 0; k < 3; ++k) + { + const btSoftBody::Node* node = face->m_n[k]; + if (node->m_im != 0) + { + int index = node->index; + f[index] += constraint.getDv(node)* (1./node->m_im); + } + } + } + } } void btDeformableContactProjection::reinitialize(bool nodeUpdated) { - m_staticConstraints.clear(); - m_nodeAnchorConstraints.clear(); - m_nodeRigidConstraints.clear(); - m_faceRigidConstraints.clear(); - m_deformableConstraints.clear(); - m_projectionsDict.clear(); - for (int i = 0; i < m_allFaceConstraints.size(); ++i) - { - delete m_allFaceConstraints[i]; - } - m_allFaceConstraints.clear(); + int N = m_softBodies.size(); + if (nodeUpdated) + { + m_staticConstraints.resize(N); + m_nodeAnchorConstraints.resize(N); + m_nodeRigidConstraints.resize(N); + m_faceRigidConstraints.resize(N); + m_deformableConstraints.resize(N); + + } + for (int i = 0 ; i < N; ++i) + { + m_staticConstraints[i].clear(); + m_nodeAnchorConstraints[i].clear(); + m_nodeRigidConstraints[i].clear(); + m_faceRigidConstraints[i].clear(); + m_deformableConstraints[i].clear(); + } + m_projectionsDict.clear(); } diff --git a/src/BulletSoftBody/btDeformableContactProjection.h b/src/BulletSoftBody/btDeformableContactProjection.h index 406eeba3c..3c4490765 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.h +++ b/src/BulletSoftBody/btDeformableContactProjection.h @@ -28,22 +28,33 @@ public: typedef btAlignedObjectArray TVStack; btAlignedObjectArray& m_softBodies; - // map from node index to static constraint - btHashMap m_staticConstraints; - // map from node index to node rigid constraint - btHashMap > m_nodeRigidConstraints; - // map from node index to face rigid constraint - btHashMap > m_faceRigidConstraints; - // map from node index to deformable constraint - btHashMap > m_deformableConstraints; - // map from node index to node anchor constraint - btHashMap m_nodeAnchorConstraints; - +// // map from node index to static constraint +// btHashMap m_staticConstraints; +// // map from node index to node rigid constraint +// btHashMap > m_nodeRigidConstraints; +// // map from node index to face rigid constraint +// btHashMap > m_faceRigidConstraints; +// // map from node index to deformable constraint +// btHashMap > m_deformableConstraints; +// // map from node index to node anchor constraint +// btHashMap m_nodeAnchorConstraints; + // all constraints involving face btAlignedObjectArray m_allFaceConstraints; // map from node index to projection directions btHashMap > m_projectionsDict; + + // map from node index to static constraint + btAlignedObjectArray > m_staticConstraints; + // map from node index to node rigid constraint + btAlignedObjectArray > m_nodeRigidConstraints; + // map from node index to face rigid constraint + btAlignedObjectArray > m_faceRigidConstraints; + // map from node index to deformable constraint + btAlignedObjectArray > m_deformableConstraints; + // map from node index to node anchor constraint + btAlignedObjectArray > m_nodeAnchorConstraints; btDeformableContactProjection(btAlignedObjectArray& softBodies) : m_softBodies(softBodies) @@ -61,7 +72,7 @@ public: virtual void applyDynamicFriction(TVStack& f); // update and solve the constraints - virtual btScalar update(); + virtual btScalar update(btCollisionObject** deformableBodies,int numDeformableBodies); // solve the position error using split impulse virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal); diff --git a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp index d2c810d0b..06f95d69f 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp @@ -22,32 +22,8 @@ btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(b { ///this is a special step to resolve penetrations (just for contacts) solveGroupCacheFriendlySplitImpulseIterations(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); -// -// int maxMotorIterations = 150; -// for (int iteration = 0; iteration < maxMotorIterations; ++iteration) -// { -// btScalar motorResidual = 0; -// for (int j = 0; j < m_multiBodyNonContactConstraints.size(); j++) -// { -// int index = iteration & 1 ? j : m_multiBodyNonContactConstraints.size() - 1 - j; -// -// btMultiBodySolverConstraint& constraint = m_multiBodyNonContactConstraints[index]; -// -// btScalar residual = resolveSingleConstraintRowGeneric(constraint); -// motorResidual = btMax(motorResidual, residual * residual); -// if (constraint.m_multiBodyA) -// constraint.m_multiBodyA->setPosUpdated(false); -// if (constraint.m_multiBodyB) -// constraint.m_multiBodyB->setPosUpdated(false); -// } -// if (motorResidual < infoGlobal.m_leastSquaresResidualThreshold) -// { -// break; -// } -// } - + int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations ? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations; - maxIterations = 500; for (int iteration = 0; iteration < maxIterations; iteration++) { // rigid bodies are solved using solver body velocity, but rigid/deformable contact directly uses the velocity of the actual rigid body. So we have to do the following: Solve one iteration of the rigid/rigid contact, get the updated velocity in the solver body and update the velocity of the underlying rigid body. Then solve the rigid/deformable contact. Finally, grab the (once again) updated rigid velocity and update the velocity of the wrapping solver body @@ -56,7 +32,7 @@ btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(b m_leastSquaresResidual = solveSingleIteration(iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); // solver body velocity -> rigid body velocity solverBodyWriteBack(infoGlobal); - btScalar deformableResidual = m_deformableSolver->solveContactConstraints(); + btScalar deformableResidual = m_deformableSolver->solveContactConstraints(deformableBodies,numDeformableBodies); // update rigid body velocity in rigid/deformable contact m_leastSquaresResidual = btMax(m_leastSquaresResidual, deformableResidual); // solver body velocity <- rigid body velocity From 6a8973d2f4f347e2967ca4ed79a17d2748defd29 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Thu, 19 Dec 2019 21:51:54 -0800 Subject: [PATCH 4/4] address PR comment and tune parameters for cloth --- examples/RoboticsLearning/GripperGraspExample.cpp | 12 +++++++----- .../b3RobotSimulatorClientAPI_NoDirect.cpp | 10 ++++++++-- .../b3RobotSimulatorClientAPI_NoDirect.h | 11 ++++++++++- 3 files changed, 25 insertions(+), 8 deletions(-) diff --git a/examples/RoboticsLearning/GripperGraspExample.cpp b/examples/RoboticsLearning/GripperGraspExample.cpp index c3afaee88..a07e36c77 100644 --- a/examples/RoboticsLearning/GripperGraspExample.cpp +++ b/examples/RoboticsLearning/GripperGraspExample.cpp @@ -424,12 +424,13 @@ public: m_robotSim.setGravity(btVector3(0, 0, -10)); m_robotSim.setGravity(btVector3(0, 0, -10)); - b3RobotSimulatorLoadDeformableBodyArgs args(1, .01, 0.02); - args.m_springElasticStiffness = 1; - args.m_springDampingStiffness = .001; - args.m_springBendingStiffness = .2; - args.m_frictionCoeff = 2; + b3RobotSimulatorLoadDeformableBodyArgs args(2, .01, 0.006); + args.m_springElasticStiffness = .1; + args.m_springDampingStiffness = .0004; + args.m_springBendingStiffness = 1; + args.m_frictionCoeff = 1; args.m_useSelfCollision = false; +// args.m_useFaceContact = true; args.m_useBendingSprings = true; args.m_startPosition.setValue(0, 0, 0); args.m_startOrientation.setValue(0, 0, 1, 1); @@ -475,6 +476,7 @@ public: revoluteJoint2.m_jointType = ePoint2PointType; m_robotSim.createConstraint(0, 2, 0, 4, &revoluteJoint1); m_robotSim.createConstraint(0, 3, 0, 6, &revoluteJoint2); + m_robotSim.setNumSimulationSubSteps(8); } if ((m_options & eSOFTBODY_MULTIBODY_COUPLING) != 0) diff --git a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp index 68bf2d231..0a2741d7b 100644 --- a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp +++ b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp @@ -1178,8 +1178,14 @@ void b3RobotSimulatorClientAPI_NoDirect::loadDeformableBody(const std::string& f b3LoadSoftBodySetScale(command, args.m_scale); b3LoadSoftBodySetMass(command, args.m_mass); b3LoadSoftBodySetCollisionMargin(command, args.m_collisionMargin); - b3LoadSoftBodyAddNeoHookeanForce(command, args.m_NeoHookeanMu, args.m_NeoHookeanLambda, args.m_NeoHookeanDamping); - b3LoadSoftBodyAddMassSpringForce(command, args.m_springElasticStiffness, args.m_springDampingStiffness); + if (args.m_NeoHookeanMu > 0) + { + b3LoadSoftBodyAddNeoHookeanForce(command, args.m_NeoHookeanMu, args.m_NeoHookeanLambda, args.m_NeoHookeanDamping); + } + if (args.m_springElasticStiffness > 0) + { + b3LoadSoftBodyAddMassSpringForce(command, args.m_springElasticStiffness, args.m_springDampingStiffness); + } b3LoadSoftBodySetSelfCollision(command, args.m_useSelfCollision); b3LoadSoftBodyUseFaceContact(command, args.m_useFaceContact); b3LoadSoftBodySetFrictionCoefficient(command, args.m_frictionCoeff); diff --git a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h index e119a37de..dc039974e 100644 --- a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h +++ b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h @@ -113,7 +113,16 @@ struct b3RobotSimulatorLoadDeformableBodyArgs m_startOrientation(startOrn), m_scale(scale), m_mass(mass), - m_collisionMargin(collisionMargin) + m_collisionMargin(collisionMargin), + m_springElasticStiffness(-1), + m_springDampingStiffness(-1), + m_springBendingStiffness(-1), + m_NeoHookeanMu(-1), + m_NeoHookeanDamping(-1), + m_useSelfCollision(false), + m_useFaceContact(false), + m_useBendingSprings(false), + m_frictionCoeff(0) { }