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

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
{
// 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;
// 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)
{
// 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);