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

fix bugs in poststablize and projection of colinear constraints

Browse Source
This commit is contained in:
Xuchen Han
2019-07-15 10:48:20 -07:00
parent ac628f4d39
commit bac7d461c5
5 changed files with 89 additions and 69 deletions
Download Patch File Download Diff File Expand all files Collapse all files

31
src/BulletSoftBody/btContactProjection.cpp
View File

@@ -11,6 +11,7 @@
void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocity)
{
///solve rigid body constraints
m_world->getSolverInfo().m_numIterations = 5;
m_world->btMultiBodyDynamicsWorld::solveConstraints(m_world->getSolverInfo());
// loop through constraints to set constrained values
@@ -258,21 +259,21 @@ void btContactProjection::setConstraintDirections()
}
else
{
// prune out collinear constraints
const btVector3& first_dir = c[0].m_direction;
int i = 1;
while (i < c.size())
{
if (std::abs(std::abs(first_dir.dot(c[i].m_direction)) - 1) < 4*SIMD_EPSILON)
c.removeAtIndex(i);
else
++i;
}
if (c.size() == 3)
{
if (std::abs(std::abs(c[1].m_direction.dot(c[2].m_direction)) - 1) < 4*SIMD_EPSILON)
c.removeAtIndex(2);
}
// // prune out collinear constraints
// const btVector3& first_dir = c[0].m_direction;
// int i = 1;
// while (i < c.size())
// {
// if (std::abs(std::abs(first_dir.dot(c[i].m_direction)) - 1) < 4*SIMD_EPSILON)
// c.removeAtIndex(i);
// else
// ++i;
// }
// if (c.size() == 3)
// {
// if (std::abs(std::abs(c[1].m_direction.dot(c[2].m_direction)) - 1) < 4*SIMD_EPSILON)
// c.removeAtIndex(2);
// }
}
}
}

25
src/BulletSoftBody/btContactProjection.h
View File

@@ -47,8 +47,13 @@ public:
{
// TODO : friction
btVector3 free_dir = btCross(constraints[0].m_direction, constraints[1].m_direction);
free_dir.normalize();
x[i] = x[i].dot(free_dir) * free_dir;
if (free_dir.norm() < SIMD_EPSILON)
x[i] -= x[i].dot(constraints[0].m_direction) * constraints[0].m_direction;
else
{
free_dir.normalize();
x[i] = x[i].dot(free_dir) * free_dir;
}
}
else
x[i].setZero();
@@ -69,7 +74,8 @@ public:
if (constraints.size() == 1)
{
x[i] -= x[i].dot(constraints[0].m_direction) * constraints[0].m_direction;
x[i] += constraints[0].m_value * constraints[0].m_direction;
btVector3 diff = constraints[0].m_value * constraints[0].m_direction;
x[i] += diff;
if (friction.m_direction.norm() > SIMD_EPSILON)
{
x[i] -= x[i].dot(friction.m_direction) * friction.m_direction;
@@ -79,8 +85,17 @@ public:
else if (constraints.size() == 2)
{
btVector3 free_dir = btCross(constraints[0].m_direction, constraints[1].m_direction);
free_dir.normalize();
x[i] = x[i].dot(free_dir) * free_dir + constraints[0].m_direction * constraints[0].m_value + constraints[1].m_direction * constraints[1].m_value;
if (free_dir.norm() < SIMD_EPSILON)
{
x[i] -= x[i].dot(constraints[0].m_direction) * constraints[0].m_direction;
btVector3 diff = constraints[0].m_value * constraints[0].m_direction + constraints[1].m_value * constraints[1].m_direction;
x[i] += diff;
}
else
{
free_dir.normalize();
x[i] = x[i].dot(free_dir) * free_dir + constraints[0].m_direction * constraints[0].m_value + constraints[1].m_direction * constraints[1].m_value;
}
}
else
x[i] = constraints[0].m_value * constraints[0].m_direction + constraints[1].m_value * constraints[1].m_direction + constraints[2].m_value * constraints[2].m_direction;

37
src/BulletSoftBody/btDeformableBodySolver.cpp
View File

@@ -77,8 +77,8 @@ void btDeformableBodySolver::postStabilize()
const btVector3 vr = vb - va;
const btScalar dn = btDot(vr, cti.m_normal);
const btScalar dp = btMin((btDot(c.m_node->m_x, cti.m_normal) + cti.m_offset), mrg);
btScalar dp = btMin((btDot(c.m_node->m_x, cti.m_normal) + cti.m_offset), mrg);
// dp += mrg;
// c0 is the impulse matrix, c3 is 1 - the friction coefficient or 0, c4 is the contact hardness coefficient
btScalar dvn = dn * c.m_c4;
@@ -86,23 +86,27 @@ void btDeformableBodySolver::postStabilize()
// TODO: only contact is considered here, add friction later
if (dp < 0)
{
c.m_node->m_x -= dp * cti.m_normal * c.m_c4;
// c.m_node->m_x -= impulse * c.m_c2;
////
if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
bool two_way = false;
if (two_way)
{
if (rigidCol)
rigidCol->applyImpulse(impulse, c.m_c1);
}
else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
{
if (multibodyLinkCol)
c.m_node->m_x -= impulse * c.m_c2;
if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
{
double multiplier = 0.5;
multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV, -impulse.length() * multiplier);
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);
}
}
}
else
c.m_node->m_x -= dp * cti.m_normal * c.m_c4;
}
}
}
@@ -115,7 +119,6 @@ void btDeformableBodySolver::solveConstraints(float solverdt)
bool nodeUpdated = updateNodes();
reinitialize(nodeUpdated);
backupVelocity();
postStabilize();
for (int i = 0; i < m_solveIterations; ++i)
{
m_objective->computeResidual(solverdt, m_residual);
@@ -123,7 +126,7 @@ void btDeformableBodySolver::solveConstraints(float solverdt)
updateVelocity();
}
advect(solverdt);
// postStabilize();
postStabilize();
}
void btDeformableBodySolver::reinitialize(bool nodeUpdated)

5
src/BulletSoftBody/btDeformableBodySolver.h
View File

@@ -2,7 +2,7 @@
// btDeformableBodySolver.h
// BulletSoftBody
//
// Created by Chuyuan Fu on 7/1/19.
// Created by Xuchen Han on 7/1/19.
//
#ifndef BT_DEFORMABLE_BODY_SOLVERS_H
@@ -101,8 +101,7 @@ public:
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
auto& node = psb->m_nodes[j];
node.m_x += dt * m_dv[counter++];
// node.m_x = node.m_q + dt * node.m_v;
node.m_x = node.m_q + dt * node.m_v;
}
}
}

60
src/BulletSoftBody/btSoftBodyInternals.h
View File

@@ -869,35 +869,37 @@ struct btSoftColliders
const btScalar m = n.m_im > 0 ? dynmargin : stamargin;
btSoftBody::RContact c;
if ((!n.m_battach) &&
psb->checkContact(m_colObj1Wrap, n.m_x, m, c.m_cti))
{
const btScalar ima = n.m_im;
const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f;
const btScalar ms = ima + imb;
if (ms > 0)
{
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_q - 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 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);
c.m_c1 = ra;
c.m_c2 = ima * psb->m_sst.sdt;
// c.m_c3 = fv.length2() < (dn * fc * dn * fc) ? 0 : 1 - fc;
c.m_c3 = fc;
c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR;
psb->m_rcontacts.push_back(c);
if (m_rigidBody)
m_rigidBody->activate();
}
if (!n.m_battach)
{
if (psb->checkContact(m_colObj1Wrap, n.m_x, m, c.m_cti))
{
const btScalar ima = n.m_im;
const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f;
const btScalar ms = ima + imb;
if (ms > 0)
{
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 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);
c.m_c1 = ra;
c.m_c2 = ima * psb->m_sst.sdt;
// c.m_c3 = fv.length2() < (dn * fc * dn * fc) ? 0 : 1 - fc;
c.m_c3 = fc;
c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR;
psb->m_rcontacts.push_back(c);
if (m_rigidBody)
m_rigidBody->activate();
}
}
}
}
btSoftBody* psb;