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

25
src/BulletSoftBody/btContactProjection.h
View File

@@ -47,8 +47,13 @@ public:
{ {
// TODO : friction // TODO : friction
btVector3 free_dir = btCross(constraints[0].m_direction, constraints[1].m_direction); btVector3 free_dir = btCross(constraints[0].m_direction, constraints[1].m_direction);
free_dir.normalize(); if (free_dir.norm() < SIMD_EPSILON)
x[i] = x[i].dot(free_dir) * free_dir; 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 else
x[i].setZero(); x[i].setZero();
@@ -69,7 +74,8 @@ public:
if (constraints.size() == 1) if (constraints.size() == 1)
{ {
x[i] -= x[i].dot(constraints[0].m_direction) * constraints[0].m_direction; 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) if (friction.m_direction.norm() > SIMD_EPSILON)
{ {
x[i] -= x[i].dot(friction.m_direction) * friction.m_direction; x[i] -= x[i].dot(friction.m_direction) * friction.m_direction;
@@ -79,8 +85,17 @@ public:
else if (constraints.size() == 2) else if (constraints.size() == 2)
{ {
btVector3 free_dir = btCross(constraints[0].m_direction, constraints[1].m_direction); btVector3 free_dir = btCross(constraints[0].m_direction, constraints[1].m_direction);
free_dir.normalize(); if (free_dir.norm() < SIMD_EPSILON)
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; {
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 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; 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 btVector3 vr = vb - va;
const btScalar dn = btDot(vr, cti.m_normal); 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 // 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; btScalar dvn = dn * c.m_c4;
@@ -86,23 +86,27 @@ void btDeformableBodySolver::postStabilize()
// TODO: only contact is considered here, add friction later // TODO: only contact is considered here, add friction later
if (dp < 0) if (dp < 0)
{ {
c.m_node->m_x -= dp * cti.m_normal * c.m_c4; bool two_way = false;
// c.m_node->m_x -= impulse * c.m_c2; if (two_way)
////
if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
{ {
if (rigidCol) c.m_node->m_x -= impulse * c.m_c2;
rigidCol->applyImpulse(impulse, c.m_c1);
} if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
{
if (multibodyLinkCol)
{ {
double multiplier = 0.5; if (rigidCol)
multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV, -impulse.length() * multiplier); 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(); bool nodeUpdated = updateNodes();
reinitialize(nodeUpdated); reinitialize(nodeUpdated);
backupVelocity(); backupVelocity();
postStabilize();
for (int i = 0; i < m_solveIterations; ++i) for (int i = 0; i < m_solveIterations; ++i)
{ {
m_objective->computeResidual(solverdt, m_residual); m_objective->computeResidual(solverdt, m_residual);
@@ -123,7 +126,7 @@ void btDeformableBodySolver::solveConstraints(float solverdt)
updateVelocity(); updateVelocity();
} }
advect(solverdt); advect(solverdt);
// postStabilize(); postStabilize();
} }
void btDeformableBodySolver::reinitialize(bool nodeUpdated) void btDeformableBodySolver::reinitialize(bool nodeUpdated)

5
src/BulletSoftBody/btDeformableBodySolver.h
View File

@@ -2,7 +2,7 @@
// btDeformableBodySolver.h // btDeformableBodySolver.h
// BulletSoftBody // BulletSoftBody
// //
// Created by Chuyuan Fu on 7/1/19. // Created by Xuchen Han on 7/1/19.
// //
#ifndef BT_DEFORMABLE_BODY_SOLVERS_H #ifndef BT_DEFORMABLE_BODY_SOLVERS_H
@@ -101,8 +101,7 @@ public:
for (int j = 0; j < psb->m_nodes.size(); ++j) for (int j = 0; j < psb->m_nodes.size(); ++j)
{ {
auto& node = psb->m_nodes[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; const btScalar m = n.m_im > 0 ? dynmargin : stamargin;
btSoftBody::RContact c; btSoftBody::RContact c;
if ((!n.m_battach) && if (!n.m_battach)
psb->checkContact(m_colObj1Wrap, n.m_x, m, c.m_cti)) {
{ 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 ima = n.m_im;
const btScalar ms = ima + imb; const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f;
if (ms > 0) 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 btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform();
const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic; static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0);
const btVector3 ra = n.m_q - wtr.getOrigin(); const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic;
const btVector3 va = m_rigidBody ? m_rigidBody->getVelocityInLocalPoint(ra) * psb->m_sst.sdt : btVector3(0, 0, 0); const btVector3 ra = n.m_x - wtr.getOrigin();
const btVector3 vb = n.m_x - n.m_q; const btVector3 va = m_rigidBody ? m_rigidBody->getVelocityInLocalPoint(ra) * psb->m_sst.sdt : btVector3(0, 0, 0);
const btVector3 vr = vb - va; const btVector3 vb = n.m_x - n.m_q;
const btScalar dn = btDot(vr, c.m_cti.m_normal); const btVector3 vr = vb - va;
const btVector3 fv = vr - c.m_cti.m_normal * dn; const btScalar dn = btDot(vr, c.m_cti.m_normal);
const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction(); const btVector3 fv = vr - c.m_cti.m_normal * dn;
c.m_node = &n; const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction();
c.m_c0 = ImpulseMatrix(psb->m_sst.sdt, ima, imb, iwi, ra); c.m_node = &n;
c.m_c1 = ra; c.m_c0 = ImpulseMatrix(psb->m_sst.sdt, ima, imb, iwi, ra);
c.m_c2 = ima * psb->m_sst.sdt; c.m_c1 = ra;
// c.m_c3 = fv.length2() < (dn * fc * dn * fc) ? 0 : 1 - fc; c.m_c2 = ima * psb->m_sst.sdt;
c.m_c3 = fc; // c.m_c3 = fv.length2() < (dn * fc * dn * fc) ? 0 : 1 - fc;
c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR; c.m_c3 = fc;
psb->m_rcontacts.push_back(c); c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR;
if (m_rigidBody) psb->m_rcontacts.push_back(c);
m_rigidBody->activate(); if (m_rigidBody)
} m_rigidBody->activate();
}
}
} }
} }
btSoftBody* psb; btSoftBody* psb;