AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-14 22:00:05 +00:00
Code Issues Releases Wiki Activity

switched to deformable rigid contact from Jacobi to Gauss Seidel

Browse Source
This commit is contained in:
Xuchen Han 2019-09-11 17:04:06 -07:00
parent f99cf56149
commit 109d9353af
9 changed files with 312 additions and 55 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/BulletSoftBody/btDeformableBackwardEulerObjective.h
View File

@ -71,7 +71,7 @@ public:
void setDt(btScalar dt);
// enforce constraints in CG solve
// enforce constraints in contact solve
void enforceConstraint(TVStack& x)
{
BT_PROFILE("enforceConstraint");

26
src/BulletSoftBody/btDeformableBodySolver.cpp
View File

@ -24,7 +24,7 @@ btDeformableBodySolver::btDeformableBodySolver()
, m_cg(20)
, m_maxNewtonIterations(5)
, m_newtonTolerance(1e-4)
, m_lineSearch(false)
, m_lineSearch(true)
{
m_objective = new btDeformableBackwardEulerObjective(m_softBodySet, m_backupVelocity);
}
@ -69,12 +69,14 @@ void btDeformableBodySolver::solveDeformableConstraints(btScalar solverdt)
{
break;
}
// todo xuchenhan@: this really only needs to be calculated once
m_objective->applyDynamicFriction(m_residual);
if (m_lineSearch)
{
btScalar inner_product = computeDescentStep(m_ddv,m_residual);
btScalar alpha = 0.01, beta = 0.5; // Boyd & Vandenberghe suggested alpha between 0.01 and 0.3, beta between 0.1 to 0.8
btScalar scale = 2;
// todo xuchenhan@: add damping energy to f0 and f1
btScalar f0 = m_objective->totalEnergy()+kineticEnergy(), f1, f2;
backupDv();
do {
@ -239,14 +241,18 @@ btScalar btDeformableBodySolver::solveContactConstraints()
m_dv[i].setZero();
}
btScalar maxSquaredResidual = m_objective->projection.update();
m_objective->enforceConstraint(m_dv);
// std::cout << "=================" << std::endl;
// for (int i = 0; i < m_dv.size(); ++i)
// {
// std::cout << m_dv[i].getX() << " " << m_dv[i].getY() << " " << m_dv[i].getZ() << std::endl;
// }
m_objective->updateVelocity(m_dv);
// m_objective->enforceConstraint(m_dv);
// m_objective->updateVelocity(m_dv);
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)
{
m_dv[counter] = psb->m_nodes[j].m_v - m_backupVelocity[counter];
++counter;
}
}
return maxSquaredResidual;
}
@ -374,7 +380,7 @@ void btDeformableBodySolver::predictDeformableMotion(btSoftBody* psb, btScalar d
psb->m_bUpdateRtCst = false;
psb->updateConstants();
psb->m_fdbvt.clear();
if (psb->m_cfg.collisions & btSoftBody::fCollision::SDF_RDF)
if (psb->m_cfg.collisions & btSoftBody::fCollision::SDF_RD)
{
psb->initializeFaceTree();
}

47
src/BulletSoftBody/btDeformableContactConstraint.cpp
View File

@ -15,6 +15,7 @@
#include "btDeformableContactConstraint.h"
/* ================ Deformable vs. Rigid =================== */
btDeformableRigidContactConstraint::btDeformableRigidContactConstraint(const btSoftBody::DeformableRigidContact& c)
: m_contact(&c)
, btDeformableContactConstraint(c.m_cti.m_normal)
@ -91,6 +92,8 @@ btScalar btDeformableRigidContactConstraint::solveConstraint()
btVector3 vb = getVb();
btVector3 vr = vb - va;
const btScalar dn = btDot(vr, cti.m_normal);
// if (dn > 0)
// return 0;
// 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;
@ -133,7 +136,7 @@ btScalar btDeformableRigidContactConstraint::solveConstraint()
}
}
impulse = impulse_normal + impulse_tangent;
applyImpulse(impulse);
if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
{
@ -166,6 +169,7 @@ btScalar btDeformableRigidContactConstraint::solveConstraint()
return residualSquare;
}
/* ================ Node vs. Rigid =================== */
btDeformableNodeRigidContactConstraint::btDeformableNodeRigidContactConstraint(const btSoftBody::DeformableNodeRigidContact& contact)
: m_node(contact.m_node)
, btDeformableRigidContactConstraint(contact)
@ -184,7 +188,46 @@ btVector3 btDeformableNodeRigidContactConstraint::getVb() const
}
btVector3 btDeformableNodeRigidContactConstraint::getDv(btSoftBody::Node* node) const
btVector3 btDeformableNodeRigidContactConstraint::getDv(const btSoftBody::Node* node) const
{
return m_total_normal_dv + m_total_tangent_dv;
}
/* ================ Face vs. Rigid =================== */
btDeformableFaceRigidContactConstraint::btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact)
: m_face(contact.m_face)
, m_solved(false)
, btDeformableRigidContactConstraint(contact)
{
}
btDeformableFaceRigidContactConstraint::btDeformableFaceRigidContactConstraint(const btDeformableFaceRigidContactConstraint& other)
: m_face(other.m_face)
, m_solved(false)
, btDeformableRigidContactConstraint(other)
{
}
btVector3 btDeformableFaceRigidContactConstraint::getVb() const
{
const btSoftBody::DeformableFaceRigidContact* contact = getContact();
btVector3 vb = m_face->m_n[0]->m_v * contact->m_bary[0] + m_face->m_n[1]->m_v * contact->m_bary[1] + m_face->m_n[2]->m_v * contact->m_bary[2];
return vb;
}
btVector3 btDeformableFaceRigidContactConstraint::getDv(const btSoftBody::Node* node) const
{
btVector3 face_dv = m_total_normal_dv + m_total_tangent_dv;
const btSoftBody::DeformableFaceRigidContact* contact = getContact();
if (m_face->m_n[0] == node)
{
return face_dv * contact->m_weights[0];
}
if (m_face->m_n[1] == node)
{
return face_dv * contact->m_weights[1];
}
btAssert(node == m_face->m_n[2]);
return face_dv * contact->m_weights[2];
}

53
src/BulletSoftBody/btDeformableContactConstraint.h
View File

@ -58,7 +58,7 @@ public:
virtual btVector3 getVb() const = 0;
// get the velocity change of the soft body node in the constraint
virtual btVector3 getDv(btSoftBody::Node*) const = 0;
virtual btVector3 getDv(const btSoftBody::Node*) const = 0;
};
class btDeformableStaticConstraint : public btDeformableContactConstraint
@ -96,7 +96,7 @@ public:
return btVector3(0,0,0);
}
virtual btVector3 getDv(btSoftBody::Node* n) const
virtual btVector3 getDv(const btSoftBody::Node* n) const
{
return btVector3(0,0,0);
}
@ -120,6 +120,8 @@ public:
virtual btVector3 getVa() const;
virtual btScalar solveConstraint();
virtual void applyImpulse(const btVector3& impulse) = 0;
};
@ -135,16 +137,59 @@ public:
virtual ~btDeformableNodeRigidContactConstraint()
{
}
virtual btVector3 getVb() const;
virtual btVector3 getDv(btSoftBody::Node*) const;
virtual btVector3 getDv(const btSoftBody::Node*) const;
const btSoftBody::DeformableNodeRigidContact* getContact() const
{
return static_cast<const btSoftBody::DeformableNodeRigidContact*>(m_contact);
}
virtual void applyImpulse(const btVector3& impulse)
{
const btSoftBody::DeformableNodeRigidContact* contact = getContact();
btVector3 dv = impulse * contact->m_c2;
contact->m_node->m_v -= dv;
}
};
class btDeformableFaceRigidContactConstraint : public btDeformableRigidContactConstraint
{
public:
const btSoftBody::Face* m_face;
bool m_solved;
btDeformableFaceRigidContactConstraint(){}
btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact);
btDeformableFaceRigidContactConstraint(const btDeformableFaceRigidContactConstraint& other);
virtual ~btDeformableFaceRigidContactConstraint()
{
}
virtual btVector3 getVb() const;
virtual btVector3 getDv(const btSoftBody::Node*) const;
const btSoftBody::DeformableFaceRigidContact* getContact() const
{
return static_cast<const btSoftBody::DeformableFaceRigidContact*>(m_contact);
}
virtual void applyImpulse(const btVector3& impulse)
{
const btSoftBody::DeformableFaceRigidContact* contact = getContact();
btVector3 dv = impulse * contact->m_c2;
btSoftBody::Face* face = contact->m_face;
if (face->m_n[0]->m_im > 0)
face->m_n[0]->m_v -= dv * contact->m_weights[0];
if (face->m_n[1]->m_im > 0)
face->m_n[1]->m_v -= dv * contact->m_weights[1];
if (face->m_n[2]->m_im > 0)
face->m_n[2]->m_v -= dv * contact->m_weights[2];
}
};
#endif /* BT_DEFORMABLE_CONTACT_CONSTRAINT_H */

225
src/BulletSoftBody/btDeformableContactProjection.cpp
View File

@ -20,8 +20,7 @@
btScalar btDeformableContactProjection::update()
{
btScalar residualSquare = 0;
// loop through constraints to set constrained values
// node constraints
for (int index = 0; index < m_nodeRigidConstraints.size(); ++index)
{
@ -34,15 +33,14 @@ btScalar btDeformableContactProjection::update()
}
// face constraints
// for (int index = 0; index < m_faceRigidConstraints.size(); ++index)
// {
// btAlignedObjectArray<btDeformableFaceRigidContactConstraint>& constraints = *m_faceRigidConstraints.getAtIndex(index);
// for (int i = 0; i < constraints.size(); ++i)
// {
// btScalar localResidualSquare = constraints[i].solveConstraint();
// residualSquare = btMax(residualSquare, localResidualSquare);
// }
// }
for (int index = 0; index < m_allFaceConstraints.size(); ++index)
{
btDeformableFaceRigidContactConstraint* constraint = m_allFaceConstraints[index];
btScalar localResidualSquare = constraint->solveConstraint();
residualSquare = btMax(residualSquare, localResidualSquare);
}
// todo xuchenhan@: deformable/deformable constraints
return residualSquare;
}
@ -68,8 +66,6 @@ void btDeformableContactProjection::setConstraints()
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
btMultiBodyJacobianData jacobianData_normal;
btMultiBodyJacobianData jacobianData_complementary;
for (int j = 0; j < psb->m_nodeRigidContacts.size(); ++j)
{
const btSoftBody::DeformableNodeRigidContact& contact = psb->m_nodeRigidContacts[j];
@ -99,38 +95,142 @@ void btDeformableContactProjection::setConstraints()
}
}
}
// set Deformable Face vs. Rigid constraint
for (int j = 0; j < psb->m_faceRigidContacts.size(); ++j)
{
const btSoftBody::DeformableFaceRigidContact& contact = psb->m_faceRigidContacts[j];
// skip fixed faces
if (contact.m_c2 == 0)
{
continue;
}
btDeformableFaceRigidContactConstraint* constraint = new btDeformableFaceRigidContactConstraint(contact);
btVector3 va = constraint->getVa();
btVector3 vb = constraint->getVb();
const btVector3 vr = vb - va;
const btSoftBody::sCti& cti = contact.m_cti;
const btScalar dn = btDot(vr, cti.m_normal);
if (dn < SIMD_EPSILON)
{
m_allFaceConstraints.push_back(constraint);
// add face constraints to each of the nodes
for (int k = 0; k < 3; ++k)
{
btSoftBody::Node* node = contact.m_face->m_n[k];
// static node does not need to own face/rigid constraint
if (node->m_im != 0)
{
if (m_faceRigidConstraints.find(node->index) == NULL)
{
btAlignedObjectArray<btDeformableFaceRigidContactConstraint*> constraintsList;
constraintsList.push_back(constraint);
m_faceRigidConstraints.insert(node->index, constraintsList);
}
else
{
btAlignedObjectArray<btDeformableFaceRigidContactConstraint*>& constraintsList = *m_faceRigidConstraints[node->index];
constraintsList.push_back(constraint);
}
}
}
}
else
{
delete constraint;
}
}
}
// todo xuchenhan@: set Deformable Face vs. Rigid constraint
// todo xuchenhan@: set Deformable Face vs. Deformable Node
}
void btDeformableContactProjection::enforceConstraint(TVStack& x)
{
for (int i = 0; i < x.size(); ++i)
// x is set to zero when passed in
// loop through node constraints to add in contributions to dv
for (int index = 0; index < m_nodeRigidConstraints.size(); ++index)
{
x[i].setZero();
if (m_staticConstraints.find(i) != NULL)
btAlignedObjectArray<btDeformableNodeRigidContactConstraint>& constraintsList = *m_nodeRigidConstraints.getAtIndex(index);
// i is node index
int i = m_nodeRigidConstraints.getKeyAtIndex(index).getUid1();
int numConstraints = 1;
// int numConstraints = constraintsList.size();
// if (m_faceRigidConstraints.find(i) != NULL)
// {
// numConstraints += m_faceRigidConstraints[i]->size();
// }
for (int j = 0; j < constraintsList.size(); ++j)
{
// if a node is fixed, dv = 0
continue;
const btDeformableNodeRigidContactConstraint& constraint = constraintsList[j];
x[i] += constraint.getDv(constraint.getContact()->m_node)/btScalar(numConstraints);
}
if (m_nodeRigidConstraints.find(i) != NULL)
}
// loop through face constraints to add in contributions to dv
// note that for each face constraint is owned by three nodes. Be careful here to only add the dv to the node that owns the constraint
for (int index = 0; index < m_faceRigidConstraints.size(); ++index)
{
btAlignedObjectArray<btDeformableFaceRigidContactConstraint*>& constraintsList = *m_faceRigidConstraints.getAtIndex(index);
// i is node index
int i = m_faceRigidConstraints.getKeyAtIndex(index).getUid1();
int numConstraints = 1;
// int numConstraints = constraintsList.size();
// if (m_nodeRigidConstraints.find(i) != NULL)
// {
// numConstraints += m_nodeRigidConstraints[i]->size();
// }
for (int j = 0; j < constraintsList.size(); ++j)
{
btAlignedObjectArray<btDeformableNodeRigidContactConstraint>& constraintsList = *m_nodeRigidConstraints[i];
for (int j = 0; j < constraintsList.size(); ++j)
const btDeformableFaceRigidContactConstraint* constraint = constraintsList[j];
const btSoftBody::Face* face = constraint->m_face;
btSoftBody::Node* node;
// find the node that owns the constraint
for (int k = 0; k < 3; ++k)
{
const btDeformableNodeRigidContactConstraint& constraint = constraintsList[j];
x[i] += constraint.getDv(m_nodes->at(i));
if (face->m_n[k]->index == i)
{
node = face->m_n[k];
break;
}
}
x[i] += constraint->getDv(node) / btScalar(numConstraints);
}
}
// todo xuchenhan@: add deformable deformable constraints' contribution to dv
// Finally, loop through static constraints to set dv of static nodes to zero
for (int index = 0; index < m_staticConstraints.size(); ++index)
{
const btDeformableStaticConstraint& constraint = *m_staticConstraints.getAtIndex(index);
int i = m_staticConstraints.getKeyAtIndex(index).getUid1();
x[i] = constraint.getDv(constraint.m_node);
}
//
// for (int i = 0; i < x.size(); ++i)
// {
// x[i].setZero();
// if (m_staticConstraints.find(i) != NULL)
// {
// // if a node is fixed, dv = 0
// continue;
// }
// if (m_nodeRigidConstraints.find(i) != NULL)
// {
// btAlignedObjectArray<btDeformableNodeRigidContactConstraint>& constraintsList = *m_nodeRigidConstraints[i];
// for (int j = 0; j < constraintsList.size(); ++j)
// {
// const btDeformableNodeRigidContactConstraint& constraint = constraintsList[j];
//// x[i] += constraint.getDv(m_nodes->at(i));
// x[i] += constraint.getDv(constraint.getContact()->m_node);
// }
// }
// todo xuchenhan@
// if (m_faceRigidConstraints.find(i) != NULL)
// {
//
// }
}
// }
}
void btDeformableContactProjection::project(TVStack& x)
@ -202,16 +302,27 @@ void btDeformableContactProjection::setProjection()
break;
}
const btVector3& local_normal = constraintsList[k].m_normal;
// add another projection direction if it deviates from the average by more than about 15 degrees
normals.push_back(local_normal);
averagedNormal += local_normal;
}
}
// todo: xuchenhan@ implement face
// if (!existStaticConstraint && m_faceRigidConstraints.find(index) != NULL)
// {
// }
if (!existStaticConstraint && m_faceRigidConstraints.find(index) != NULL)
{
hasConstraint = true;
btAlignedObjectArray<btDeformableFaceRigidContactConstraint*>& constraintsList = *m_faceRigidConstraints[index];
for (int k = 0; k < constraintsList.size(); ++k)
{
if (constraintsList[k]->m_static)
{
existStaticConstraint = true;
break;
}
const btVector3& local_normal = constraintsList[k]->m_normal;
normals.push_back(local_normal);
averagedNormal += local_normal;
}
}
// build projections
@ -252,6 +363,48 @@ void btDeformableContactProjection::setProjection()
void btDeformableContactProjection::applyDynamicFriction(TVStack& f)
{
// loop over constraints
for (int i = 0; i < f.size(); ++i)
{
if (m_projectionsDict.find(i) != NULL)
{
// doesn't need to add friction force for fully constrained vertices
btAlignedObjectArray<btVector3>& projectionDirs = *m_projectionsDict[i];
if (projectionDirs.size() >= 3)
{
continue;
}
}
if (m_nodeRigidConstraints.find(i) != NULL)
{
btAlignedObjectArray<btDeformableNodeRigidContactConstraint>& constraintsList = *m_nodeRigidConstraints[i];
for (int j = 0; j < constraintsList.size(); ++j)
{
const btDeformableNodeRigidContactConstraint& constraint = constraintsList[j];
btSoftBody::Node* node = constraint.getContact()->m_node;
// it's ok to add the friction force generated by the entire impulse here because the normal component of the residual will be projected out anyway.
f[i] += constraint.getDv(node)* (1./node->m_im);
}
}
// todo xuchenhan@
if (m_faceRigidConstraints.find(i) != NULL)
{
btAlignedObjectArray<btDeformableFaceRigidContactConstraint*>& constraintsList = *m_faceRigidConstraints[i];
for (int j = 0; j < constraintsList.size(); ++j)
{
const btDeformableFaceRigidContactConstraint* constraint = constraintsList[j];
btSoftBody::Face* face = constraint->getContact()->m_face;
// it's ok to add the friction force generated by the entire impulse here because the normal component of the residual will be projected out anyway.
// todo xuchenhan@: figure out the index for m_faceRigidConstraints
// f[i] += constraint.getDv(face->m_n[0])* (1./face->m_n[0]->m_im);
// f[i] += constraint.getDv(face->m_n[1])* (1./face->m_n[1]->m_im);
// f[i] += constraint.getDv(face->m_n[2])* (1./face->m_n[2]->m_im);
}
}
}
// for (int index = 0; index < m_constraints.size(); ++index)
// {
// const DeformableContactConstraint& constraint = *m_constraints.getAtIndex(index);
@ -286,7 +439,13 @@ void btDeformableContactProjection::reinitialize(bool nodeUpdated)
btCGProjection::reinitialize(nodeUpdated);
m_staticConstraints.clear();
m_nodeRigidConstraints.clear();
// m_faceRigidConstraints.clear();
m_faceRigidConstraints.clear();
m_projectionsDict.clear();
for (int i = 0; i < m_allFaceConstraints.size(); ++i)
{
delete m_allFaceConstraints[i];
}
m_allFaceConstraints.clear();
}

8
src/BulletSoftBody/btDeformableContactProjection.h
View File

@ -21,6 +21,7 @@
#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
#include "btDeformableContactConstraint.h"
#include "LinearMath/btHashMap.h"
#include <vector>
class btDeformableContactProjection : public btCGProjection
{
public:
@ -28,9 +29,10 @@ public:
btHashMap<btHashInt, btDeformableStaticConstraint> m_staticConstraints;
// map from node index to node rigid constraint
btHashMap<btHashInt, btAlignedObjectArray<btDeformableNodeRigidContactConstraint> > m_nodeRigidConstraints;
// // map from node index to face rigid constraint
// btHashMap<btHashInt, btAlignedObjectArray<btDeformableFaceRigidContactConstraint> > m_faceRigidConstraints;
// map from node index to face rigid constraint
btHashMap<btHashInt, btAlignedObjectArray<btDeformableFaceRigidContactConstraint*> > m_faceRigidConstraints;
btAlignedObjectArray<btDeformableFaceRigidContactConstraint*> m_allFaceConstraints;
// map from node index to projection directions
btHashMap<btHashInt, btAlignedObjectArray<btVector3> > m_projectionsDict;

1
src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp
View File

@ -39,6 +39,7 @@ btScalar btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlyIteration
printf("residual = %f at iteration #%d\n", m_leastSquaresResidual, iteration);
#endif
m_analyticsData.m_numSolverCalls++;
std::cout << m_leastSquaresResidual << std::endl;
m_analyticsData.m_numIterationsUsed = iteration+1;
m_analyticsData.m_islandId = -2;
if (numBodies>0)

4
src/BulletSoftBody/btSoftBody.cpp
View File

@ -2368,7 +2368,7 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO
btVector3 guess(0,0,0);
const btConvexShape* csh = static_cast<const btConvexShape*>(shp);
btGjkEpaSolver2::SignedDistance(&triangle, triangle_transform, csh, wtr, guess, results);
btScalar dst = results.distance;
btScalar dst = results.distance - margin;
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);
if (!predict)
@ -3431,7 +3431,7 @@ void btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap
docollideFace.m_rigidBody = prb1;
docollideFace.dynmargin = basemargin + timemargin;
docollideFace.stamargin = basemargin;
m_fdbvt.collideTV(m_fdbvt.m_root, volume, docollideFace);
// m_fdbvt.collideTV(m_fdbvt.m_root, volume, docollideFace);
}
break;
}

1
src/BulletSoftBody/btSoftBodyInternals.h
View File

@ -1104,6 +1104,7 @@ struct btSoftColliders
btSoftBody::sCti& cti = c.m_cti;
c.m_contactPoint = contact_point;
c.m_bary = bary;
// todo xuchenhan@: check m_c2 and m_weights
c.m_weights = btScalar(2)/(btScalar(1) + bary.length2()) * bary;
c.m_face = &f;
const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction();