diff --git a/data/torus_deform.urdf b/data/torus_deform.urdf
index c5d661993..7d09ac281 100644
--- a/data/torus_deform.urdf
+++ b/data/torus_deform.urdf
@@ -6,6 +6,7 @@
+
diff --git a/examples/DeformableDemo/GraspDeformable.cpp b/examples/DeformableDemo/GraspDeformable.cpp
index a773ab430..0fede1b56 100644
--- a/examples/DeformableDemo/GraspDeformable.cpp
+++ b/examples/DeformableDemo/GraspDeformable.cpp
@@ -73,10 +73,10 @@ public:
void resetCamera()
{
- float dist = 2;
+ float dist = 0.3;
float pitch = -45;
float yaw = 100;
- float targetPos[3] = {0, -0, 0};
+ float targetPos[3] = {0, -0.1, 0};
m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
}
@@ -105,12 +105,12 @@ public:
if (dofIndex == 6)
{
motor->setVelocityTarget(-fingerTargetVelocities[1], 1);
- motor->setMaxAppliedImpulse(2);
+ motor->setMaxAppliedImpulse(20);
}
if (dofIndex == 7)
{
motor->setVelocityTarget(fingerTargetVelocities[1], 1);
- motor->setMaxAppliedImpulse(2);
+ motor->setMaxAppliedImpulse(20);
}
motor->setMaxAppliedImpulse(1);
}
@@ -208,9 +208,9 @@ void GraspDeformable::initPhysics()
bool canSleep = false;
bool selfCollide = true;
int numLinks = 2;
- btVector3 linkHalfExtents(.1, .2, .04);
- btVector3 baseHalfExtents(.1, 0.02, .2);
- btMultiBody* mbC = createFeatherstoneMultiBody(getDeformableDynamicsWorld(), btVector3(0.f, .7f,0.f), linkHalfExtents, baseHalfExtents, false);
+ btVector3 linkHalfExtents(0.02, 0.018, .003);
+ btVector3 baseHalfExtents(0.02, 0.002, .002);
+ btMultiBody* mbC = createFeatherstoneMultiBody(getDeformableDynamicsWorld(), btVector3(0.f, 0.05f,0.f), linkHalfExtents, baseHalfExtents, false);
mbC->setCanSleep(canSleep);
mbC->setHasSelfCollision(selfCollide);
@@ -258,7 +258,7 @@ void GraspDeformable::initPhysics()
btTransform groundTransform;
groundTransform.setIdentity();
- groundTransform.setOrigin(btVector3(0, -5.3, 0));
+ groundTransform.setOrigin(btVector3(0, -5.1, 0));
groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0));
//We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here:
btScalar mass(0.);
@@ -309,7 +309,7 @@ void GraspDeformable::initPhysics()
// psb->scale(btVector3(.3, .3, .3)); // for tube, torus, boot
// psb->scale(btVector3(.1, .1, .1)); // for ditto
// psb->translate(btVector3(.25, 10, 0.4));
- psb->getCollisionShape()->setMargin(0.0005);
+ psb->getCollisionShape()->setMargin(0.01);
psb->setMaxStress(50);
psb->setTotalMass(.1);
psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
@@ -334,8 +334,8 @@ void GraspDeformable::initPhysics()
if(1)
{
bool onGround = false;
- const btScalar s = .5;
- const btScalar h = .1;
+ const btScalar s = .05;
+ const btScalar h = -0.02;
btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, h, -s),
btVector3(+s, h, -s),
btVector3(-s, h, +s),
@@ -352,37 +352,35 @@ void GraspDeformable::initPhysics()
2,2,
0, true);
- psb->getCollisionShape()->setMargin(0.01);
+ psb->getCollisionShape()->setMargin(0.003);
psb->generateBendingConstraints(2);
- psb->setTotalMass(1);
+ psb->setTotalMass(0.005);
psb->setSpringStiffness(10);
psb->setDampingCoefficient(0.05);
psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
psb->m_cfg.kCHR = 1; // collision hardness with rigid body
- psb->m_cfg.kDF = 2;
+ psb->m_cfg.kDF = 1;
psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDF;
psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_MDF;
-// psb->m_cfg.collisions |= btSoftBody::fCollision::VF_DD;
getDeformableDynamicsWorld()->addSoftBody(psb);
-// getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce(.0,0.0, true));
- getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce(10,1, true));
- getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity));
+ getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce(0.05,0.005, true));
+ getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity*0.1));
}
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
{
SliderParams slider("Moving velocity", &sGripperVerticalVelocity);
- slider.m_minVal = -.2;
- slider.m_maxVal = .2;
+ slider.m_minVal = -.02;
+ slider.m_maxVal = .02;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
{
SliderParams slider("Closing velocity", &sGripperClosingTargetVelocity);
- slider.m_minVal = -.5;
- slider.m_maxVal = .5;
+ slider.m_minVal = -1;
+ slider.m_maxVal = 1;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
@@ -436,8 +434,8 @@ btMultiBody* GraspDeformable::createFeatherstoneMultiBody(btMultiBodyDynamicsWor
{
//init the base
btVector3 baseInertiaDiag(0.f, 0.f, 0.f);
- float baseMass = 100.f;
- float linkMass = 100.f;
+ float baseMass = 0.1;
+ float linkMass = 0.1;
int numLinks = 2;
if (baseMass)
@@ -465,8 +463,8 @@ btMultiBody* GraspDeformable::createFeatherstoneMultiBody(btMultiBodyDynamicsWor
//y-axis assumed up
btAlignedObjectArray parentComToCurrentCom;
- parentComToCurrentCom.push_back(btVector3(0, -linkHalfExtents[1] * 8.f, -baseHalfExtents[2] * 4.f));
- parentComToCurrentCom.push_back(btVector3(0, -linkHalfExtents[1] * 8.f, +baseHalfExtents[2] * 4.f));//par body's COM to cur body's COM offset
+ parentComToCurrentCom.push_back(btVector3(0, -linkHalfExtents[1] * 8.f, -baseHalfExtents[2] * 2.f));
+ parentComToCurrentCom.push_back(btVector3(0, -linkHalfExtents[1] * 8.f, +baseHalfExtents[2] * 2.f));//par body's COM to cur body's COM offset
btVector3 currentPivotToCurrentCom(0, -linkHalfExtents[1]*8.f, 0); //cur body's COM to cur body's PIV offset
@@ -507,6 +505,7 @@ void GraspDeformable::addColliders(btMultiBody* pMultiBody, btMultiBodyDynamicsW
if (1)
{
btCollisionShape* box = new btBoxShape(baseHalfExtents);
+ box->setMargin(0.001);
btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, -1);
col->setCollisionShape(box);
@@ -537,6 +536,7 @@ void GraspDeformable::addColliders(btMultiBody* pMultiBody, btMultiBodyDynamicsW
btScalar quat[4] = {-world_to_local[i + 1].x(), -world_to_local[i + 1].y(), -world_to_local[i + 1].z(), world_to_local[i + 1].w()};
btCollisionShape* box = new btBoxShape(linkHalfExtents);
+ box->setMargin(0.001);
btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, i);
col->setCollisionShape(box);
diff --git a/examples/DeformableDemo/Pinch.cpp b/examples/DeformableDemo/Pinch.cpp
index aa325cdbc..4f294c7b6 100644
--- a/examples/DeformableDemo/Pinch.cpp
+++ b/examples/DeformableDemo/Pinch.cpp
@@ -300,21 +300,21 @@ void Pinch::initPhysics()
psb->setTotalMass(1);
psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
psb->m_cfg.kCHR = 1; // collision hardness with rigid body
- psb->m_cfg.kDF = 2;
+ psb->m_cfg.kDF = .5;
psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDF;
getDeformableDynamicsWorld()->addSoftBody(psb);
btSoftBodyHelpers::generateBoundaryFaces(psb);
- btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(1,0.05);
- getDeformableDynamicsWorld()->addForce(psb, mass_spring);
- m_forces.push_back(mass_spring);
-
btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity);
getDeformableDynamicsWorld()->addForce(psb, gravity_force);
m_forces.push_back(gravity_force);
- btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(.2,1);
+ btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(8,3, 0.02);
+ neohookean->setPoissonRatio(0.3);
+ neohookean->setYoungsModulus(25);
+ neohookean->setDamping(0.01);
+ psb->m_cfg.drag = 0.001;
getDeformableDynamicsWorld()->addForce(psb, neohookean);
m_forces.push_back(neohookean);
// add a grippers
diff --git a/examples/DeformableDemo/SplitImpulse.cpp b/examples/DeformableDemo/SplitImpulse.cpp
index b8c9ce201..306f98f96 100644
--- a/examples/DeformableDemo/SplitImpulse.cpp
+++ b/examples/DeformableDemo/SplitImpulse.cpp
@@ -165,7 +165,7 @@ void SplitImpulse::initPhysics()
psb->setTotalMass(1);
psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
psb->m_cfg.kCHR = 1; // collision hardness with rigid body
- psb->m_cfg.kDF = 0.1;
+ psb->m_cfg.kDF = 1;
psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDF;
getDeformableDynamicsWorld()->addSoftBody(psb);
diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.cpp b/examples/Importers/ImportURDFDemo/UrdfParser.cpp
index f656e4fde..a37e610ed 100644
--- a/examples/Importers/ImportURDFDemo/UrdfParser.cpp
+++ b/examples/Importers/ImportURDFDemo/UrdfParser.cpp
@@ -1163,6 +1163,17 @@ bool UrdfParser::parseDeformable(UrdfModel& model, tinyxml2::XMLElement* config,
}
deformable.m_friction = urdfLexicalCast(friction_xml->Attribute("value"));
}
+
+ XMLElement* repulsion_xml = config->FirstChildElement("repulsion_stiffness");
+ if (repulsion_xml)
+ {
+ if (!repulsion_xml->Attribute("value"))
+ {
+ logger->reportError("repulsion_stiffness element must have value attribute");
+ return false;
+ }
+ deformable.m_repulsionStiffness = urdfLexicalCast(repulsion_xml->Attribute("value"));
+ }
XMLElement* spring_xml = config->FirstChildElement("spring");
if (spring_xml)
diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.h b/examples/Importers/ImportURDFDemo/UrdfParser.h
index 9b99bc36f..d873e4d30 100644
--- a/examples/Importers/ImportURDFDemo/UrdfParser.h
+++ b/examples/Importers/ImportURDFDemo/UrdfParser.h
@@ -225,6 +225,7 @@ struct UrdfDeformable
double m_mass;
double m_collisionMargin;
double m_friction;
+ double m_repulsionStiffness;
SpringCoeffcients m_springCoefficients;
LameCoefficients m_corotatedCoefficients;
@@ -234,7 +235,7 @@ struct UrdfDeformable
std::string m_simFileName;
btHashMap m_userData;
- UrdfDeformable() : m_mass(1.), m_collisionMargin(0.02), m_friction(1.), m_visualFileName(""), m_simFileName("")
+ UrdfDeformable() : m_mass(1.), m_collisionMargin(0.02), m_friction(1.), m_repulsionStiffness(0.5), m_visualFileName(""), m_simFileName("")
{
}
};
diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp
index 1ec54f498..b76c3466f 100644
--- a/examples/SharedMemory/PhysicsClientC_API.cpp
+++ b/examples/SharedMemory/PhysicsClientC_API.cpp
@@ -404,6 +404,15 @@ B3_SHARED_API int b3LoadSoftBodySetCollisionHardness(b3SharedMemoryCommandHandle
return 0;
}
+B3_SHARED_API int b3LoadSoftBodySetRepulsionStiffness(b3SharedMemoryCommandHandle commandHandle, double stiffness)
+{
+ struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
+ b3Assert(command->m_type == CMD_LOAD_SOFT_BODY);
+ command->m_loadSoftBodyArguments.m_repulsionStiffness = stiffness;
+ command->m_updateFlags |= LOAD_SOFT_BODY_SET_REPULSION_STIFFNESS;
+ return 0;
+}
+
B3_SHARED_API int b3LoadSoftBodySetSelfCollision(b3SharedMemoryCommandHandle commandHandle, int useSelfCollision)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h
index 7c52708a3..438a4b3c0 100644
--- a/examples/SharedMemory/PhysicsClientC_API.h
+++ b/examples/SharedMemory/PhysicsClientC_API.h
@@ -648,7 +648,8 @@ extern "C"
B3_SHARED_API int b3LoadSoftBodyAddGravityForce(b3SharedMemoryCommandHandle commandHandle, double gravityX, double gravityY, double gravityZ);
B3_SHARED_API int b3LoadSoftBodySetCollisionHardness(b3SharedMemoryCommandHandle commandHandle, double collisionHardness);
B3_SHARED_API int b3LoadSoftBodySetSelfCollision(b3SharedMemoryCommandHandle commandHandle, int useSelfCollision);
- B3_SHARED_API int b3LoadSoftBodyUseFaceContact(b3SharedMemoryCommandHandle commandHandle, int useFaceContact);
+ B3_SHARED_API int b3LoadSoftBodySetRepulsionStiffness(b3SharedMemoryCommandHandle commandHandle, double stiffness);
+ B3_SHARED_API int b3LoadSoftBodyUseFaceContact(b3SharedMemoryCommandHandle commandHandle, int useFaceContact);
B3_SHARED_API int b3LoadSoftBodySetFrictionCoefficient(b3SharedMemoryCommandHandle commandHandle, double frictionCoefficient);
B3_SHARED_API int b3LoadSoftBodyUseBendingSprings(b3SharedMemoryCommandHandle commandHandle, int useBendingSprings, double bendingStiffness);
diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
index 58d72dc3c..c6b637256 100644
--- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
+++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
@@ -8312,6 +8312,10 @@ void constructUrdfDeformable(const struct SharedMemoryCommand& clientCmd, UrdfDe
{
deformable.m_friction = loadSoftBodyArgs.m_frictionCoeff;
}
+ if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_SET_REPULSION_STIFFNESS)
+ {
+ deformable.m_repulsionStiffness = loadSoftBodyArgs.m_repulsionStiffness;
+ }
#endif
}
@@ -8531,6 +8535,7 @@ bool PhysicsServerCommandProcessor::processDeformable(const UrdfDeformable& defo
psb->setCollisionFlags(0);
psb->setTotalMass(deformable.m_mass);
psb->setSelfCollision(useSelfCollision);
+ psb->setSpringStiffness(deformable.m_repulsionStiffness);
psb->initializeFaceTree();
}
#endif //SKIP_DEFORMABLE_BODY
diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h
index c0d00ac72..af6cb907b 100644
--- a/examples/SharedMemory/SharedMemoryCommands.h
+++ b/examples/SharedMemory/SharedMemoryCommands.h
@@ -497,17 +497,18 @@ enum EnumLoadSoftBodyUpdateFlags
LOAD_SOFT_BODY_UPDATE_MASS = 1<<2,
LOAD_SOFT_BODY_UPDATE_COLLISION_MARGIN = 1<<3,
LOAD_SOFT_BODY_INITIAL_POSITION = 1<<4,
- LOAD_SOFT_BODY_INITIAL_ORIENTATION = 1<<5,
- LOAD_SOFT_BODY_ADD_COROTATED_FORCE = 1<<6,
- LOAD_SOFT_BODY_ADD_MASS_SPRING_FORCE = 1<<7,
- LOAD_SOFT_BODY_ADD_GRAVITY_FORCE = 1<<8,
- LOAD_SOFT_BODY_SET_COLLISION_HARDNESS = 1<<9,
- LOAD_SOFT_BODY_SET_FRICTION_COEFFICIENT = 1<<10,
- LOAD_SOFT_BODY_ADD_BENDING_SPRINGS = 1<<11,
- LOAD_SOFT_BODY_ADD_NEOHOOKEAN_FORCE = 1<<12,
- LOAD_SOFT_BODY_USE_SELF_COLLISION = 1<<13,
- LOAD_SOFT_BODY_USE_FACE_CONTACT = 1<<14,
- LOAD_SOFT_BODY_SIM_MESH = 1<<15,
+ LOAD_SOFT_BODY_INITIAL_ORIENTATION = 1<<5,
+ LOAD_SOFT_BODY_ADD_COROTATED_FORCE = 1<<6,
+ LOAD_SOFT_BODY_ADD_MASS_SPRING_FORCE = 1<<7,
+ LOAD_SOFT_BODY_ADD_GRAVITY_FORCE = 1<<8,
+ LOAD_SOFT_BODY_SET_COLLISION_HARDNESS = 1<<9,
+ LOAD_SOFT_BODY_SET_FRICTION_COEFFICIENT = 1<<10,
+ LOAD_SOFT_BODY_ADD_BENDING_SPRINGS = 1<<11,
+ LOAD_SOFT_BODY_ADD_NEOHOOKEAN_FORCE = 1<<12,
+ LOAD_SOFT_BODY_USE_SELF_COLLISION = 1<<13,
+ LOAD_SOFT_BODY_USE_FACE_CONTACT = 1<<14,
+ LOAD_SOFT_BODY_SIM_MESH = 1<<15,
+ LOAD_SOFT_BODY_SET_REPULSION_STIFFNESS = 1<<16,
};
enum EnumSimParamInternalSimFlags
@@ -540,6 +541,7 @@ struct LoadSoftBodyArgs
double m_NeoHookeanDamping;
int m_useFaceContact;
char m_simFileName[MAX_FILENAME_LENGTH];
+ double m_repulsionStiffness;
};
struct b3LoadSoftBodyResultArgs
diff --git a/examples/pybullet/examples/deformable_torus.py b/examples/pybullet/examples/deformable_torus.py
index 2801944d8..7e2732fb3 100644
--- a/examples/pybullet/examples/deformable_torus.py
+++ b/examples/pybullet/examples/deformable_torus.py
@@ -14,7 +14,7 @@ planeId = p.loadURDF("plane.urdf", [0,0,-2])
boxId = p.loadURDF("cube.urdf", [0,3,2],useMaximalCoordinates = True)
-bunnyId = p.loadSoftBody("torus.vtk", useNeoHookean = 1, NeoHookeanMu = 60, NeoHookeanLambda = 200, NeoHookeanDamping = 0.01, useSelfCollision = 1, frictionCoeff = 0.5)
+bunnyId = p.loadSoftBody("torus.vtk", mass = 1, useNeoHookean = 1, NeoHookeanMu = 60, NeoHookeanLambda = 200, NeoHookeanDamping = 0.01, collisionMargin = 0.006, useSelfCollision = 1, frictionCoeff = 0.5, repulsionStiffness = 50)
bunny2 = p.loadURDF("torus_deform.urdf", [0,1,0], flags=p.URDF_USE_SELF_COLLISION)
@@ -23,4 +23,3 @@ p.setRealTimeSimulation(1)
while p.isConnected():
p.setGravity(0,0,-10)
- sleep(1./240.)
diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c
index d6beded74..9362e4a11 100644
--- a/examples/pybullet/pybullet.c
+++ b/examples/pybullet/pybullet.c
@@ -2038,7 +2038,7 @@ static PyObject* pybullet_loadSoftBody(PyObject* self, PyObject* args, PyObject*
int physicsClientId = 0;
int flags = 0;
- static char* kwlist[] = {"fileName", "basePosition", "baseOrientation", "scale", "mass", "collisionMargin", "physicsClientId", "useMassSpring", "useBendingSprings", "useNeoHookean", "springElasticStiffness", "springDampingStiffness", "springBendingStiffness", "NeoHookeanMu", "NeoHookeanLambda", "NeoHookeanDamping", "frictionCoeff", "useFaceContact", "useSelfCollision", NULL};
+ static char* kwlist[] = {"fileName", "basePosition", "baseOrientation", "scale", "mass", "collisionMargin", "useMassSpring", "useBendingSprings", "useNeoHookean", "springElasticStiffness", "springDampingStiffness", "springBendingStiffness", "NeoHookeanMu", "NeoHookeanLambda", "NeoHookeanDamping", "frictionCoeff", "useFaceContact", "useSelfCollision", "repulsionStiffness", "physicsClientId", NULL};
int bodyUniqueId = -1;
const char* fileName = "";
@@ -2057,7 +2057,7 @@ static PyObject* pybullet_loadSoftBody(PyObject* self, PyObject* args, PyObject*
double frictionCoeff = 0;
int useFaceContact = 0;
int useSelfCollision = 0;
-
+ double repulsionStiffness = 0.5;
b3PhysicsClientHandle sm = 0;
@@ -2068,7 +2068,7 @@ static PyObject* pybullet_loadSoftBody(PyObject* self, PyObject* args, PyObject*
PyObject* basePosObj = 0;
PyObject* baseOrnObj = 0;
- if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|OOdddiiiidddddddii", kwlist, &fileName, &basePosObj, &baseOrnObj, &scale, &mass, &collisionMargin, &physicsClientId, &useMassSpring, &useBendingSprings, &useNeoHookean, &springElasticStiffness, &springDampingStiffness, &springBendingStiffness, &NeoHookeanMu, &NeoHookeanLambda, &NeoHookeanDamping, &frictionCoeff, &useFaceContact, &useSelfCollision))
+ if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|OOdddiiidddddddiidi", kwlist, &fileName, &basePosObj, &baseOrnObj, &scale, &mass, &collisionMargin, &useMassSpring, &useBendingSprings, &useNeoHookean, &springElasticStiffness, &springDampingStiffness, &springBendingStiffness, &NeoHookeanMu, &NeoHookeanLambda, &NeoHookeanDamping, &frictionCoeff, &useFaceContact, &useSelfCollision, &repulsionStiffness, &physicsClientId))
{
return NULL;
}
@@ -2134,6 +2134,10 @@ static PyObject* pybullet_loadSoftBody(PyObject* self, PyObject* args, PyObject*
{
b3LoadSoftBodySetSelfCollision(command, useSelfCollision);
}
+ if (repulsionStiffness > 0)
+ {
+ b3LoadSoftBodySetRepulsionStiffness(command, repulsionStiffness);
+ }
b3LoadSoftBodySetFrictionCoefficient(command, frictionCoeff);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
statusType = b3GetStatusType(statusHandle);
diff --git a/src/BulletSoftBody/CMakeLists.txt b/src/BulletSoftBody/CMakeLists.txt
index 57fe3b975..945276801 100644
--- a/src/BulletSoftBody/CMakeLists.txt
+++ b/src/BulletSoftBody/CMakeLists.txt
@@ -59,6 +59,7 @@ SET(BulletSoftBody_HDRS
btDeformableContactProjection.h
btDeformableMultiBodyDynamicsWorld.h
btDeformableContactConstraint.h
+ btKrylovSolver.h
poly34.h
btSoftBodySolverVertexBuffer.h
diff --git a/src/BulletSoftBody/btConjugateGradient.h b/src/BulletSoftBody/btConjugateGradient.h
index bd51e584b..f56dbc188 100644
--- a/src/BulletSoftBody/btConjugateGradient.h
+++ b/src/BulletSoftBody/btConjugateGradient.h
@@ -15,24 +15,17 @@
#ifndef BT_CONJUGATE_GRADIENT_H
#define BT_CONJUGATE_GRADIENT_H
-#include
-#include
-#include
-#include
-#include
-#include "LinearMath/btQuickprof.h"
+#include "btKrylovSolver.h"
template
-class btConjugateGradient
+class btConjugateGradient : public btKrylovSolver
{
typedef btAlignedObjectArray TVStack;
+ typedef btKrylovSolver Base;
TVStack r,p,z,temp;
- int max_iterations;
- btScalar tolerance_squared;
public:
btConjugateGradient(const int max_it_in)
- : max_iterations(max_it_in)
+ : btKrylovSolver(max_it_in, SIMD_EPSILON)
{
- tolerance_squared = 1e-5;
}
virtual ~btConjugateGradient(){}
@@ -45,13 +38,13 @@ public:
reinitialize(b);
// r = b - A * x --with assigned dof zeroed out
A.multiply(x, temp);
- r = sub(b, temp);
+ r = this->sub(b, temp);
A.project(r);
// z = M^(-1) * r
A.precondition(r, z);
A.project(z);
- btScalar r_dot_z = dot(z,r);
- if (r_dot_z <= tolerance_squared) {
+ btScalar r_dot_z = this->dot(z,r);
+ if (r_dot_z <= Base::m_tolerance) {
if (verbose)
{
std::cout << "Iteration = 0" << std::endl;
@@ -61,11 +54,11 @@ public:
}
p = z;
btScalar r_dot_z_new = r_dot_z;
- for (int k = 1; k <= max_iterations; k++) {
+ for (int k = 1; k <= Base::m_maxIterations; k++) {
// temp = A*p
A.multiply(p, temp);
A.project(temp);
- if (dot(p,temp) < SIMD_EPSILON)
+ if (this->dot(p,temp) < SIMD_EPSILON)
{
if (verbose)
std::cout << "Encountered negative direction in CG!" << std::endl;
@@ -76,16 +69,16 @@ public:
return k;
}
// alpha = r^T * z / (p^T * A * p)
- btScalar alpha = r_dot_z_new / dot(p, temp);
+ btScalar alpha = r_dot_z_new / this->dot(p, temp);
// x += alpha * p;
- multAndAddTo(alpha, p, x);
+ this->multAndAddTo(alpha, p, x);
// r -= alpha * temp;
- multAndAddTo(-alpha, temp, r);
+ this->multAndAddTo(-alpha, temp, r);
// z = M^(-1) * r
A.precondition(r, z);
r_dot_z = r_dot_z_new;
- r_dot_z_new = dot(r,z);
- if (r_dot_z_new < tolerance_squared) {
+ r_dot_z_new = this->dot(r,z);
+ if (r_dot_z_new < Base::m_tolerance) {
if (verbose)
{
std::cout << "ConjugateGradient iterations " << k << std::endl;
@@ -94,13 +87,13 @@ public:
}
btScalar beta = r_dot_z_new/r_dot_z;
- p = multAndAdd(beta, p, z);
+ p = this->multAndAdd(beta, p, z);
}
if (verbose)
{
- std::cout << "ConjugateGradient max iterations reached " << max_iterations << std::endl;
+ std::cout << "ConjugateGradient max iterations reached " << Base::m_maxIterations << std::endl;
}
- return max_iterations;
+ return Base::m_maxIterations;
}
void reinitialize(const TVStack& b)
@@ -110,49 +103,5 @@ public:
z.resize(b.size());
temp.resize(b.size());
}
-
- TVStack sub(const TVStack& a, const TVStack& b)
- {
- // c = a-b
- btAssert(a.size() == b.size());
- TVStack c;
- c.resize(a.size());
- for (int i = 0; i < a.size(); ++i)
- {
- c[i] = a[i] - b[i];
- }
- return c;
- }
-
- btScalar squaredNorm(const TVStack& a)
- {
- return dot(a,a);
- }
-
- btScalar dot(const TVStack& a, const TVStack& b)
- {
- btScalar ans(0);
- for (int i = 0; i < a.size(); ++i)
- ans += a[i].dot(b[i]);
- return ans;
- }
-
- void multAndAddTo(btScalar s, const TVStack& a, TVStack& result)
- {
-// result += s*a
- btAssert(a.size() == result.size());
- for (int i = 0; i < a.size(); ++i)
- result[i] += s * a[i];
- }
-
- TVStack multAndAdd(btScalar s, const TVStack& a, const TVStack& b)
- {
- // result = a*s + b
- TVStack result;
- result.resize(a.size());
- for (int i = 0; i < a.size(); ++i)
- result[i] = s * a[i] + b[i];
- return result;
- }
};
#endif /* btConjugateGradient_h */
diff --git a/src/BulletSoftBody/btConjugateResidual.h b/src/BulletSoftBody/btConjugateResidual.h
index 7b211c417..d4739da83 100644
--- a/src/BulletSoftBody/btConjugateResidual.h
+++ b/src/BulletSoftBody/btConjugateResidual.h
@@ -15,28 +15,22 @@
#ifndef BT_CONJUGATE_RESIDUAL_H
#define BT_CONJUGATE_RESIDUAL_H
-#include
-#include
-#include
-#include
-#include
-#include
-#include "LinearMath/btQuickprof.h"
+#include "btKrylovSolver.h"
+
template
-class btConjugateResidual
+class btConjugateResidual : public btKrylovSolver
{
typedef btAlignedObjectArray TVStack;
+ typedef btKrylovSolver Base;
TVStack r,p,z,temp_p, temp_r, best_x;
// temp_r = A*r
// temp_p = A*p
// z = M^(-1) * temp_p = M^(-1) * A * p
- int max_iterations;
- btScalar tolerance_squared, best_r;
+ btScalar best_r;
public:
btConjugateResidual(const int max_it_in)
- : max_iterations(max_it_in)
+ : Base(max_it_in, 1e-8)
{
- tolerance_squared = 1e-2;
}
virtual ~btConjugateResidual(){}
@@ -49,17 +43,12 @@ public:
reinitialize(b);
// r = b - A * x --with assigned dof zeroed out
A.multiply(x, temp_r); // borrow temp_r here to store A*x
- r = sub(b, temp_r);
+ r = this->sub(b, temp_r);
// z = M^(-1) * r
A.precondition(r, z); // borrow z to store preconditioned r
r = z;
- btScalar residual_norm = norm(r);
- if (residual_norm <= tolerance_squared) {
- if (verbose)
- {
- std::cout << "Iteration = 0" << std::endl;
- std::cout << "Two norm of the residual = " << residual_norm << std::endl;
- }
+ btScalar residual_norm = this->norm(r);
+ if (residual_norm <= Base::m_tolerance) {
return 0;
}
p = r;
@@ -68,52 +57,41 @@ public:
A.multiply(p, temp_p);
// temp_r = A*r
temp_r = temp_p;
- r_dot_Ar = dot(r, temp_r);
- for (int k = 1; k <= max_iterations; k++) {
+ r_dot_Ar = this->dot(r, temp_r);
+ for (int k = 1; k <= Base::m_maxIterations; k++) {
// z = M^(-1) * Ap
A.precondition(temp_p, z);
// alpha = r^T * A * r / (Ap)^T * M^-1 * Ap)
- btScalar alpha = r_dot_Ar / dot(temp_p, z);
+ btScalar alpha = r_dot_Ar / this->dot(temp_p, z);
// x += alpha * p;
- multAndAddTo(alpha, p, x);
+ this->multAndAddTo(alpha, p, x);
// r -= alpha * z;
- multAndAddTo(-alpha, z, r);
- btScalar norm_r = norm(r);
+ this->multAndAddTo(-alpha, z, r);
+ btScalar norm_r = this->norm(r);
if (norm_r < best_r)
{
best_x = x;
best_r = norm_r;
- if (norm_r < tolerance_squared) {
- if (verbose)
- {
- std::cout << "ConjugateResidual iterations " << k << std::endl;
- }
+ if (norm_r < Base::m_tolerance) {
return k;
}
- else
- {
- if (verbose)
- {
- std::cout << "ConjugateResidual iterations " << k << " has residual "<< norm_r << std::endl;
- }
- }
}
// temp_r = A * r;
A.multiply(r, temp_r);
- r_dot_Ar_new = dot(r, temp_r);
+ r_dot_Ar_new = this->dot(r, temp_r);
btScalar beta = r_dot_Ar_new/r_dot_Ar;
r_dot_Ar = r_dot_Ar_new;
// p = beta*p + r;
- p = multAndAdd(beta, p, r);
+ p = this->multAndAdd(beta, p, r);
// temp_p = beta*temp_p + temp_r;
- temp_p = multAndAdd(beta, temp_p, temp_r);
+ temp_p = this->multAndAdd(beta, temp_p, temp_r);
}
if (verbose)
{
- std::cout << "ConjugateResidual max iterations reached " << max_iterations << std::endl;
+ std::cout << "ConjugateResidual max iterations reached, residual = " << best_r << std::endl;
}
x = best_x;
- return max_iterations;
+ return Base::m_maxIterations;
}
void reinitialize(const TVStack& b)
@@ -126,63 +104,6 @@ public:
best_x.resize(b.size());
best_r = SIMD_INFINITY;
}
-
- TVStack sub(const TVStack& a, const TVStack& b)
- {
- // c = a-b
- btAssert(a.size() == b.size());
- TVStack c;
- c.resize(a.size());
- for (int i = 0; i < a.size(); ++i)
- {
- c[i] = a[i] - b[i];
- }
- return c;
- }
-
- btScalar squaredNorm(const TVStack& a)
- {
- return dot(a,a);
- }
-
- btScalar norm(const TVStack& a)
- {
- btScalar ret = 0;
- for (int i = 0; i < a.size(); ++i)
- {
- for (int d = 0; d < 3; ++d)
- {
- ret = btMax(ret, btFabs(a[i][d]));
- }
- }
- return ret;
- }
-
- btScalar dot(const TVStack& a, const TVStack& b)
- {
- btScalar ans(0);
- for (int i = 0; i < a.size(); ++i)
- ans += a[i].dot(b[i]);
- return ans;
- }
-
- void multAndAddTo(btScalar s, const TVStack& a, TVStack& result)
- {
- // result += s*a
- btAssert(a.size() == result.size());
- for (int i = 0; i < a.size(); ++i)
- result[i] += s * a[i];
- }
-
- TVStack multAndAdd(btScalar s, const TVStack& a, const TVStack& b)
- {
- // result = a*s + b
- TVStack result;
- result.resize(a.size());
- for (int i = 0; i < a.size(); ++i)
- result[i] = s * a[i] + b[i];
- return result;
- }
};
#endif /* btConjugateResidual_h */
diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h
index 86579e71a..d4060ce4a 100644
--- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h
+++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h
@@ -168,6 +168,31 @@ public:
}
}
}
+
+ void calculateContactForce(const TVStack& dv, const TVStack& rhs, TVStack& f)
+ {
+ size_t counter = 0;
+ for (int i = 0; i < m_softBodies.size(); ++i)
+ {
+ btSoftBody* psb = m_softBodies[i];
+ for (int j = 0; j < psb->m_nodes.size(); ++j)
+ {
+ const btSoftBody::Node& node = psb->m_nodes[j];
+ f[counter] = (node.m_im == 0) ? btVector3(0,0,0) : dv[counter] / node.m_im;
+ ++counter;
+ }
+ }
+ for (int i = 0; i < m_lf.size(); ++i)
+ {
+ // add damping matrix
+ m_lf[i]->addScaledDampingForceDifferential(-m_dt, dv, f);
+ }
+ counter = 0;
+ for (; counter < f.size(); ++counter)
+ {
+ f[counter] = rhs[counter] - f[counter];
+ }
+ }
};
#endif /* btBackwardEulerObjective_h */
diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp
index 132699c54..736fe4695 100644
--- a/src/BulletSoftBody/btDeformableBodySolver.cpp
+++ b/src/BulletSoftBody/btDeformableBodySolver.cpp
@@ -262,11 +262,6 @@ btScalar btDeformableBodySolver::solveContactConstraints(btCollisionObject** def
return maxSquaredResidual;
}
-void btDeformableBodySolver::splitImpulseSetup(const btContactSolverInfo& infoGlobal)
-{
- m_objective->m_projection.splitImpulseSetup(infoGlobal);
-}
-
void btDeformableBodySolver::updateVelocity()
{
int counter = 0;
@@ -286,7 +281,7 @@ void btDeformableBodySolver::updateVelocity()
{
m_dv[counter].setZero();
}
- psb->m_nodes[j].m_v = m_backupVelocity[counter]+m_dv[counter];
+ psb->m_nodes[j].m_v = m_backupVelocity[counter] + m_dv[counter] - psb->m_nodes[j].m_splitv;
psb->m_maxSpeedSquared = btMax(psb->m_maxSpeedSquared, psb->m_nodes[j].m_v.length2());
++counter;
}
@@ -349,7 +344,7 @@ void btDeformableBodySolver::setupDeformableSolve(bool implicit)
}
else
{
- m_dv[counter] = psb->m_nodes[j].m_v - m_backupVelocity[counter];
+ m_dv[counter] = psb->m_nodes[j].m_v + psb->m_nodes[j].m_splitv - m_backupVelocity[counter];
}
psb->m_nodes[j].m_v = m_backupVelocity[counter];
++counter;
@@ -441,6 +436,7 @@ void btDeformableBodySolver::predictDeformableMotion(btSoftBody* psb, btScalar d
n.m_v *= max_v;
}
n.m_q = n.m_x + n.m_v * dt;
+ n.m_splitv.setZero();
n.m_penetration = 0;
}
diff --git a/src/BulletSoftBody/btDeformableBodySolver.h b/src/BulletSoftBody/btDeformableBodySolver.h
index d4e5f4c60..9d1ca9c43 100644
--- a/src/BulletSoftBody/btDeformableBodySolver.h
+++ b/src/BulletSoftBody/btDeformableBodySolver.h
@@ -67,9 +67,6 @@ public:
// solve the momentum equation
virtual void solveDeformableConstraints(btScalar solverdt);
-
- // set up the position error in split impulse
- void splitImpulseSetup(const btContactSolverInfo& infoGlobal);
// resize/clear data structures
void reinitialize(const btAlignedObjectArray& softBodies, btScalar dt);
diff --git a/src/BulletSoftBody/btDeformableContactConstraint.cpp b/src/BulletSoftBody/btDeformableContactConstraint.cpp
index 2864446de..dda9d5ef6 100644
--- a/src/BulletSoftBody/btDeformableContactConstraint.cpp
+++ b/src/BulletSoftBody/btDeformableContactConstraint.cpp
@@ -142,13 +142,16 @@ btDeformableRigidContactConstraint::btDeformableRigidContactConstraint(const btS
m_total_tangent_dv.setZero();
// The magnitude of penetration is the depth of penetration.
m_penetration = c.m_cti.m_offset;
-// m_penetration = btMin(btScalar(0),c.m_cti.m_offset);
+ m_total_split_impulse = 0;
+ m_binding = false;
}
btDeformableRigidContactConstraint::btDeformableRigidContactConstraint(const btDeformableRigidContactConstraint& other)
: m_contact(other.m_contact)
, btDeformableContactConstraint(other)
, m_penetration(other.m_penetration)
+, m_total_split_impulse(other.m_total_split_impulse)
+, m_binding(other.m_binding)
{
m_total_normal_dv = other.m_total_normal_dv;
m_total_tangent_dv = other.m_total_tangent_dv;
@@ -213,12 +216,28 @@ btScalar btDeformableRigidContactConstraint::solveConstraint(const btContactSolv
btVector3 va = getVa();
btVector3 vb = getVb();
btVector3 vr = vb - va;
- btScalar dn = btDot(vr, cti.m_normal) + m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep;
+ btScalar dn = btDot(vr, cti.m_normal);
+ if (!infoGlobal.m_splitImpulse)
+ {
+ dn += m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep;
+ }
// dn is the normal component of velocity diffrerence. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt
- btScalar residualSquare = dn*dn;
- btVector3 impulse = m_contact->m_c0 * (vr + m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep * cti.m_normal) ;
- const btVector3 impulse_normal = m_contact->m_c0 * (cti.m_normal * dn);
+ btVector3 impulse = m_contact->m_c0 * vr;
+ if (!infoGlobal.m_splitImpulse)
+ {
+ impulse += m_contact->m_c0 * (m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep * cti.m_normal);
+ }
+ btVector3 impulse_normal = m_contact->m_c0 * (cti.m_normal * dn);
btVector3 impulse_tangent = impulse - impulse_normal;
+ if (dn > 0)
+ {
+ dn = 0;
+ impulse_normal.setZero();
+ m_binding = false;
+ return 0;
+ }
+ m_binding = true;
+ btScalar residualSquare = dn*dn;
btVector3 old_total_tangent_dv = m_total_tangent_dv;
// m_c2 is the inverse mass of the deformable node/face
m_total_normal_dv -= impulse_normal * m_contact->m_c2;
@@ -228,7 +247,6 @@ btScalar btDeformableRigidContactConstraint::solveConstraint(const btContactSolv
{
// separating in the normal direction
m_static = false;
- m_total_tangent_dv = btVector3(0,0,0);
impulse_tangent.setZero();
}
else
@@ -257,8 +275,6 @@ btScalar btDeformableRigidContactConstraint::solveConstraint(const btContactSolv
impulse = impulse_normal + impulse_tangent;
// apply impulse to deformable nodes involved and change their velocities
applyImpulse(impulse);
- if (residualSquare < 1e-7)
- return residualSquare;
// apply impulse to the rigid/multibodies involved and change their velocities
if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
{
@@ -288,13 +304,35 @@ btScalar btDeformableRigidContactConstraint::solveConstraint(const btContactSolv
}
}
}
-// va = getVa();
-// vb = getVb();
-// vr = vb - va;
-// btScalar dn1 = btDot(vr, cti.m_normal) / 150;
-// m_penetration += dn1;
return residualSquare;
}
+
+btScalar btDeformableRigidContactConstraint::solveSplitImpulse(const btContactSolverInfo& infoGlobal)
+{
+ btScalar MAX_PENETRATION_CORRECTION = 0.1;
+ const btSoftBody::sCti& cti = m_contact->m_cti;
+ btVector3 vb = getSplitVb();
+ btScalar p = m_penetration;
+ btScalar dn = btDot(vb, cti.m_normal) + p * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep;
+ if (dn > 0)
+ {
+ return 0;
+ }
+ if (m_total_split_impulse + dn > MAX_PENETRATION_CORRECTION)
+ {
+ dn = MAX_PENETRATION_CORRECTION - m_total_split_impulse;
+ }
+ if (m_total_split_impulse + dn < -MAX_PENETRATION_CORRECTION)
+ {
+ dn = -MAX_PENETRATION_CORRECTION - m_total_split_impulse;
+ }
+ m_total_split_impulse += dn;
+
+ btScalar residualSquare = dn*dn;
+ const btVector3 impulse = 1.0/m_contact->m_c2 * (cti.m_normal * dn);
+ applySplitImpulse(impulse);
+ return residualSquare;
+}
/* ================ Node vs. Rigid =================== */
btDeformableNodeRigidContactConstraint::btDeformableNodeRigidContactConstraint(const btSoftBody::DeformableNodeRigidContact& contact, const btContactSolverInfo& infoGlobal)
: m_node(contact.m_node)
@@ -313,6 +351,10 @@ btVector3 btDeformableNodeRigidContactConstraint::getVb() const
return m_node->m_v;
}
+btVector3 btDeformableNodeRigidContactConstraint::getSplitVb() const
+{
+ return m_node->m_splitv;
+}
btVector3 btDeformableNodeRigidContactConstraint::getDv(const btSoftBody::Node* node) const
{
@@ -326,6 +368,13 @@ void btDeformableNodeRigidContactConstraint::applyImpulse(const btVector3& impul
contact->m_node->m_v -= dv;
}
+void btDeformableNodeRigidContactConstraint::applySplitImpulse(const btVector3& impulse)
+{
+ const btSoftBody::DeformableNodeRigidContact* contact = getContact();
+ btVector3 dv = impulse * contact->m_c2;
+ contact->m_node->m_splitv -= dv;
+}
+
/* ================ Face vs. Rigid =================== */
btDeformableFaceRigidContactConstraint::btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact, const btContactSolverInfo& infoGlobal, bool useStrainLimiting)
: m_face(contact.m_face)
@@ -441,6 +490,40 @@ void btDeformableFaceRigidContactConstraint::applyImpulse(const btVector3& impul
}
}
+
+btVector3 btDeformableFaceRigidContactConstraint::getSplitVb() const
+{
+ const btSoftBody::DeformableFaceRigidContact* contact = getContact();
+ btVector3 vb = (m_face->m_n[0]->m_splitv) * contact->m_bary[0] + (m_face->m_n[1]->m_splitv) * contact->m_bary[1] + (m_face->m_n[2]->m_splitv)* contact->m_bary[2];
+ return vb;
+}
+
+void btDeformableFaceRigidContactConstraint::applySplitImpulse(const btVector3& impulse)
+{
+ const btSoftBody::DeformableFaceRigidContact* contact = getContact();
+ btVector3 dv = impulse * contact->m_c2;
+ btSoftBody::Face* face = contact->m_face;
+ btVector3& v0 = face->m_n[0]->m_splitv;
+ btVector3& v1 = face->m_n[1]->m_splitv;
+ btVector3& v2 = face->m_n[2]->m_splitv;
+ const btScalar& im0 = face->m_n[0]->m_im;
+ const btScalar& im1 = face->m_n[1]->m_im;
+ const btScalar& im2 = face->m_n[2]->m_im;
+ if (im0 > 0)
+ {
+ v0 -= dv * contact->m_weights[0];
+ }
+ if (im1 > 0)
+ {
+ v1 -= dv * contact->m_weights[1];
+ }
+ if (im2 > 0)
+ {
+ v2 -= dv * contact->m_weights[2];
+ }
+}
+
+
/* ================ Face vs. Node =================== */
btDeformableFaceNodeContactConstraint::btDeformableFaceNodeContactConstraint(const btSoftBody::DeformableFaceNodeContact& contact, const btContactSolverInfo& infoGlobal)
: m_node(contact.m_node)
diff --git a/src/BulletSoftBody/btDeformableContactConstraint.h b/src/BulletSoftBody/btDeformableContactConstraint.h
index 9f9d5bf0a..9c4f58ca1 100644
--- a/src/BulletSoftBody/btDeformableContactConstraint.h
+++ b/src/BulletSoftBody/btDeformableContactConstraint.h
@@ -148,6 +148,8 @@ public:
btVector3 m_total_normal_dv;
btVector3 m_total_tangent_dv;
btScalar m_penetration;
+ btScalar m_total_split_impulse;
+ bool m_binding;
const btSoftBody::DeformableRigidContact* m_contact;
btDeformableRigidContactConstraint(const btSoftBody::DeformableRigidContact& c, const btContactSolverInfo& infoGlobal);
@@ -160,12 +162,19 @@ public:
// object A is the rigid/multi body, and object B is the deformable node/face
virtual btVector3 getVa() const;
+ // get the split impulse velocity of the deformable face at the contact point
+ virtual btVector3 getSplitVb() const = 0;
+
virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal);
virtual void setPenetrationScale(btScalar scale)
{
m_penetration *= scale;
}
+
+ btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal);
+
+ virtual void applySplitImpulse(const btVector3& impulse) = 0;
};
//
@@ -186,6 +195,9 @@ public:
// get the velocity of the deformable node in contact
virtual btVector3 getVb() const;
+ // get the split impulse velocity of the deformable face at the contact point
+ virtual btVector3 getSplitVb() const;
+
// get the velocity change of the input soft body node in the constraint
virtual btVector3 getDv(const btSoftBody::Node*) const;
@@ -196,6 +208,8 @@ public:
}
virtual void applyImpulse(const btVector3& impulse);
+
+ virtual void applySplitImpulse(const btVector3& impulse);
};
//
@@ -215,9 +229,12 @@ public:
// get the velocity of the deformable face at the contact point
virtual btVector3 getVb() const;
+ // get the split impulse velocity of the deformable face at the contact point
+ virtual btVector3 getSplitVb() const;
+
// get the velocity change of the input soft body node in the constraint
virtual btVector3 getDv(const btSoftBody::Node*) const;
-
+
// cast the contact to the desired type
const btSoftBody::DeformableFaceRigidContact* getContact() const
{
@@ -225,6 +242,8 @@ public:
}
virtual void applyImpulse(const btVector3& impulse);
+
+ virtual void applySplitImpulse(const btVector3& impulse);
};
//
diff --git a/src/BulletSoftBody/btDeformableContactProjection.cpp b/src/BulletSoftBody/btDeformableContactProjection.cpp
index 22ca8bf58..4e88c79d9 100644
--- a/src/BulletSoftBody/btDeformableContactProjection.cpp
+++ b/src/BulletSoftBody/btDeformableContactProjection.cpp
@@ -58,23 +58,27 @@ btScalar btDeformableContactProjection::update(btCollisionObject** deformableBod
return residualSquare;
}
-void btDeformableContactProjection::splitImpulseSetup(const btContactSolverInfo& infoGlobal)
+btScalar btDeformableContactProjection::solveSplitImpulse(btCollisionObject** deformableBodies,int numDeformableBodies, const btContactSolverInfo& infoGlobal)
{
- 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 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_faceRigidConstraints[j].size(); ++k)
+ {
+ btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[j][k];
+ btScalar localResidualSquare = constraint.solveSplitImpulse(infoGlobal);
+ residualSquare = btMax(residualSquare, localResidualSquare);
+ }
+ }
+ }
+ return residualSquare;
}
void btDeformableContactProjection::setConstraints(const btContactSolverInfo& infoGlobal)
@@ -143,15 +147,16 @@ void btDeformableContactProjection::setConstraints(const btContactSolverInfo& in
continue;
}
btDeformableFaceRigidContactConstraint constraint(contact, infoGlobal, m_useStrainLimiting);
- 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);
- }
+ m_faceRigidConstraints[i].push_back(constraint);
+ // 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);
+ // }
}
}
}
@@ -499,6 +504,10 @@ void btDeformableContactProjection::setLagrangeMultiplier()
}
for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
{
+ if (!m_nodeRigidConstraints[i][j].m_binding)
+ {
+ continue;
+ }
int index = m_nodeRigidConstraints[i][j].m_node->index;
m_nodeRigidConstraints[i][j].m_node->m_penetration = -m_nodeRigidConstraints[i][j].getContact()->m_cti.m_offset;
LagrangeMultiplier lm;
@@ -521,6 +530,10 @@ void btDeformableContactProjection::setLagrangeMultiplier()
}
for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j)
{
+ if (!m_faceRigidConstraints[i][j].m_binding)
+ {
+ continue;
+ }
const btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face;
btVector3 bary = m_faceRigidConstraints[i][j].getContact()->m_bary;
diff --git a/src/BulletSoftBody/btDeformableContactProjection.h b/src/BulletSoftBody/btDeformableContactProjection.h
index 8d7e94d4f..554c693ba 100644
--- a/src/BulletSoftBody/btDeformableContactProjection.h
+++ b/src/BulletSoftBody/btDeformableContactProjection.h
@@ -91,7 +91,7 @@ public:
virtual void reinitialize(bool nodeUpdated);
- virtual void splitImpulseSetup(const btContactSolverInfo& infoGlobal);
+ btScalar solveSplitImpulse(btCollisionObject** deformableBodies,int numDeformableBodies, const btContactSolverInfo& infoGlobal);
virtual void setLagrangeMultiplier();
diff --git a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp
index c8cc47923..e289d87a5 100644
--- a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp
+++ b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp
@@ -21,7 +21,7 @@ btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(b
{
{
///this is a special step to resolve penetrations (just for contacts)
- solveGroupCacheFriendlySplitImpulseIterations(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
+ solveGroupCacheFriendlySplitImpulseIterations(bodies, numBodies, deformableBodies, numDeformableBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations ? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations;
for (int iteration = 0; iteration < maxIterations; iteration++)
@@ -41,7 +41,8 @@ btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(b
if (m_leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || (iteration >= (maxIterations - 1)))
{
#ifdef VERBOSE_RESIDUAL_PRINTF
- printf("residual = %f at iteration #%d\n", m_leastSquaresResidual, iteration);
+ if (iteration >= (maxIterations - 1))
+ printf("residual = %f at iteration #%d\n", m_leastSquaresResidual, iteration);
#endif
m_analyticsData.m_numSolverCalls++;
m_analyticsData.m_numIterationsUsed = iteration+1;
@@ -105,14 +106,13 @@ void btDeformableMultiBodyConstraintSolver::solverBodyWriteBack(const btContactS
}
}
-void btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
+void btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies, int numBodies, btCollisionObject** deformableBodies,int numDeformableBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
{
BT_PROFILE("solveGroupCacheFriendlySplitImpulseIterations");
int iteration;
if (infoGlobal.m_splitImpulse)
{
{
-// m_deformableSolver->splitImpulseSetup(infoGlobal);
for (iteration = 0; iteration < infoGlobal.m_numIterations; iteration++)
{
btScalar leastSquaresResidual = 0.f;
@@ -128,12 +128,14 @@ void btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlySplitImpulseI
}
// solve the position correction between deformable and rigid/multibody
// btScalar residual = m_deformableSolver->solveSplitImpulse(infoGlobal);
-// leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
+ btScalar residual = m_deformableSolver->m_objective->m_projection.solveSplitImpulse(deformableBodies, numDeformableBodies, infoGlobal);
+ leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
}
if (leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || iteration >= (infoGlobal.m_numIterations - 1))
{
#ifdef VERBOSE_RESIDUAL_PRINTF
- printf("residual = %f at iteration #%d\n", leastSquaresResidual, iteration);
+ if (iteration >= (infoGlobal.m_numIterations - 1))
+ printf("split impulse residual = %f at iteration #%d\n", leastSquaresResidual, iteration);
#endif
break;
}
diff --git a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h
index 0c7cc26a8..984a026b6 100644
--- a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h
+++ b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h
@@ -44,7 +44,7 @@ protected:
// write the velocity of the underlying rigid body to the the the solver body
void writeToSolverBody(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal);
- virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer);
+ virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies, int numBodies, btCollisionObject** deformableBodies,int numDeformableBodies, 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:
diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp
index 6b742978e..3fc3ce964 100644
--- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp
+++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp
@@ -61,7 +61,7 @@ m_deformableBodySolver(deformableBodySolver), m_solverCallback(0)
m_internalTime = 0.0;
m_implicit = false;
m_lineSearch = false;
- m_useProjection = true;
+ m_useProjection = false;
m_ccdIterations = 5;
m_solverDeformableBodyIslandCallback = new DeformableBodyInplaceSolverIslandCallback(constraintSolver, dispatcher);
}
@@ -315,7 +315,7 @@ void btDeformableMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
node.m_v[c] = -clampDeltaV;
}
}
- node.m_x = node.m_x + timeStep * node.m_v;
+ node.m_x = node.m_x + timeStep * (node.m_v + node.m_splitv);
node.m_q = node.m_x;
node.m_vn = node.m_v;
}
@@ -537,6 +537,7 @@ void btDeformableMultiBodyDynamicsWorld::reinitialize(btScalar timeStep)
}
else
{
+ m_deformableBodySolver->m_useProjection = false;
m_deformableBodySolver->m_objective->m_preconditioner = m_deformableBodySolver->m_objective->m_KKTPreconditioner;
}
diff --git a/src/BulletSoftBody/btKrylovSolver.h b/src/BulletSoftBody/btKrylovSolver.h
new file mode 100644
index 000000000..61954bd60
--- /dev/null
+++ b/src/BulletSoftBody/btKrylovSolver.h
@@ -0,0 +1,108 @@
+/*
+ Written by Xuchen Han
+
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+#ifndef BT_KRYLOV_SOLVER_H
+#define BT_KRYLOV_SOLVER_H
+#include
+#include
+#include
+#include
+#include
+#include
+#include "LinearMath/btQuickprof.h"
+
+template
+class btKrylovSolver
+{
+ typedef btAlignedObjectArray TVStack;
+public:
+ int m_maxIterations;
+ btScalar m_tolerance;
+ btKrylovSolver(int maxIterations, btScalar tolerance)
+ : m_maxIterations(maxIterations)
+ , m_tolerance(tolerance)
+ {
+
+ }
+
+ virtual ~btKrylovSolver(){}
+
+ virtual int solve(MatrixX& A, TVStack& x, const TVStack& b, bool verbose = false) = 0;
+
+ virtual void reinitialize(const TVStack& b) = 0;
+
+ virtual SIMD_FORCE_INLINE TVStack sub(const TVStack& a, const TVStack& b)
+ {
+ // c = a-b
+ btAssert(a.size() == b.size());
+ TVStack c;
+ c.resize(a.size());
+ for (int i = 0; i < a.size(); ++i)
+ {
+ c[i] = a[i] - b[i];
+ }
+ return c;
+ }
+
+ virtual SIMD_FORCE_INLINE btScalar squaredNorm(const TVStack& a)
+ {
+ return dot(a,a);
+ }
+
+ virtual SIMD_FORCE_INLINE btScalar norm(const TVStack& a)
+ {
+ btScalar ret = 0;
+ for (int i = 0; i < a.size(); ++i)
+ {
+ for (int d = 0; d < 3; ++d)
+ {
+ ret = btMax(ret, btFabs(a[i][d]));
+ }
+ }
+ return ret;
+ }
+
+ virtual SIMD_FORCE_INLINE btScalar dot(const TVStack& a, const TVStack& b)
+ {
+ btScalar ans(0);
+ for (int i = 0; i < a.size(); ++i)
+ ans += a[i].dot(b[i]);
+ return ans;
+ }
+
+ virtual SIMD_FORCE_INLINE void multAndAddTo(btScalar s, const TVStack& a, TVStack& result)
+ {
+ // result += s*a
+ btAssert(a.size() == result.size());
+ for (int i = 0; i < a.size(); ++i)
+ result[i] += s * a[i];
+ }
+
+ virtual SIMD_FORCE_INLINE TVStack multAndAdd(btScalar s, const TVStack& a, const TVStack& b)
+ {
+ // result = a*s + b
+ TVStack result;
+ result.resize(a.size());
+ for (int i = 0; i < a.size(); ++i)
+ result[i] = s * a[i] + b[i];
+ return result;
+ }
+
+ virtual SIMD_FORCE_INLINE void setTolerance(btScalar tolerance)
+ {
+ m_tolerance = tolerance;
+ }
+};
+#endif /* BT_KRYLOV_SOLVER_H */
diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp
index 81b846d7f..e70e94f5f 100644
--- a/src/BulletSoftBody/btSoftBody.cpp
+++ b/src/BulletSoftBody/btSoftBody.cpp
@@ -2911,6 +2911,7 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO
//
// regular face contact
+ if (0)
{
btGjkEpaSolver2::sResults results;
btTransform triangle_transform;
@@ -2932,9 +2933,34 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO
cti.m_normal = nrm;
cti.m_offset = dst + (curr - contact_point).dot(nrm);
}
- return (dst < 0);
+ if(1)
+ {
+ btGjkEpaSolver2::sResults results;
+ btTransform triangle_transform;
+ triangle_transform.setIdentity();
+ triangle_transform.setOrigin(f.m_n[0]->m_q);
+ btTriangleShape triangle(btVector3(0,0,0), f.m_n[1]->m_q-f.m_n[0]->m_q, f.m_n[2]->m_q-f.m_n[0]->m_q);
+ btVector3 guess(0,0,0);
+ const btConvexShape* csh = static_cast(shp);
+ btGjkEpaSolver2::SignedDistance(&triangle, triangle_transform, csh, wtr, guess, results);
+ dst = results.distance-csh->getMargin();
+ dst -= margin;
+ if (dst >= 0)
+ return false;
+ wtr = colObjWrap->getWorldTransform();
+ btTriangleShape triangle2(btVector3(0,0,0), f.m_n[1]->m_x-f.m_n[0]->m_x, f.m_n[2]->m_x-f.m_n[0]->m_x);
+ triangle_transform.setOrigin(f.m_n[0]->m_x);
+ btGjkEpaSolver2::SignedDistance(&triangle2, triangle_transform, csh, wtr, guess, results);
+ contact_point = results.witnesses[0];
+ getBarycentric(contact_point, f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary);
+ dst = results.distance-csh->getMargin()-margin;
+ cti.m_colObj = colObjWrap->getCollisionObject();
+ cti.m_normal = results.normal;
+ cti.m_offset = dst;
+ return true;
+ }
+ return true;
}
-
//
void btSoftBody::updateNormals()
{
diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h
index 6a55eccbd..954adf4d1 100644
--- a/src/BulletSoftBody/btSoftBody.h
+++ b/src/BulletSoftBody/btSoftBody.h
@@ -271,7 +271,8 @@ public:
btDbvtNode* m_leaf; // Leaf data
btScalar m_penetration; // depth of penetration
int m_battach : 1; // Attached
- int index;
+ int index;
+ btVector3 m_splitv; // velocity associated with split impulse
};
/* Link */
ATTRIBUTE_ALIGNED16(struct)
@@ -296,7 +297,7 @@ public:
btDbvtNode* m_leaf; // Leaf data
btVector4 m_pcontact; // barycentric weights of the persistent contact
btVector3 m_n0, m_n1, m_vn;
- int m_index;
+ int m_index;
};
/* Tetra */
struct Tetra : Feature