Scarfski/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3.git synced 2026-09-05 09:58:36 +00:00
Code Issues Packages Projects Releases Wiki Activity

bug fixes in constraints projections; cpplized various functions

Browse Source
This commit is contained in:
Xuchen Han
2019-07-09 14:26:04 -07:00
parent 786b0436ec
commit 13d4e1cc2b
12 changed files with 450 additions and 263 deletions
Download Patch File Download Diff File Expand all files Collapse all files

151
src/BulletSoftBody/btContactProjection.cpp
View File

@@ -7,44 +7,16 @@
#include "btContactProjection.h"
#include "btDeformableRigidDynamicsWorld.h"
void btContactProjection::update(btScalar dt, const TVStack& dv)
void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocity)
{
///solve rigid body constraints
m_world->btSoftRigidDynamicsWorld::btDiscreteDynamicsWorld::solveConstraints(m_world->getSolverInfo());
// clear the old constraints
for (int i = 0; i < m_constrainedDirections.size(); ++i)
{
m_constrainedDirections[i].clear();
m_constrainedValues[i].clear();
}
// Set dirichlet constraints
size_t counter = 0;
for (int i = 0; i < m_softBodies.size(); ++i)
{
const btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
if (psb->m_nodes[j].m_im == 0)
{
m_constrainedDirections[counter].push_back(btVector3(1,0,0));
m_constrainedDirections[counter].push_back(btVector3(0,1,0));
m_constrainedDirections[counter].push_back(btVector3(0,0,1));
m_constrainedValues[counter].push_back(0);
m_constrainedValues[counter].push_back(0);
m_constrainedValues[counter].push_back(0);
}
++counter;
}
}
// loop through contacts to create contact constraints
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
btMultiBodyJacobianData jacobianData;
const btScalar mrg = psb->getCollisionShape()->getMargin();
for (int i = 0, ni = psb->m_rcontacts.size(); i < ni; ++i)
{
const btSoftBody::RContact& c = psb->m_rcontacts[i];
@@ -65,7 +37,7 @@ void btContactProjection::update(btScalar dt, const TVStack& dv)
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);
va = rigidCol ? (rigidCol->getVelocityInLocalPoint(c.m_c1)) * m_dt : btVector3(0, 0, 0);
}
else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
{
@@ -86,29 +58,25 @@ void btContactProjection::update(btScalar dt, const TVStack& dv)
{
vel += multibodyLinkCol->m_multiBody->getVelocityVector()[j] * jac[j];
}
va = cti.m_normal * vel * dt;
va = cti.m_normal * vel * m_dt;
}
}
// TODO: rethink what the velocity of the soft body node should be
// const btVector3 vb = c.m_node->m_x - c.m_node->m_q;
const btVector3 vb = c.m_node->m_v * dt;
const btVector3 vb = c.m_node->m_v * m_dt;
const btVector3 vr = vb - va;
const btScalar dn = btDot(vr, cti.m_normal);
if (dn <= SIMD_EPSILON)
if (1) // in the same CG solve, the set of constraits doesn't change
// 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)));
const btVector3 impulse = c.m_c0 * ((vr - (fv * c.m_c3))+ (cti.m_normal * (dp * c.m_c4)));
const btVector3 impulse = c.m_c0 *(cti.m_normal * dn);
// TODO: only contact is considered here, add friction later
btVector3 normal = cti.m_normal.normalized();
btVector3 dv = -impulse * c.m_c2/dt;
btScalar dvn = dv.dot(normal);
m_constrainedDirections[m_indices[c.m_node]].push_back(normal);
m_constrainedValues[m_indices[c.m_node]].push_back(dvn);
// dv = new_impulse + accumulated velocity change in previous CG iterations
// so we have the invariant node->m_v = backupVelocity + dv;
btVector3 dv = -impulse * c.m_c2/m_dt + c.m_node->m_v - backupVelocity[m_indices[c.m_node]];
btScalar dvn = dv.dot(cti.m_normal);
m_constrainedValues[m_indices[c.m_node]][0]=(dvn);
if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
{
@@ -128,3 +96,94 @@ void btContactProjection::update(btScalar dt, const TVStack& dv)
}
}
}
void btContactProjection::setConstraintDirections()
{
// set Dirichlet constraint
size_t counter = 0;
for (int i = 0; i < m_softBodies.size(); ++i)
{
const btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
if (psb->m_nodes[j].m_im == 0)
{
m_constrainedDirections[counter].push_back(btVector3(1,0,0));
m_constrainedDirections[counter].push_back(btVector3(0,1,0));
m_constrainedDirections[counter].push_back(btVector3(0,0,1));
m_constrainedValues[counter].push_back(0);
m_constrainedValues[counter].push_back(0);
m_constrainedValues[counter].push_back(0);
}
++counter;
}
}
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
btMultiBodyJacobianData jacobianData;
int j = 0;
while (j < psb->m_rcontacts.size())
{
const btSoftBody::RContact& c = psb->m_rcontacts[j];
// skip anchor points
if (c.m_node->m_im == 0)
{
psb->m_rcontacts.removeAtIndex(j);
continue;
}
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;
// 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->getVelocityInLocalPoint(c.m_c1)) * m_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 * m_dt;
}
}
const btVector3 vb = c.m_node->m_v * m_dt;
const btVector3 vr = vb - va;
const btScalar dn = btDot(vr, cti.m_normal);
if (dn < SIMD_EPSILON)
{
++j;
m_constrainedDirections[m_indices[c.m_node]].push_back(cti.m_normal);
m_constrainedValues[m_indices[c.m_node]].resize(m_constrainedValues[m_indices[c.m_node]].size()+1);
continue;
}
}
psb->m_rcontacts.removeAtIndex(j);
}
}
}