AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2025-01-18 21:10:05 +00:00
Code Issues Releases Wiki Activity

Merge pull request #2800 from xhan0619/splitImpulseMulti

Browse Source 
Stability improvements for deformable.
This commit is contained in:
erwincoumans 2020-05-21 09:47:33 -07:00 committed by GitHub
commit 5233b72160
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
34 changed files with 297 additions and 177 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
data/torus_deform.urdf
View File

@ -6,7 +6,7 @@
<inertia ixx="0.0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
</inertial>
<collision_margin value="0.006"/>
<repulsion_stiffness value="50.0"/>
<repulsion_stiffness value="800.0"/>
<friction value= "0.5"/>
<neohookean mu= "60" lambda= "200" damping= "0.01" />
<visual filename="torus.vtk"/>

4
examples/DeformableDemo/ClothFriction.cpp
View File

@ -144,7 +144,7 @@ void ClothFriction::initPhysics()
10,10,
0, true);
psb->getCollisionShape()->setMargin(0.05);
psb->getCollisionShape()->setMargin(0.06);
psb->generateBendingConstraints(2);
psb->setTotalMass(1);
psb->setSpringStiffness(100);
@ -173,7 +173,7 @@ void ClothFriction::initPhysics()
btVector3(+s, h, +s),
5,5,
0, true);
psb2->getCollisionShape()->setMargin(0.05);
psb2->getCollisionShape()->setMargin(0.06);
psb2->generateBendingConstraints(2);
psb2->setTotalMass(1);
psb2->setSpringStiffness(100);

4
examples/DeformableDemo/DeformableContact.cpp
View File

@ -143,7 +143,7 @@ void DeformableContact::initPhysics()
20,20,
1 + 2 + 4 + 8, true);
psb->getCollisionShape()->setMargin(0.1);
psb->getCollisionShape()->setMargin(0.05);
psb->generateBendingConstraints(2);
psb->setSpringStiffness(10);
psb->setTotalMass(1);
@ -172,7 +172,7 @@ void DeformableContact::initPhysics()
btVector3(+s, h, +s),
10,10,
0, true);
psb2->getCollisionShape()->setMargin(0.1);
psb2->getCollisionShape()->setMargin(0.05);
psb2->generateBendingConstraints(2);
psb2->setSpringStiffness(10);
psb2->setTotalMass(1);

3
examples/DeformableDemo/DeformableMultibody.cpp
View File

@ -192,7 +192,7 @@ void DeformableMultibody::initPhysics()
// 3,3,
1 + 2 + 4 + 8, true);
psb->getCollisionShape()->setMargin(0.25);
psb->getCollisionShape()->setMargin(0.025);
psb->generateBendingConstraints(2);
psb->setTotalMass(1);
psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
@ -341,6 +341,7 @@ void DeformableMultibody::addColliders_testMultiDof(btMultiBody* pMultiBody, btM
btScalar quat[4] = {-world_to_local[0].x(), -world_to_local[0].y(), -world_to_local[0].z(), world_to_local[0].w()};
btCollisionShape* box = new btBoxShape(baseHalfExtents);
box->setMargin(0.01);
btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, -1);
col->setCollisionShape(box);

44
examples/DeformableDemo/DeformableRigid.cpp
View File

@ -53,6 +53,48 @@ public:
//use a smaller internal timestep, there are stability issues
float internalTimeStep = 1. / 240.f;
m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep);
//
// btCollisionShape* boxShape = new btBoxShape(btVector3(1, 1, 1));
// boxShape->setMargin(1e-3);
// if (0)
// {
// btVector3 p(0.99,1.01,0.99);
// for (int i = 0; i < 40; ++i)
// {
// p[1] -= 0.001;
// btScalar margin(.000001);
// btTransform trans;
// trans.setIdentity();
// btGjkEpaSolver2::sResults results;
// const btConvexShape* csh = static_cast<const btConvexShape*>(boxShape);
// btScalar d = btGjkEpaSolver2::SignedDistance(p, margin, csh, trans, results);
// printf("d = %f\n", d);
// printf("----\n");
// }
// }
//
// btVector3 p(.991,1.01,.99);
// for (int i = 0; i < 40; ++i)
// {
// p[1] -= 0.001;
// btScalar margin(.006);
// btTransform trans;
// trans.setIdentity();
// btScalar dst;
// btGjkEpaSolver2::sResults results;
// btTransform point_transform;
// point_transform.setIdentity();
// point_transform.setOrigin(p);
// btSphereShape sphere(margin);
// btVector3 guess(0,0,0);
// const btConvexShape* csh = static_cast<const btConvexShape*>(boxShape);
// btGjkEpaSolver2::SignedDistance(&sphere, point_transform, csh, trans, guess, results);
// dst = results.distance-csh->getMargin();
// dst -= margin;
// printf("d = %f\n", dst);
// printf("----\n");
// }
}
void Ctor_RbUpStack(int count)
@ -214,7 +256,7 @@ void DeformableRigid::initPhysics()
2,2,
0, true);
psb->getCollisionShape()->setMargin(0.1);
psb->getCollisionShape()->setMargin(0.05);
psb->generateBendingConstraints(2);
psb->setTotalMass(1);
psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects

8
examples/DeformableDemo/DeformableSelfCollision.cpp
View File

@ -41,7 +41,7 @@ public:
void resetCamera()
{
float dist = 1.0;
float dist = 2.0;
float pitch = -8;
float yaw = 100;
float targetPos[3] = {0, -1.0, 0};
@ -93,7 +93,7 @@ void DeformableSelfCollision::initPhysics()
{
///create a ground
btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(150.), btScalar(2.5), btScalar(150.)));
groundShape->setMargin(0.0001);
m_collisionShapes.push_back(groundShape);
btTransform groundTransform;
@ -128,7 +128,7 @@ void DeformableSelfCollision::initPhysics()
void DeformableSelfCollision::addCloth(btVector3 origin)
// create a piece of cloth
{
const btScalar s = 0.3;
const btScalar s = 0.6;
const btScalar h = 0;
btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, h, -2*s),
@ -140,7 +140,7 @@ void DeformableSelfCollision::addCloth(btVector3 origin)
0, true, 0.0);
psb->getCollisionShape()->setMargin(0.01);
psb->getCollisionShape()->setMargin(0.02);
psb->generateBendingConstraints(2);
psb->setTotalMass(.5);
psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects

11
examples/DeformableDemo/GraspDeformable.cpp
View File

@ -198,6 +198,9 @@ void GraspDeformable::initPhysics()
btVector3 gravity = btVector3(0, -9.81, 0);
m_dynamicsWorld->setGravity(gravity);
getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity;
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_erp = 0.1;
getDeformableDynamicsWorld()->getSolverInfo().m_deformable_cfm = 0;
getDeformableDynamicsWorld()->getSolverInfo().m_numIterations = 150;
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
m_maxPickingForce = 0.001;
// build a gripper
@ -253,7 +256,7 @@ void GraspDeformable::initPhysics()
//create a ground
{
btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(10.), btScalar(5.), btScalar(10.)));
groundShape->setMargin(0.001);
m_collisionShapes.push_back(groundShape);
btTransform groundTransform;
@ -352,17 +355,17 @@ void GraspDeformable::initPhysics()
2,2,
0, true);
psb->getCollisionShape()->setMargin(0.003);
psb->getCollisionShape()->setMargin(0.001);
psb->generateBendingConstraints(2);
psb->setTotalMass(0.005);
psb->setTotalMass(0.01);
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 = 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::SDF_RDF;
getDeformableDynamicsWorld()->addSoftBody(psb);
getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce(0.05,0.005, true));
getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity*0.1));

2
examples/DeformableDemo/MultibodyClothAnchor.cpp
View File

@ -143,7 +143,7 @@ void MultibodyClothAnchor::initPhysics()
btVector3(+s, h, -s),
btVector3(-s, h, +s),
btVector3(+s, h, +s), r, r, 4 + 8, true);
psb->getCollisionShape()->setMargin(0.1);
psb->getCollisionShape()->setMargin(0.01);
psb->generateBendingConstraints(2);
psb->setTotalMass(1);
psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects

2
examples/DeformableDemo/Pinch.cpp
View File

@ -296,7 +296,7 @@ void Pinch::initPhysics()
psb->scale(btVector3(2, 2, 2));
psb->translate(btVector3(0, 4, 0));
psb->getCollisionShape()->setMargin(0.1);
psb->getCollisionShape()->setMargin(0.01);
psb->setTotalMass(1);
psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
psb->m_cfg.kCHR = 1; // collision hardness with rigid body

6
examples/DeformableDemo/PinchFriction.cpp
View File

@ -265,7 +265,7 @@ void PinchFriction::initPhysics()
psb->scale(btVector3(2, 2, 1));
psb->translate(btVector3(0, 2.1, 2.2));
psb->getCollisionShape()->setMargin(0.05);
psb->getCollisionShape()->setMargin(0.025);
psb->setSpringStiffness(10);
psb->setTotalMass(.6);
psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
@ -296,7 +296,7 @@ void PinchFriction::initPhysics()
psb2->scale(btVector3(2, 2, 1));
psb2->translate(btVector3(0, 2.1, -2.2));
psb2->getCollisionShape()->setMargin(0.05);
psb2->getCollisionShape()->setMargin(0.025);
psb2->setTotalMass(.6);
psb2->m_cfg.kKHR = 1; // collision hardness with kinematic objects
psb2->m_cfg.kCHR = 1; // collision hardness with rigid body
@ -327,7 +327,7 @@ void PinchFriction::initPhysics()
psb3->scale(btVector3(2, 2, 1));
psb3->translate(btVector3(0, 2.1, 0));
psb3->getCollisionShape()->setMargin(0.05);
psb3->getCollisionShape()->setMargin(0.025);
psb3->setTotalMass(.6);
psb3->setSpringStiffness(10);
psb3->m_cfg.kKHR = 1; // collision hardness with kinematic objects

2
examples/DeformableDemo/SplitImpulse.cpp
View File

@ -160,7 +160,7 @@ void SplitImpulse::initPhysics()
// 0, true);
psb->getCollisionShape()->setMargin(0.15);
psb->getCollisionShape()->setMargin(0.015);
psb->generateBendingConstraints(2);
psb->setTotalMass(1);
psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects

2
examples/DeformableDemo/VolumetricDeformable.cpp
View File

@ -208,7 +208,7 @@ void VolumetricDeformable::initPhysics()
getDeformableDynamicsWorld()->addSoftBody(psb);
psb->scale(btVector3(2, 2, 2));
psb->translate(btVector3(0, 5, 0));
psb->getCollisionShape()->setMargin(0.25);
psb->getCollisionShape()->setMargin(0.025);
psb->setTotalMass(0.5);
psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
psb->m_cfg.kCHR = 1; // collision hardness with rigid body

8
examples/Importers/ImportURDFDemo/UrdfParser.h
View File

@ -205,10 +205,12 @@ struct SpringCoeffcients{
double elastic_stiffness;
double damping_stiffness;
double bending_stiffness;
int damp_all_directions;
SpringCoeffcients():
elastic_stiffness(0.),
damping_stiffness(0.),
bending_stiffness(0.){}
elastic_stiffness(0.),
damping_stiffness(0.),
bending_stiffness(0.),
damp_all_directions(0){}
};
struct LameCoefficients

13
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@ -381,8 +381,8 @@ B3_SHARED_API int b3LoadSoftBodyAddMassSpringForce(b3SharedMemoryCommandHandle c
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
b3Assert(command->m_type == CMD_LOAD_SOFT_BODY);
command->m_loadSoftBodyArguments.m_springElasticStiffness = springElasticStiffness;
command->m_loadSoftBodyArguments.m_springDampingStiffness = springDampingStiffness;
command->m_loadSoftBodyArguments.m_springElasticStiffness = springElasticStiffness;
command->m_loadSoftBodyArguments.m_springDampingStiffness = springDampingStiffness;
command->m_updateFlags |= LOAD_SOFT_BODY_ADD_MASS_SPRING_FORCE;
return 0;
}
@ -441,6 +441,15 @@ B3_SHARED_API int b3LoadSoftBodyUseBendingSprings(b3SharedMemoryCommandHandle co
return 0;
}
B3_SHARED_API int b3LoadSoftBodyUseAllDirectionDampingSprings(b3SharedMemoryCommandHandle commandHandle, int allDirectionDamping)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
b3Assert(command->m_type == CMD_LOAD_SOFT_BODY);
command->m_loadSoftBodyArguments.m_dampAllDirections = allDirectionDamping;
command->m_updateFlags |= LOAD_SOFT_BODY_SET_DAMPING_SPRING_MODE;
return 0;
}
B3_SHARED_API int b3LoadSoftBodyUseFaceContact(b3SharedMemoryCommandHandle commandHandle, int useFaceContact)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;

1
examples/SharedMemory/PhysicsClientC_API.h
View File

@ -657,6 +657,7 @@ extern "C"
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);
B3_SHARED_API int b3LoadSoftBodyUseAllDirectionDampingSprings(b3SharedMemoryCommandHandle commandHandle, int useAllDirectionDamping);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitCreateSoftBodyAnchorConstraintCommand(b3PhysicsClientHandle physClient, int softBodyUniqueId, int nodeIndex, int bodyUniqueId, int linkIndex, const double bodyFramePosition[3]);

8
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@ -8396,6 +8396,11 @@ void constructUrdfDeformable(const struct SharedMemoryCommand& clientCmd, UrdfDe
deformable.m_springCoefficients.bending_stiffness = loadSoftBodyArgs.m_springBendingStiffness;
}
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_SET_DAMPING_SPRING_MODE)
{
deformable.m_springCoefficients.damp_all_directions = loadSoftBodyArgs.m_dampAllDirections;
}
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_COROTATED_FORCE)
{
deformable.m_corotatedCoefficients.mu = loadSoftBodyArgs.m_corotatedMu;
@ -8501,7 +8506,8 @@ bool PhysicsServerCommandProcessor::processDeformable(const UrdfDeformable& defo
btDeformableLagrangianForce* springForce =
new btDeformableMassSpringForce(deformable.m_springCoefficients.elastic_stiffness,
deformable.m_springCoefficients.damping_stiffness,
true, deformable.m_springCoefficients.bending_stiffness);
!deformable.m_springCoefficients.damp_all_directions,
deformable.m_springCoefficients.bending_stiffness);
deformWorld->addForce(psb, springForce);
m_data->m_lf.push_back(springForce);
}

2
examples/SharedMemory/SharedMemoryCommands.h
View File

@ -514,6 +514,7 @@ enum EnumLoadSoftBodyUpdateFlags
LOAD_SOFT_BODY_USE_FACE_CONTACT = 1<<14,
LOAD_SOFT_BODY_SIM_MESH = 1<<15,
LOAD_SOFT_BODY_SET_REPULSION_STIFFNESS = 1<<16,
LOAD_SOFT_BODY_SET_DAMPING_SPRING_MODE = 1<<17,
};
enum EnumSimParamInternalSimFlags
@ -534,6 +535,7 @@ struct LoadSoftBodyArgs
double m_initialOrientation[4];
double m_springElasticStiffness;
double m_springDampingStiffness;
int m_dampAllDirections;
double m_springBendingStiffness;
double m_corotatedMu;
double m_corotatedLambda;

2
examples/pybullet/examples/deformable_anchor.py
View File

@ -15,7 +15,7 @@ planeId = p.loadURDF("plane.urdf", [0,0,-2],planeOrn)
boxId = p.loadURDF("cube.urdf", [0,1,2],useMaximalCoordinates = True)
clothId = p.loadSoftBody("cloth_z_up.obj", basePosition = [0,0,2], scale = 0.5, mass = 1., useNeoHookean = 0, useBendingSprings=1,useMassSpring=1, springElasticStiffness=40, springDampingStiffness=.1, useSelfCollision = 0, frictionCoeff = .5, useFaceContact=1)
clothId = p.loadSoftBody("cloth_z_up.obj", basePosition = [0,0,2], scale = 0.5, mass = 1., useNeoHookean = 0, useBendingSprings=1,useMassSpring=1, springElasticStiffness=40, springDampingStiffness=.1, springDampingAllDirections = 1, useSelfCollision = 0, frictionCoeff = .5, useFaceContact=1)
p.createSoftBodyAnchor(clothId ,0,-1,-1)

2
examples/pybullet/examples/deformable_torus.py
View File

@ -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", mass = 1, useNeoHookean = 1, NeoHookeanMu = 60, NeoHookeanLambda = 200, NeoHookeanDamping = 0.01, collisionMargin = 0.006, useSelfCollision = 1, frictionCoeff = 0.5, repulsionStiffness = 50)
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 = 800)
bunny2 = p.loadURDF("torus_deform.urdf", [0,1,0], flags=p.URDF_USE_SELF_COLLISION)

6
examples/pybullet/pybullet.c
View File

@ -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", "useMassSpring", "useBendingSprings", "useNeoHookean", "springElasticStiffness", "springDampingStiffness", "springBendingStiffness", "NeoHookeanMu", "NeoHookeanLambda", "NeoHookeanDamping", "frictionCoeff", "useFaceContact", "useSelfCollision", "repulsionStiffness", "physicsClientId", NULL};
static char* kwlist[] = {"fileName", "basePosition", "baseOrientation", "scale", "mass", "collisionMargin", "useMassSpring", "useBendingSprings", "useNeoHookean", "springElasticStiffness", "springDampingStiffness", "springDampingAllDirections", "springBendingStiffness", "NeoHookeanMu", "NeoHookeanLambda", "NeoHookeanDamping", "frictionCoeff", "useFaceContact", "useSelfCollision", "repulsionStiffness", "physicsClientId", NULL};
int bodyUniqueId = -1;
const char* fileName = "";
@ -2050,6 +2050,7 @@ static PyObject* pybullet_loadSoftBody(PyObject* self, PyObject* args, PyObject*
int useNeoHookean = 0;
double springElasticStiffness = 1;
double springDampingStiffness = 0.1;
int springDampingAllDirections = 0;
double springBendingStiffness = 0.1;
double NeoHookeanMu = 1;
double NeoHookeanLambda = 1;
@ -2068,7 +2069,7 @@ static PyObject* pybullet_loadSoftBody(PyObject* self, PyObject* args, PyObject*
PyObject* basePosObj = 0;
PyObject* baseOrnObj = 0;
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))
if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|OOdddiiiddidddddiidi", kwlist, &fileName, &basePosObj, &baseOrnObj, &scale, &mass, &collisionMargin, &useMassSpring, &useBendingSprings, &useNeoHookean, &springElasticStiffness, &springDampingStiffness, &springDampingAllDirections, &springBendingStiffness, &NeoHookeanMu, &NeoHookeanLambda, &NeoHookeanDamping, &frictionCoeff, &useFaceContact, &useSelfCollision, &repulsionStiffness, &physicsClientId))
{
return NULL;
}
@ -2125,6 +2126,7 @@ static PyObject* pybullet_loadSoftBody(PyObject* self, PyObject* args, PyObject*
{
b3LoadSoftBodyAddMassSpringForce(command, springElasticStiffness, springDampingStiffness);
b3LoadSoftBodyUseBendingSprings(command, useBendingSprings, springBendingStiffness);
b3LoadSoftBodyUseAllDirectionDampingSprings(command, springDampingAllDirections);
}
if (useNeoHookean)
{

4
src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h
View File

@ -47,6 +47,7 @@ struct btContactSolverInfoData
btScalar m_erp; //error reduction for non-contact constraints
btScalar m_erp2; //error reduction for contact constraints
btScalar m_deformable_erp; //error reduction for deformable constraints
btScalar m_deformable_cfm; //constraint force mixing for deformable constraints
btScalar m_globalCfm; //constraint force mixing for contacts and non-contacts
btScalar m_frictionERP; //error reduction for friction constraints
btScalar m_frictionCFM; //constraint force mixing for friction constraints
@ -83,7 +84,8 @@ struct btContactSolverInfo : public btContactSolverInfoData
m_numIterations = 10;
m_erp = btScalar(0.2);
m_erp2 = btScalar(0.2);
m_deformable_erp = btScalar(0.1);
m_deformable_erp = btScalar(0.06);
m_deformable_cfm = btScalar(0.01);
m_globalCfm = btScalar(0.);
m_frictionERP = btScalar(0.2); //positional friction 'anchors' are disabled by default
m_frictionCFM = btScalar(0.);

6
src/BulletDynamics/Dynamics/btRigidBody.h
View File

@ -465,6 +465,12 @@ public:
//for kinematic objects, we could also use use:
// return (m_worldTransform(rel_pos) - m_interpolationWorldTransform(rel_pos)) / m_kinematicTimeStep;
}
btVector3 getPushVelocityInLocalPoint(const btVector3& rel_pos) const
{
//we also calculate lin/ang velocity for kinematic objects
return m_pushVelocity + m_turnVelocity.cross(rel_pos);
}
void translate(const btVector3& v)
{

2
src/BulletDynamics/Featherstone/btMultiBody.cpp
View File

@ -344,6 +344,8 @@ void btMultiBody::finalizeMultiDof()
{
m_deltaV.resize(0);
m_deltaV.resize(6 + m_dofCount);
m_splitV.resize(0);
m_splitV.resize(6 + m_dofCount);
m_realBuf.resize(6 + m_dofCount + m_dofCount * m_dofCount + 6 + m_dofCount); //m_dofCount for joint-space vels + m_dofCount^2 for "D" matrices + delta-pos vector (6 base "vels" + joint "vels")
m_vectorBuf.resize(2 * m_dofCount); //two 3-vectors (i.e. one six-vector) for each system dof ("h" matrices)
m_matrixBuf.resize(m_links.size() + 1);

26
src/BulletDynamics/Featherstone/btMultiBody.h
View File

@ -278,6 +278,11 @@ public:
{
return &m_deltaV[0];
}
const btScalar *getSplitVelocityVector() const
{
return &m_splitV[0];
}
/* btScalar * getVelocityVector()
{
return &real_buf[0];
@ -397,6 +402,26 @@ public:
m_deltaV[dof] += delta_vee[dof] * multiplier;
}
}
void applyDeltaSplitVeeMultiDof(const btScalar *delta_vee, btScalar multiplier)
{
for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
{
m_splitV[dof] += delta_vee[dof] * multiplier;
}
}
void addSplitV()
{
applyDeltaVeeMultiDof(&m_splitV[0], 1);
}
void substractSplitV()
{
applyDeltaVeeMultiDof(&m_splitV[0], -1);
for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
{
m_splitV[dof] = 0.f;
}
}
void processDeltaVeeMultiDof2()
{
applyDeltaVeeMultiDof(&m_deltaV[0], 1);
@ -711,6 +736,7 @@ private:
// offset size array
// 0 num_links+1 rot_from_parent
//
btAlignedObjectArray<btScalar> m_splitV;
btAlignedObjectArray<btScalar> m_deltaV;
btAlignedObjectArray<btScalar> m_realBuf;
btAlignedObjectArray<btVector3> m_vectorBuf;

2
src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
View File

@ -592,6 +592,7 @@ void btMultiBodyDynamicsWorld::integrateMultiBodyTransforms(btScalar timeStep)
if (!isSleeping)
{
bod->addSplitV();
int nLinks = bod->getNumLinks();
///base + num m_links
@ -610,6 +611,7 @@ void btMultiBodyDynamicsWorld::integrateMultiBodyTransforms(btScalar timeStep)
m_scratch_world_to_local.resize(nLinks + 1);
m_scratch_local_origin.resize(nLinks + 1);
bod->updateCollisionObjectWorldTransforms(m_scratch_world_to_local, m_scratch_local_origin);
bod->substractSplitV();
}
else
{

2
src/BulletSoftBody/btConjugateResidual.h
View File

@ -29,7 +29,7 @@ class btConjugateResidual : public btKrylovSolver<MatrixX>
btScalar best_r;
public:
btConjugateResidual(const int max_it_in)
: Base(max_it_in, 1e-8)
: Base(max_it_in, 1e-4)
{
}

4
src/BulletSoftBody/btDeformableBodySolver.cpp
View File

@ -18,7 +18,7 @@
#include "btDeformableBodySolver.h"
#include "btSoftBodyInternals.h"
#include "LinearMath/btQuickprof.h"
static const int kMaxConjugateGradientIterations = 50;
static const int kMaxConjugateGradientIterations = 300;
btDeformableBodySolver::btDeformableBodySolver()
: m_numNodes(0)
, m_cg(kMaxConjugateGradientIterations)
@ -437,7 +437,7 @@ void btDeformableBodySolver::predictDeformableMotion(btSoftBody* psb, btScalar d
}
n.m_q = n.m_x + n.m_v * dt;
n.m_splitv.setZero();
n.m_penetration = 0;
n.m_constrained = false;
}
/* Nodes */

123
src/BulletSoftBody/btDeformableContactConstraint.cpp
View File

@ -210,34 +210,87 @@ btVector3 btDeformableRigidContactConstraint::getVa() const
return va;
}
btVector3 btDeformableRigidContactConstraint::getSplitVa() const
{
const btSoftBody::sCti& cti = m_contact->m_cti;
btVector3 va(0, 0, 0);
if (cti.m_colObj->hasContactResponse())
{
btRigidBody* rigidCol = 0;
btMultiBodyLinkCollider* multibodyLinkCol = 0;
// grab the velocity of the rigid body
if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
{
rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
va = rigidCol ? (rigidCol->getPushVelocityInLocalPoint(m_contact->m_c1)) : btVector3(0, 0, 0);
}
else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
{
multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
if (multibodyLinkCol)
{
const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
const btScalar* J_n = &m_contact->jacobianData_normal.m_jacobians[0];
const btScalar* J_t1 = &m_contact->jacobianData_t1.m_jacobians[0];
const btScalar* J_t2 = &m_contact->jacobianData_t2.m_jacobians[0];
const btScalar* local_split_v = multibodyLinkCol->m_multiBody->getSplitVelocityVector();
// add in the normal component of the va
btScalar vel = 0.0;
for (int k = 0; k < ndof; ++k)
{
vel += local_split_v[k] * J_n[k];
}
va = cti.m_normal * vel;
// add in the tangential components of the va
vel = 0.0;
for (int k = 0; k < ndof; ++k)
{
vel += local_split_v[k] * J_t1[k];
}
va += m_contact->t1 * vel;
vel = 0.0;
for (int k = 0; k < ndof; ++k)
{
vel += local_split_v[k] * J_t2[k];
}
va += m_contact->t2 * vel;
}
}
}
return va;
}
btScalar btDeformableRigidContactConstraint::solveConstraint(const btContactSolverInfo& infoGlobal)
{
const btSoftBody::sCti& cti = m_contact->m_cti;
btVector3 va = getVa();
btVector3 vb = getVb();
btVector3 vr = vb - va;
btScalar dn = btDot(vr, cti.m_normal);
if (!infoGlobal.m_splitImpulse)
{
dn += m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep;
}
btScalar dn = btDot(vr, cti.m_normal) + m_total_normal_dv.dot(cti.m_normal) * infoGlobal.m_deformable_cfm;
if (m_penetration > 0)
{
dn += m_penetration / infoGlobal.m_timeStep;
}
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
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 = m_contact->m_c0 * (vr + m_total_normal_dv * infoGlobal.m_deformable_cfm + ((m_penetration > 0) ? m_penetration / infoGlobal.m_timeStep * cti.m_normal : btVector3(0,0,0)));
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;
if (dn > 0)
{
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;
@ -312,8 +365,14 @@ btScalar btDeformableRigidContactConstraint::solveSplitImpulse(const btContactSo
btScalar MAX_PENETRATION_CORRECTION = 0.1;
const btSoftBody::sCti& cti = m_contact->m_cti;
btVector3 vb = getSplitVb();
btVector3 va = getSplitVa();
btScalar p = m_penetration;
btScalar dn = btDot(vb, cti.m_normal) + p * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep;
if (p > 0)
{
return 0;
}
btVector3 vr = vb - va;
btScalar dn = btDot(vr, cti.m_normal) + p * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep;
if (dn > 0)
{
return 0;
@ -329,8 +388,30 @@ btScalar btDeformableRigidContactConstraint::solveSplitImpulse(const btContactSo
m_total_split_impulse += dn;
btScalar residualSquare = dn*dn;
const btVector3 impulse = 1.0/m_contact->m_c2 * (cti.m_normal * dn);
const btVector3 impulse = m_contact->m_c0 * (cti.m_normal * dn);
applySplitImpulse(impulse);
// apply split impulse to the rigid/multibodies involved and change their velocities
if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
{
btRigidBody* rigidCol = 0;
rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
if (rigidCol)
{
rigidCol->applyPushImpulse(impulse, m_contact->m_c1);
}
}
else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
{
btMultiBodyLinkCollider* multibodyLinkCol = 0;
multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
if (multibodyLinkCol)
{
const btScalar* deltaV_normal = &m_contact->jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
// apply normal component of the impulse
multibodyLinkCol->m_multiBody->applyDeltaSplitVeeMultiDof(deltaV_normal, impulse.dot(cti.m_normal));
}
}
return residualSquare;
}
/* ================ Node vs. Rigid =================== */

3
src/BulletSoftBody/btDeformableContactConstraint.h
View File

@ -165,6 +165,9 @@ public:
// get the split impulse velocity of the deformable face at the contact point
virtual btVector3 getSplitVb() const = 0;
// get the split impulse velocity of the rigid/multibdoy at the contaft
virtual btVector3 getSplitVa() const;
virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal);
virtual void setPenetrationScale(btScalar scale)

38
src/BulletSoftBody/btDeformableContactProjection.cpp
View File

@ -126,15 +126,7 @@ void btDeformableContactProjection::setConstraints(const btContactSolverInfo& in
continue;
}
btDeformableNodeRigidContactConstraint constraint(contact, infoGlobal);
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);
}
m_nodeRigidConstraints[i].push_back(constraint);
}
// set Deformable Face vs. Rigid constraint
@ -229,7 +221,7 @@ void btDeformableContactProjection::setProjection()
for (int j = 0; j < m_staticConstraints[i].size(); ++j)
{
int index = m_staticConstraints[i][j].m_node->index;
m_staticConstraints[i][j].m_node->m_penetration = SIMD_INFINITY;
m_staticConstraints[i][j].m_node->m_constrained = true;
if (m_projectionsDict.find(index) == NULL)
{
m_projectionsDict.insert(index, units);
@ -246,7 +238,7 @@ void btDeformableContactProjection::setProjection()
for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j)
{
int index = m_nodeAnchorConstraints[i][j].m_anchor->m_node->index;
m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_penetration = SIMD_INFINITY;
m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_constrained = true;
if (m_projectionsDict.find(index) == NULL)
{
m_projectionsDict.insert(index, units);
@ -263,7 +255,7 @@ void btDeformableContactProjection::setProjection()
for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
{
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;
m_nodeRigidConstraints[i][j].m_node->m_constrained = true;
if (m_nodeRigidConstraints[i][j].m_static)
{
if (m_projectionsDict.find(index) == NULL)
@ -297,15 +289,16 @@ void btDeformableContactProjection::setProjection()
for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j)
{
const btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face;
btScalar penetration = -m_faceRigidConstraints[i][j].getContact()->m_cti.m_offset;
for (int k = 0; k < 3; ++k)
{
face->m_n[k]->m_penetration = btMax(face->m_n[k]->m_penetration, penetration);
}
if (m_faceRigidConstraints[i][j].m_binding)
{
for (int k = 0; k < 3; ++k)
{
face->m_n[k]->m_constrained = true;
}
}
for (int k = 0; k < 3; ++k)
{
btSoftBody::Node* node = face->m_n[k];
node->m_penetration = true;
int index = node->index;
if (m_faceRigidConstraints[i][j].m_static)
{
@ -477,7 +470,7 @@ void btDeformableContactProjection::setLagrangeMultiplier()
for (int j = 0; j < m_staticConstraints[i].size(); ++j)
{
int index = m_staticConstraints[i][j].m_node->index;
m_staticConstraints[i][j].m_node->m_penetration = SIMD_INFINITY;
m_staticConstraints[i][j].m_node->m_constrained = true;
LagrangeMultiplier lm;
lm.m_num_nodes = 1;
lm.m_indices[0] = index;
@ -491,7 +484,7 @@ void btDeformableContactProjection::setLagrangeMultiplier()
for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j)
{
int index = m_nodeAnchorConstraints[i][j].m_anchor->m_node->index;
m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_penetration = SIMD_INFINITY;
m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_constrained = true;
LagrangeMultiplier lm;
lm.m_num_nodes = 1;
lm.m_indices[0] = index;
@ -509,7 +502,7 @@ void btDeformableContactProjection::setLagrangeMultiplier()
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;
m_nodeRigidConstraints[i][j].m_node->m_constrained = true;
LagrangeMultiplier lm;
lm.m_num_nodes = 1;
lm.m_indices[0] = index;
@ -537,12 +530,11 @@ void btDeformableContactProjection::setLagrangeMultiplier()
const btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face;
btVector3 bary = m_faceRigidConstraints[i][j].getContact()->m_bary;
btScalar penetration = -m_faceRigidConstraints[i][j].getContact()->m_cti.m_offset;
LagrangeMultiplier lm;
lm.m_num_nodes = 3;
for (int k = 0; k<3; ++k)
{
face->m_n[k]->m_penetration = btMax(face->m_n[k]->m_penetration, penetration);
face->m_n[k]->m_constrained = true;
lm.m_indices[k] = face->m_n[k]->index;
lm.m_weights[k] = bary[k];
}

3
src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp
View File

@ -532,12 +532,13 @@ void btDeformableMultiBodyDynamicsWorld::reinitialize(btScalar timeStep)
if (m_useProjection)
{
m_deformableBodySolver->m_useProjection = true;
// m_deformableBodySolver->m_objective->m_projection.m_useStrainLimiting = true;
m_deformableBodySolver->m_objective->m_projection.m_useStrainLimiting = true;
m_deformableBodySolver->m_objective->m_preconditioner = m_deformableBodySolver->m_objective->m_massPreconditioner;
}
else
{
m_deformableBodySolver->m_useProjection = false;
m_deformableBodySolver->m_objective->m_projection.m_useStrainLimiting = false;
m_deformableBodySolver->m_objective->m_preconditioner = m_deformableBodySolver->m_objective->m_KKTPreconditioner;
}

2
src/BulletSoftBody/btPreconditioner.h
View File

@ -178,7 +178,7 @@ public:
}
}
}
#define USE_FULL_PRECONDITIONER
//#define USE_FULL_PRECONDITIONER
#ifndef USE_FULL_PRECONDITIONER
virtual void operator()(const TVStack& x, TVStack& b)
{

109
src/BulletSoftBody/btSoftBody.cpp
View File

@ -2877,93 +2877,28 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO
return (dst < 0);
}
#endif
// // regular face contact
// {
// btGjkEpaSolver2::sResults results;
// btTransform triangle_transform;
// triangle_transform.setIdentity();
// triangle_transform.setOrigin(f.m_n[0]->m_x);
// btTriangleShape triangle(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);
// btVector3 guess(0,0,0);
// if (predict)
// {
// triangle_transform.setOrigin(f.m_n[0]->m_q);
// triangle = btTriangleShape(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);
// }
// const btConvexShape* csh = static_cast<const btConvexShape*>(shp);
//// btGjkEpaSolver2::SignedDistance(&triangle, triangle_transform, csh, wtr, guess, results);
//// dst = results.distance - margin;
//// contact_point = results.witnesses[0];
// btGjkEpaSolver2::Penetration(&triangle, triangle_transform, csh, wtr, guess, results);
// if (results.status == btGjkEpaSolver2::sResults::Separated)
// return false;
// dst = results.distance - margin;
// contact_point = results.witnesses[1];
// getBarycentric(contact_point, f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary);
// nrm = results.normal;
// for (int i = 0; i < 3; ++i)
// f.m_pcontact[i] = bary[i];
// }
//
// if (!predict)
// {
// cti.m_colObj = colObjWrap->getCollisionObject();
// cti.m_normal = nrm;
// cti.m_offset = dst;
// }
//
// regular face contact
if (0)
{
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<const btConvexShape*>(shp);
btGjkEpaSolver2::SignedDistance(&triangle, triangle_transform, csh, wtr, guess, results);
dst = results.distance-csh->getMargin();
dst -= margin;
if (dst >= 0)
return false;
contact_point = results.witnesses[0];
getBarycentric(contact_point, f.m_n[0]->m_q, f.m_n[1]->m_q, f.m_n[2]->m_q, bary);
btVector3 curr = BaryEval(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary);
nrm = results.normal;
cti.m_colObj = colObjWrap->getCollisionObject();
cti.m_normal = nrm;
cti.m_offset = dst + (curr - contact_point).dot(nrm);
}
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<const btConvexShape*>(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;
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<const btConvexShape*>(shp);
btGjkEpaSolver2::SignedDistance(&triangle, triangle_transform, csh, wtr, guess, results);
dst = results.distance-2.0*csh->getMargin()-margin; // margin padding so that the distance = the actual distance between face and rigid - margin of rigid - margin of deformable
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;
}
//
void btSoftBody::updateNormals()

18
src/BulletSoftBody/btSoftBody.h
View File

@ -269,7 +269,7 @@ public:
btScalar m_im; // 1/mass
btScalar m_area; // Area
btDbvtNode* m_leaf; // Leaf data
btScalar m_penetration; // depth of penetration
int m_constrained; // depth of penetration
int m_battach : 1; // Attached
int index;
btVector3 m_splitv; // velocity associated with split impulse
@ -1313,20 +1313,22 @@ public:
I = -btMin(m_repulsionStiffness * timeStep * d, mass * (OVERLAP_REDUCTION_FACTOR * d / timeStep - vn));
if (vn < 0)
I += 0.5 * mass * vn;
btScalar face_penetration = 0, node_penetration = node->m_penetration;
int face_penetration = 0, node_penetration = node->m_constrained;
for (int i = 0; i < 3; ++i)
face_penetration = btMax(face_penetration, face->m_n[i]->m_penetration);
face_penetration |= face->m_n[i]->m_constrained;
btScalar I_tilde = .5 *I /(1.0+w.length2());
// double the impulse if node or face is constrained.
if (face_penetration > 0 || node_penetration > 0)
I_tilde *= 2.0;
if (face_penetration <= node_penetration)
{
I_tilde *= 2.0;
}
if (face_penetration <= 0)
{
for (int j = 0; j < 3; ++j)
face->m_n[j]->m_v += w[j]*n*I_tilde*node->m_im;
}
if (face_penetration >= node_penetration)
if (node_penetration <= 0)
{
node->m_v -= I_tilde*node->m_im*n;
}
@ -1344,12 +1346,12 @@ public:
// double the impulse if node or face is constrained.
// if (face_penetration > 0 || node_penetration > 0)
// I_tilde *= 2.0;
if (face_penetration <= node_penetration)
if (face_penetration <= 0)
{
for (int j = 0; j < 3; ++j)
face->m_n[j]->m_v += w[j] * vt * I_tilde * (face->m_n[j])->m_im;
}
if (face_penetration >= node_penetration)
if (node_penetration <= 0)
{
node->m_v -= I_tilde * node->m_im * vt;
}