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

add contact constraint as projections in CG

Browse Source
This commit is contained in:
Xuchen Han
2019-07-04 23:07:02 -07:00
parent 32836b0694
commit 35ce2ae0e2
9 changed files with 392 additions and 141 deletions
Download Patch File Download Diff File Expand all files Collapse all files

116
src/BulletSoftBody/btDeformableBodySolver.h
View File

@@ -10,12 +10,14 @@
#include "btSoftBodySolvers.h"
#include "btBackwardEulerObjective.h"
#include "btDeformableRigidDynamicsWorld.h"
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
struct btCollisionObjectWrapper;
class btDeformableRigidDynamicsWorld;
class btDeformableBodySolver : public btSoftBodySolver
{
using TVStack = btAlignedObjectArray<btVector3>;
@@ -29,13 +31,16 @@ protected:
btBackwardEulerObjective m_objective;
int m_solveIterations;
int m_impulseIterations;
btDeformableRigidDynamicsWorld* m_world;
btAlignedObjectArray<btVector3> m_backupVelocity;
public:
btDeformableBodySolver()
: m_numNodes(0)
, m_objective(m_softBodySet)
, m_objective(m_softBodySet, m_backupVelocity)
, m_solveIterations(1)
, m_impulseIterations(1)
, m_world(nullptr)
{
}
@@ -76,23 +81,11 @@ public:
{
bool nodeUpdated = updateNodes();
reinitialize(nodeUpdated);
backupVelocity();
for (int i = 0; i < m_solveIterations; ++i)
{
// get the velocity after contact solve
// TODO: perform contact solve here
for (int j = 0; j < m_impulseIterations; ++j)
{
for (int s = 0; s < m_softBodySet.size(); ++s)
VSolve_RContacts(m_softBodySet[s], 0, solverdt);
}
// advect with v_n+1 ** to update position based states
// where v_n+1 ** is the velocity after contact response
// only need to advect x here if elastic force is implicit
// prepareSolve(solverdt);
m_objective.computeResidual(solverdt, m_residual);
m_objective.computeStep(m_dv, m_residual, solverdt);
@@ -107,7 +100,9 @@ public:
{
m_dv.resize(m_numNodes);
m_residual.resize(m_numNodes);
m_backupVelocity.resize(m_numNodes);
}
for (int i = 0; i < m_dv.size(); ++i)
{
m_dv[i].setZero();
@@ -151,7 +146,22 @@ public:
btSoftBody* psb = m_softBodySet[i];
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
psb->m_nodes[j].m_v += m_dv[counter++];
psb->m_nodes[j].m_v = m_backupVelocity[counter] + m_dv[counter];
++counter;
}
}
}
void backupVelocity()
{
// 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)
{
m_backupVelocity[counter++] = psb->m_nodes[j].m_v;
}
}
}
@@ -168,6 +178,7 @@ public:
}
return false;
}
virtual void predictMotion(float solverdt)
{
for (int i = 0; i < m_softBodySet.size(); ++i)
@@ -192,78 +203,11 @@ public:
// TODO
}
void VSolve_RContacts(btSoftBody* psb, btScalar kst, btScalar dt)
virtual void setWorld(btDeformableRigidDynamicsWorld* world)
{
const btScalar mrg = psb->getCollisionShape()->getMargin();
btMultiBodyJacobianData jacobianData;
for (int i = 0, ni = psb->m_rcontacts.size(); i < ni; ++i)
{
const btSoftBody::RContact& c = psb->m_rcontacts[i];
const btSoftBody::sCti& cti = c.m_cti;
if (cti.m_colObj->hasContactResponse())
{
btVector3 va(0, 0, 0);
btRigidBody* rigidCol = 0;
btMultiBodyLinkCollider* multibodyLinkCol = 0;
btScalar* deltaV;
if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
{
rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
va = rigidCol ? rigidCol->getVelocityInLocalPoint(c.m_c1) * dt : 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;
jacobianData.m_jacobians.resize(ndof);
jacobianData.m_deltaVelocitiesUnitImpulse.resize(ndof);
btScalar* jac = &jacobianData.m_jacobians[0];
multibodyLinkCol->m_multiBody->fillContactJacobianMultiDof(multibodyLinkCol->m_link, c.m_node->m_x, cti.m_normal, jac, jacobianData.scratch_r, jacobianData.scratch_v, jacobianData.scratch_m);
deltaV = &jacobianData.m_deltaVelocitiesUnitImpulse[0];
multibodyLinkCol->m_multiBody->calcAccelerationDeltasMultiDof(&jacobianData.m_jacobians[0], deltaV, jacobianData.scratch_r, jacobianData.scratch_v);
btScalar vel = 0.0;
for (int j = 0; j < ndof; ++j)
{
vel += multibodyLinkCol->m_multiBody->getVelocityVector()[j] * jac[j];
}
va = cti.m_normal * vel * dt;
}
}
const btVector3 vb = c.m_node->m_x - c.m_node->m_q;
const btVector3 vr = vb - va;
const btScalar dn = btDot(vr, cti.m_normal);
if (dn <= SIMD_EPSILON)
{
const btScalar dp = btMin((btDot(c.m_node->m_x, cti.m_normal) + cti.m_offset), mrg);
const btVector3 fv = vr - (cti.m_normal * dn);
// c0 is the impulse matrix, c3 is 1 - the friction coefficient or 0, c4 is the contact hardness coefficient
const btVector3 impulse = c.m_c0 * ((vr - (fv * c.m_c3) + (cti.m_normal * (dp * c.m_c4))) * kst);
c.m_node->m_v -= impulse * c.m_c2 / dt;
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 = 0.5;
multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV, -impulse.length() * multiplier);
}
}
}
}
}
m_world = world;
m_objective.setWorld(world);
}
};
#endif /* btDeformableBodySolver_h */