Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

combat friction drift in positionCorrect by changing velocity and change it back (effectively only changing position)

Browse Source
This commit is contained in:
Xuchen Han
2019-07-23 12:15:23 -07:00
parent a90cad2a96
commit 243b9fc8c7
15 changed files with 291 additions and 152 deletions
Download Patch File Download Diff File Expand all files Collapse all files

23
examples/DeformableContact/DeformableContact.cpp
View File

@@ -54,13 +54,6 @@ class DeformableContact : public CommonMultiBodyBase
btMultiBody* m_multiBody;
btAlignedObjectArray<btMultiBodyJointFeedback*> m_jointFeedbacks;
public:
struct TetraBunny
{
#include "../SoftDemo/bunny.inl"
};
DeformableContact(struct GUIHelperInterface* helper)
: CommonMultiBodyBase(helper)
{
@@ -147,7 +140,7 @@ void DeformableContact::initPhysics()
btTransform groundTransform;
groundTransform.setIdentity();
groundTransform.setOrigin(btVector3(0, -40, 0));
groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.0));
groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.1));
//We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here:
btScalar mass(0.);
@@ -170,16 +163,16 @@ void DeformableContact::initPhysics()
{
bool damping = true;
bool gyro = true;
int numLinks = 3;
bool damping = false;
bool gyro = false;
int numLinks = 0;
bool spherical = true; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals
bool canSleep = false;
bool selfCollide = true;
btVector3 linkHalfExtents(1, 1, 1);
btVector3 baseHalfExtents(1, 1, 1);
btMultiBody* mbC = createFeatherstoneMultiBody_testMultiDof(m_dynamicsWorld, numLinks, btVector3(0.f, 10.f,0.f), linkHalfExtents, baseHalfExtents, spherical, g_floatingBase);
btMultiBody* mbC = createFeatherstoneMultiBody_testMultiDof(m_dynamicsWorld, numLinks, btVector3(0.f, 2.f,0.f), linkHalfExtents, baseHalfExtents, spherical, g_floatingBase);
mbC->setCanSleep(canSleep);
mbC->setHasSelfCollision(selfCollide);
@@ -216,12 +209,13 @@ void DeformableContact::initPhysics()
// create a patch of cloth
{
const btScalar s = 6;
const btScalar s = 4;
btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, 0, -s),
btVector3(+s, 0, -s),
btVector3(-s, 0, +s),
btVector3(+s, 0, +s),
20,20,
// 20,20,
3,3,
1 + 2 + 4 + 8, true);
psb->getCollisionShape()->setMargin(0.25);
@@ -232,7 +226,6 @@ void DeformableContact::initPhysics()
psb->m_cfg.kCHR = 1; // collision hardness with rigid body
psb->m_cfg.kDF = 0;
getDeformableDynamicsWorld()->addSoftBody(psb);
}
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);

21
examples/DeformableDemo/DeformableDemo.cpp
View File

@@ -105,15 +105,15 @@ public:
// }
btTransform startTransform;
startTransform.setIdentity();
startTransform.setOrigin(btVector3(1, 2, 1));
startTransform.setOrigin(btVector3(1, 1.5, 1));
createRigidBody(mass, startTransform, shape[0]);
startTransform.setOrigin(btVector3(1, 2, -1));
startTransform.setOrigin(btVector3(1, 1.5, -1));
createRigidBody(mass, startTransform, shape[0]);
startTransform.setOrigin(btVector3(-1, 2, 1));
startTransform.setOrigin(btVector3(-1, 1.5, 1));
createRigidBody(mass, startTransform, shape[0]);
startTransform.setOrigin(btVector3(-1, 2, -1));
startTransform.setOrigin(btVector3(-1, 1.5, -1));
createRigidBody(mass, startTransform, shape[0]);
startTransform.setOrigin(btVector3(0, 4, 0));
startTransform.setOrigin(btVector3(0, 3.5, 0));
createRigidBody(mass, startTransform, shape[0]);
}
@@ -182,7 +182,7 @@ void DeformableDemo::initPhysics()
btTransform groundTransform;
groundTransform.setIdentity();
groundTransform.setOrigin(btVector3(0, -30, 0));
groundTransform.setOrigin(btVector3(0, -32, 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.);
@@ -206,7 +206,7 @@ void DeformableDemo::initPhysics()
// create a piece of cloth
{
bool onGround = true;
bool onGround = false;
const btScalar s = 4;
btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, 0, -s),
btVector3(+s, 0, -s),
@@ -222,13 +222,14 @@ void DeformableDemo::initPhysics()
btVector3(+s, 0, -s),
btVector3(-s, 0, +s),
btVector3(+s, 0, +s),
20,20,
// 20,20,
2,2,
0, true);
psb->getCollisionShape()->setMargin(0.1);
psb->generateBendingConstraints(2);
psb->setTotalMass(10);
psb->setSpringStiffness(10);
psb->setTotalMass(1);
psb->setSpringStiffness(1);
psb->setDampingCoefficient(0.01);
psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
psb->m_cfg.kCHR = 1; // collision hardness with rigid body

136
examples/Pinch/Pinch.cpp
View File

@@ -73,9 +73,9 @@ public:
void resetCamera()
{
float dist = 25;
float pitch = -45;
float pitch = -30;
float yaw = 100;
float targetPos[3] = {0, -3, 0};
float targetPos[3] = {0, -0, 0};
m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
}
@@ -137,17 +137,56 @@ void dynamics(btScalar time, btDeformableRigidDynamicsWorld* world)
return;
btRigidBody* rb0 = rbs[0];
btScalar pressTime = 0.9;
btScalar liftTime = 2.5;
btScalar shiftTime = 3.5;
btScalar holdTime = 4.5*1000;
btScalar dropTime = 5.3*1000;
btTransform rbTransform;
rbTransform.setIdentity();
btVector3 translation = btVector3(0.5,3,4);
btVector3 velocity = btVector3(0,5,0);
btVector3 translation;
btVector3 velocity;
btVector3 initialTranslationLeft = btVector3(0.5,3,4);
btVector3 initialTranslationRight = btVector3(0.5,3,-4);
btVector3 pinchVelocityLeft = btVector3(0,0,-2);
btVector3 pinchVelocityRight = btVector3(0,0,2);
btVector3 liftVelocity = btVector3(0,5,0);
btVector3 shiftVelocity = btVector3(0,0,5);
btVector3 holdVelocity = btVector3(0,0,0);
btVector3 openVelocityLeft = btVector3(0,0,4);
btVector3 openVelocityRight = btVector3(0,0,-4);
if (time < pressTime)
{
velocity = btVector3(0,0,-2);
translation += velocity * time;
velocity = pinchVelocityLeft;
translation = initialTranslationLeft + pinchVelocityLeft * time;
}
else if (time < liftTime)
{
velocity = liftVelocity;
translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (time - pressTime);
}
else if (time < shiftTime)
{
velocity = shiftVelocity;
translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (time - liftTime);
}
else if (time < holdTime)
{
velocity = btVector3(0,0,0);
translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (time - shiftTime);
}
else if (time < dropTime)
{
velocity = openVelocityLeft;
translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (holdTime - shiftTime)+ openVelocityLeft * (time - holdTime);
}
else
translation += btVector3(0,0,-2) * pressTime + (time-pressTime)*velocity;
{
velocity = holdVelocity;
translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (holdTime - shiftTime)+ openVelocityLeft * (dropTime - holdTime);
}
rbTransform.setOrigin(translation);
rbTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0));
rb0->setCenterOfMassTransform(rbTransform);
@@ -155,23 +194,45 @@ void dynamics(btScalar time, btDeformableRigidDynamicsWorld* world)
rb0->setLinearVelocity(velocity);
btRigidBody* rb1 = rbs[1];
translation = btVector3(0.5,3,-4);
velocity = btVector3(0,5,0);
if (time < pressTime)
{
velocity = btVector3(0,0,2);
translation += velocity * time;
velocity = pinchVelocityRight;
translation = initialTranslationRight + pinchVelocityRight * time;
}
else if (time < liftTime)
{
velocity = liftVelocity;
translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (time - pressTime);
}
else if (time < shiftTime)
{
velocity = shiftVelocity;
translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (time - liftTime);
}
else if (time < holdTime)
{
velocity = btVector3(0,0,0);
translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (time - shiftTime);
}
else if (time < dropTime)
{
velocity = openVelocityRight;
translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (holdTime - shiftTime)+ openVelocityRight * (time - holdTime);
}
else
translation += btVector3(0,0,2) * pressTime + (time-pressTime)*velocity;
{
velocity = holdVelocity;
translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (holdTime - shiftTime)+ openVelocityRight * (dropTime - holdTime);
}
rbTransform.setOrigin(translation);
rbTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0));
rb1->setCenterOfMassTransform(rbTransform);
rb1->setAngularVelocity(btVector3(0,0,0));
rb1->setLinearVelocity(velocity);
rb0->setFriction(2);
rb1->setFriction(2);
rb0->setFriction(20);
rb1->setFriction(20);
}
void Pinch::initPhysics()
@@ -194,7 +255,7 @@ void Pinch::initPhysics()
m_dynamicsWorld->setGravity(btVector3(0, -10, 0));
getDeformableDynamicsWorld()->getWorldInfo().m_gravity.setValue(0, -10, 0);
getDeformableDynamicsWorld()->before_solver_callbacks.push_back(dynamics);
getDeformableDynamicsWorld()->m_beforeSolverCallbacks.push_back(dynamics);
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
//create a ground
@@ -229,21 +290,58 @@ void Pinch::initPhysics()
// create a soft block
{
btScalar verts[24] = {0.f, 0.f, 0.f,
1.f, 0.f, 0.f,
0.f, 1.f, 0.f,
0.f, 0.f, 1.f,
1.f, 1.f, 0.f,
0.f, 1.f, 1.f,
1.f, 0.f, 1.f,
1.f, 1.f, 1.f
};
int triangles[60] = {0, 6, 3,
0,1,6,
7,5,3,
7,3,6,
4,7,6,
4,6,1,
7,2,5,
7,4,2,
0,3,2,
2,3,5,
0,2,4,
0,4,1,
0,6,5,
0,6,4,
3,4,2,
3,4,7,
2,7,3,
2,7,1,
4,5,0,
4,5,6,
};
// btSoftBody* psb = btSoftBodyHelpers::CreateFromTriMesh(getDeformableDynamicsWorld()->getWorldInfo(), &verts[0], &triangles[0], 20);
////
btSoftBody* psb = btSoftBodyHelpers::CreateFromTetGenData(getDeformableDynamicsWorld()->getWorldInfo(),
TetraCube::getElements(),
0,
TetraCube::getNodes(),
false, true, true);
getDeformableDynamicsWorld()->addSoftBody(psb);
psb->scale(btVector3(2, 2, 2));
psb->translate(btVector3(0, 4, 0));
psb->getCollisionShape()->setMargin(0.1);
psb->setTotalMass(1);
psb->setSpringStiffness(10);
psb->setDampingCoefficient(0.01);
// psb->scale(btVector3(5, 5, 5));
// psb->translate(btVector3(-2.5, 4, -2.5));
// psb->getCollisionShape()->setMargin(0.1);
// psb->setTotalMass(1);
psb->setSpringStiffness(4);
psb->setDampingCoefficient(0.02);
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.5;
psb->m_cfg.kDF = 2;
getDeformableDynamicsWorld()->addSoftBody(psb);
// add a grippers
createGrip();
}

4
examples/VolumetricDeformable/VolumetricDeformable.cpp
View File

@@ -77,8 +77,8 @@ public:
void stepSimulation(float deltaTime)
{
//use a smaller internal timestep, there are stability issues
float internalTimeStep = 1. / 240.f;
m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep);
float internalTimeStep = 1. / 480.f;
m_dynamicsWorld->stepSimulation(deltaTime, 8, internalTimeStep);
}
void createStaticBox(const btVector3& halfEdge, const btVector3& translation)

7
src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp
View File

@@ -233,7 +233,7 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher, btCollisi
// printf("error in island management\n");
}
btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
// btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
if (colObj0->getIslandTag() == islandId)
{
if (colObj0->getActivationState() == ACTIVE_TAG ||
@@ -257,7 +257,7 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher, btCollisi
// printf("error in island management\n");
}
btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
// btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
if (colObj0->getIslandTag() == islandId)
{
@@ -278,7 +278,8 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher, btCollisi
// printf("error in island management\n");
}
btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
// btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
if (colObj0->getIslandTag() == islandId)
{

54
src/BulletSoftBody/btContactProjection.cpp
View File

@@ -50,12 +50,12 @@ void btDeformableContactProjection::update()
// loop through constraints to set constrained values
for (auto& it : m_constraints)
{
btAlignedObjectArray<Friction>& frictions = m_frictions[it.first];
btAlignedObjectArray<Constraint>& constraints = it.second;
btAlignedObjectArray<DeformableFrictionConstraint>& frictions = m_frictions[it.first];
btAlignedObjectArray<DeformableContactConstraint>& constraints = it.second;
for (int i = 0; i < constraints.size(); ++i)
{
Constraint& constraint = constraints[i];
Friction& friction = frictions[i];
DeformableContactConstraint& constraint = constraints[i];
DeformableFrictionConstraint& friction = frictions[i];
for (int j = 0; j < constraint.m_contact.size(); ++j)
{
if (constraint.m_contact[j] == nullptr)
@@ -229,16 +229,16 @@ void btDeformableContactProjection::setConstraints()
{
if (psb->m_nodes[j].m_im == 0)
{
btAlignedObjectArray<Constraint> c;
c.push_back(Constraint(btVector3(1,0,0)));
c.push_back(Constraint(btVector3(0,1,0)));
c.push_back(Constraint(btVector3(0,0,1)));
btAlignedObjectArray<DeformableContactConstraint> c;
c.push_back(DeformableContactConstraint(btVector3(1,0,0)));
c.push_back(DeformableContactConstraint(btVector3(0,1,0)));
c.push_back(DeformableContactConstraint(btVector3(0,0,1)));
m_constraints[&(psb->m_nodes[j])] = c;
btAlignedObjectArray<Friction> f;
f.push_back(Friction());
f.push_back(Friction());
f.push_back(Friction());
btAlignedObjectArray<DeformableFrictionConstraint> f;
f.push_back(DeformableFrictionConstraint());
f.push_back(DeformableFrictionConstraint());
f.push_back(DeformableFrictionConstraint());
m_frictions[&(psb->m_nodes[j])] = f;
}
}
@@ -310,11 +310,11 @@ void btDeformableContactProjection::setConstraints()
if (m_constraints.find(c.m_node) == m_constraints.end())
{
btAlignedObjectArray<Constraint> constraints;
constraints.push_back(Constraint(c, jacobianData_normal));
btAlignedObjectArray<DeformableContactConstraint> constraints;
constraints.push_back(DeformableContactConstraint(c, jacobianData_normal));
m_constraints[c.m_node] = constraints;
btAlignedObjectArray<Friction> frictions;
frictions.push_back(Friction(complementaryDirection, jacobianData_complementary));
btAlignedObjectArray<DeformableFrictionConstraint> frictions;
frictions.push_back(DeformableFrictionConstraint(complementaryDirection, jacobianData_complementary));
m_frictions[c.m_node] = frictions;
}
else
@@ -322,8 +322,8 @@ void btDeformableContactProjection::setConstraints()
// group colinear constraints into one
const btScalar angle_epsilon = 0.015192247; // less than 10 degree
bool merged = false;
btAlignedObjectArray<Constraint>& constraints = m_constraints[c.m_node];
btAlignedObjectArray<Friction>& frictions = m_frictions[c.m_node];
btAlignedObjectArray<DeformableContactConstraint>& constraints = m_constraints[c.m_node];
btAlignedObjectArray<DeformableFrictionConstraint>& frictions = m_frictions[c.m_node];
for (int j = 0; j < constraints.size(); ++j)
{
const btAlignedObjectArray<btVector3>& dirs = constraints[j].m_direction;
@@ -343,8 +343,8 @@ void btDeformableContactProjection::setConstraints()
// hard coded no more than 3 constraint directions
if (!merged && constraints.size() < dim)
{
constraints.push_back(Constraint(c, jacobianData_normal));
frictions.push_back(Friction(complementaryDirection, jacobianData_complementary));
constraints.push_back(DeformableContactConstraint(c, jacobianData_normal));
frictions.push_back(DeformableFrictionConstraint(complementaryDirection, jacobianData_complementary));
}
}
}
@@ -358,9 +358,9 @@ void btDeformableContactProjection::enforceConstraint(TVStack& x)
const int dim = 3;
for (auto& it : m_constraints)
{
const btAlignedObjectArray<Constraint>& constraints = it.second;
const btAlignedObjectArray<DeformableContactConstraint>& constraints = it.second;
size_t i = m_indices[it.first];
const btAlignedObjectArray<Friction>& frictions = m_frictions[it.first];
const btAlignedObjectArray<DeformableFrictionConstraint>& frictions = m_frictions[it.first];
btAssert(constraints.size() <= dim);
btAssert(constraints.size() > 0);
if (constraints.size() == 1)
@@ -401,7 +401,7 @@ void btDeformableContactProjection::enforceConstraint(TVStack& x)
{
for (int f = 0; f < frictions.size(); ++f)
{
const Friction& friction= frictions[f];
const DeformableFrictionConstraint& friction= frictions[f];
for (int j = 0; j < friction.m_direction.size(); ++j)
{
// clear the old constraint
@@ -425,9 +425,9 @@ void btDeformableContactProjection::project(TVStack& x)
const int dim = 3;
for (auto& it : m_constraints)
{
const btAlignedObjectArray<Constraint>& constraints = it.second;
const btAlignedObjectArray<DeformableContactConstraint>& constraints = it.second;
size_t i = m_indices[it.first];
btAlignedObjectArray<Friction>& frictions = m_frictions[it.first];
btAlignedObjectArray<DeformableFrictionConstraint>& frictions = m_frictions[it.first];
btAssert(constraints.size() <= dim);
btAssert(constraints.size() > 0);
if (constraints.size() == 1)
@@ -450,14 +450,14 @@ void btDeformableContactProjection::project(TVStack& x)
bool has_static_constraint = false;
for (int f = 0; f < frictions.size(); ++f)
{
Friction& friction= frictions[f];
DeformableFrictionConstraint& friction= frictions[f];
for (int j = 0; j < friction.m_static.size(); ++j)
has_static_constraint = has_static_constraint || friction.m_static[j];
}
for (int f = 0; f < frictions.size(); ++f)
{
Friction& friction= frictions[f];
DeformableFrictionConstraint& friction= frictions[f];
for (int j = 0; j < friction.m_direction.size(); ++j)
{
// clear the old friction force

15
src/BulletSoftBody/btDeformableBodySolver.cpp
View File

@@ -29,6 +29,7 @@ void btDeformableBodySolver::solveConstraints(float solverdt)
// save v_{n+1}^* velocity after explicit forces
backupVelocity();
m_objective->computeResidual(solverdt, m_residual);
// m_objective->initialGuess(m_dv, m_residual);
computeStep(m_dv, m_residual);
@@ -98,6 +99,20 @@ void btDeformableBodySolver::backupVelocity()
}
}
void btDeformableBodySolver::revertVelocity()
{
// serial implementation
int counter = 0;
for (int i = 0; i < m_softBodySet.size(); ++i)
{
btSoftBody* psb = m_softBodySet[i];
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
psb->m_nodes[j].m_v = m_backupVelocity[counter++];
}
}
}
bool btDeformableBodySolver::updateNodes()
{
int numNodes = 0;

6
src/BulletSoftBody/btDeformableBodySolver.h
View File

@@ -27,12 +27,14 @@ protected:
TVStack m_dv;
TVStack m_residual;
btAlignedObjectArray<btSoftBody *> m_softBodySet;
btDeformableBackwardEulerObjective* m_objective;
btAlignedObjectArray<btVector3> m_backupVelocity;
btScalar m_dt;
btConjugateGradient<btDeformableBackwardEulerObjective> cg;
public:
btDeformableBackwardEulerObjective* m_objective;
btDeformableBodySolver();
virtual ~btDeformableBodySolver();
@@ -57,7 +59,7 @@ public:
void predictDeformableMotion(btSoftBody* psb, btScalar dt);
void backupVelocity();
void revertVelocity();
void updateVelocity();
bool updateNodes();

91
src/BulletSoftBody/btDeformableContactProjection.cpp
View File

@@ -143,6 +143,8 @@ void btDeformableContactProjection::update()
friction.m_direction[j] = -local_tangent_dir;
// do not allow switching from static friction to dynamic friction
// it causes cg to explode
btScalar comp1 = -accumulated_normal*c->m_c3;
btScalar comp2 = tangent_norm;
if (-accumulated_normal*c->m_c3 < tangent_norm && friction.m_static_prev[j] == false && friction.m_released[j] == false)
{
friction.m_static[j] = false;
@@ -167,49 +169,44 @@ void btDeformableContactProjection::update()
// the incremental impulse applied to rb in the tangential direction
btVector3 incremental_tangent = (friction.m_impulse_prev[j] * friction.m_direction_prev[j])-(friction.m_impulse[j] * friction.m_direction[j]);
// TODO cleanup
if (1) // in the same CG solve, the set of constraits doesn't change
// dv = new_impulse + accumulated velocity change in previous CG iterations
// so we have the invariant node->m_v = backupVelocity + dv;
btScalar dvn = -accumulated_normal * c->m_c2/m_dt;
// the following is equivalent
/*
btVector3 dv = -impulse_normal * c->m_c2/m_dt + c->m_node->m_v - backupVelocity[m_indices[c->m_node]];
btScalar dvn = dv.dot(cti.m_normal);
*/
constraint.m_value[j] = dvn;
// the incremental impulse:
// in the normal direction it's the normal component of "impulse"
// in the tangent direction it's the difference between the frictional impulse in the iteration and the previous iteration
impulse = impulse_normal + incremental_tangent;
if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
{
if (rigidCol)
rigidCol->applyImpulse(impulse, c->m_c1);
}
else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
{
// c0 is the impulse matrix, c3 is 1 - the friction coefficient or 0, c4 is the contact hardness coefficient
// dv = new_impulse + accumulated velocity change in previous CG iterations
// so we have the invariant node->m_v = backupVelocity + dv;
btScalar dvn = -accumulated_normal * c->m_c2/m_dt;
// the following is equivalent
/*
btVector3 dv = -impulse_normal * c->m_c2/m_dt + c->m_node->m_v - backupVelocity[m_indices[c->m_node]];
btScalar dvn = dv.dot(cti.m_normal);
*/
constraint.m_value[j] = dvn;
// the incremental impulse:
// in the normal direction it's the normal component of "impulse"
// in the tangent direction it's the difference between the frictional impulse in the iteration and the previous iteration
impulse = impulse_normal + incremental_tangent;
if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
{
if (rigidCol)
rigidCol->applyImpulse(impulse, c->m_c1);
}
else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
if (multibodyLinkCol)
{
double multiplier = 1;
multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_normal, -impulse_normal.length() * multiplier);
if (multibodyLinkCol)
if (incremental_tangent.norm() > SIMD_EPSILON)
{
double multiplier = 1;
multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_normal, -impulse_normal.length() * multiplier);
if (incremental_tangent.norm() > SIMD_EPSILON)
{
btMultiBodyJacobianData jacobian_tangent;
btVector3 tangent = incremental_tangent.normalized();
findJacobian(multibodyLinkCol, jacobian_tangent, c->m_node->m_x, tangent);
const btScalar* deltaV_tangent = &jacobian_tangent.m_deltaVelocitiesUnitImpulse[0];
multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_tangent, incremental_tangent.length() * multiplier);
}
btMultiBodyJacobianData jacobian_tangent;
btVector3 tangent = incremental_tangent.normalized();
findJacobian(multibodyLinkCol, jacobian_tangent, c->m_node->m_x, tangent);
const btScalar* deltaV_tangent = &jacobian_tangent.m_deltaVelocitiesUnitImpulse[0];
multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_tangent, incremental_tangent.length() * multiplier);
}
}
}
@@ -404,14 +401,10 @@ void btDeformableContactProjection::enforceConstraint(TVStack& x)
const DeformableFrictionConstraint& friction= frictions[f];
for (int j = 0; j < friction.m_direction.size(); ++j)
{
// clear the old constraint
if (friction.m_static_prev[j] == true)
{
x[i] -= friction.m_direction_prev[j] * friction.m_dv_prev[j];
}
// add the new constraint
// add the friction constraint
if (friction.m_static[j] == true)
{
x[i] -= x[i].dot(friction.m_direction[j]) * friction.m_direction[j];
x[i] += friction.m_direction[j] * friction.m_dv[j];
}
}
@@ -467,9 +460,15 @@ void btDeformableContactProjection::project(TVStack& x)
}
// only add to the rhs if there is no static friction constraint on the node
if (friction.m_static[j] == false && !has_static_constraint)
if (friction.m_static[j] == false)
{
x[i] += friction.m_direction[j] * friction.m_impulse[j];
if (!has_static_constraint)
x[i] += friction.m_direction[j] * friction.m_impulse[j];
}
else
{
// otherwise clear the constraint in the friction direction
x[i] -= x[i].dot(friction.m_direction[j]) * friction.m_direction[j];
}
}
}

1
src/BulletSoftBody/btDeformableGravityForce.h
View File

@@ -23,6 +23,7 @@ public:
virtual void addScaledImplicitForce(btScalar scale, TVStack& force)
{
// addScaledGravityForce(scale, force);
}
virtual void addScaledExplicitForce(btScalar scale, TVStack& force)

3
src/BulletSoftBody/btDeformableMassSpringForce.h
View File

@@ -22,6 +22,7 @@ public:
virtual void addScaledImplicitForce(btScalar scale, TVStack& force)
{
addScaledDampingForce(scale, force);
// addScaledElasticForce(scale, force);
}
virtual void addScaledExplicitForce(btScalar scale, TVStack& force)
@@ -102,6 +103,8 @@ public:
}
}
}
};
#endif /* btMassSpring_h */

67
src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp
View File

@@ -13,7 +13,7 @@
void btDeformableRigidDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
{
reinitialize(timeStep);
// beforeSolverCallbacks(timeStep);
// add gravity to velocity of rigid and multi bodys
applyRigidBodyGravity(timeStep);
@@ -30,7 +30,7 @@ void btDeformableRigidDynamicsWorld::internalSingleStepSimulation(btScalar timeS
///solve deformable bodies constraints
solveDeformableBodiesConstraints(timeStep);
positionCorrection();
afterSolverCallbacks(timeStep);
integrateTransforms(timeStep);
@@ -42,36 +42,57 @@ void btDeformableRigidDynamicsWorld::internalSingleStepSimulation(btScalar timeS
// ///////////////////////////////
}
void btDeformableRigidDynamicsWorld::positionCorrection()
void btDeformableRigidDynamicsWorld::positionCorrection(btScalar dt)
{
// perform position correction for all geometric collisions
for (int i = 0; i < m_softBodies.size(); ++i)
// perform position correction for all constraints
for (auto& it : m_deformableBodySolver->m_objective->projection.m_constraints)
{
btSoftBody* psb = m_softBodies[i];
const btScalar mrg = psb->getCollisionShape()->getMargin();
for (int j = 0; j < psb->m_rcontacts.size(); ++j)
btAlignedObjectArray<DeformableFrictionConstraint>& frictions = m_deformableBodySolver->m_objective->projection.m_frictions[it.first];
btAlignedObjectArray<DeformableContactConstraint>& constraints = it.second;
for (int i = 0; i < constraints.size(); ++i)
{
const btSoftBody::RContact& c = psb->m_rcontacts[j];
// skip anchor points
if (c.m_node->m_im == 0)
continue;
const btSoftBody::sCti& cti = c.m_cti;
if (cti.m_colObj->hasContactResponse())
DeformableContactConstraint& constraint = constraints[i];
DeformableFrictionConstraint& friction = frictions[i];
for (int j = 0; j < constraint.m_contact.size(); ++j)
{
btScalar dp = btMin((btDot(c.m_node->m_x, cti.m_normal) + cti.m_offset), mrg);
if (dp < 0)
const btSoftBody::RContact* c = constraint.m_contact[j];
// skip anchor points
if (c == nullptr || c->m_node->m_im == 0)
continue;
const btSoftBody::sCti& cti = c->m_cti;
btRigidBody* rigidCol = 0;
btVector3 va(0, 0, 0);
// grab the velocity of the rigid body
if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
{
// m_c4 is the collision hardness
c.m_node->m_q -= dp * cti.m_normal * c.m_c4;
rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
va = rigidCol ? (rigidCol->getVelocityInLocalPoint(c->m_c1)): btVector3(0, 0, 0);
}
if (cti.m_colObj->hasContactResponse())
{
btScalar dp = cti.m_offset;
rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
if (friction.m_static[j] == true)
{
c->m_node->m_v = va;
}
if (dp < 0)
{
c->m_node->m_v -= dp * cti.m_normal / dt;
}
}
}
}
}
}
void btDeformableRigidDynamicsWorld::integrateTransforms(btScalar dt)
{
m_deformableBodySolver->backupVelocity();
positionCorrection(dt);
btMultiBodyDynamicsWorld::integrateTransforms(dt);
for (int i = 0; i < m_softBodies.size(); ++i)
{
@@ -82,6 +103,7 @@ void btDeformableRigidDynamicsWorld::integrateTransforms(btScalar dt)
node.m_x = node.m_q + dt * node.m_v;
}
}
m_deformableBodySolver->revertVelocity();
}
void btDeformableRigidDynamicsWorld::solveDeformableBodiesConstraints(btScalar timeStep)
@@ -146,7 +168,12 @@ void btDeformableRigidDynamicsWorld::beforeSolverCallbacks(btScalar timeStep)
{
(*m_internalTickCallback)(this, timeStep);
}
for (int i = 0; i < m_beforeSolverCallbacks.size(); ++i)
m_beforeSolverCallbacks[i](m_internalTime, this);
}
void btDeformableRigidDynamicsWorld::afterSolverCallbacks(btScalar timeStep)
{
for (int i = 0; i < m_beforeSolverCallbacks.size(); ++i)
m_beforeSolverCallbacks[i](m_internalTime, this);
}

4
src/BulletSoftBody/btDeformableRigidDynamicsWorld.h
View File

@@ -47,7 +47,7 @@ protected:
virtual void integrateTransforms(btScalar timeStep);
void positionCorrection();
void positionCorrection(btScalar dt);
void solveDeformableBodiesConstraints(btScalar timeStep);
@@ -124,6 +124,8 @@ public:
void beforeSolverCallbacks(btScalar timeStep);
void afterSolverCallbacks(btScalar timeStep);
int getDrawFlags() const { return (m_drawFlags); }
void setDrawFlags(int f) { m_drawFlags = f; }
};

3
src/BulletSoftBody/btSoftBody.cpp
View File

@@ -2286,7 +2286,8 @@ bool btSoftBody::checkContact(const btCollisionObjectWrapper* colObjWrap,
{
cti.m_colObj = colObjWrap->getCollisionObject();
cti.m_normal = wtr.getBasis() * nrm;
cti.m_offset = -btDot(cti.m_normal, x - cti.m_normal * dst);
// cti.m_offset = -btDot(cti.m_normal, x - cti.m_normal * dst);
cti.m_offset = dst;
return (true);
}
return (false);

8
src/BulletSoftBody/btSoftBodyInternals.h
View File

@@ -878,15 +878,11 @@ struct btSoftColliders
const btScalar ms = ima + imb;
if (ms > 0)
{
psb->checkContact(m_colObj1Wrap, n.m_q, m, c.m_cti);
const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform();
static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0);
const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic;
const btVector3 ra = n.m_x - wtr.getOrigin();
const btVector3 va = m_rigidBody ? m_rigidBody->getVelocityInLocalPoint(ra) * psb->m_sst.sdt : btVector3(0, 0, 0);
const btVector3 vb = n.m_x - n.m_q;
const btVector3 vr = vb - va;
const btScalar dn = btDot(vr, c.m_cti.m_normal);
const btVector3 fv = vr - c.m_cti.m_normal * dn;
const btVector3 ra = n.m_q - wtr.getOrigin();
const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction();
c.m_node = &n;
c.m_c0 = ImpulseMatrix(psb->m_sst.sdt, ima, imb, iwi, ra);