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

bug fix in projection; start friction

Browse Source
This commit is contained in:
Xuchen Han
2019-07-12 10:03:38 -07:00
parent 4e5f4b9fe9
commit 696c96f392
6 changed files with 75 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
src/BulletSoftBody/btBackwardEulerObjective.cpp
View File

@@ -8,7 +8,7 @@
#include "btBackwardEulerObjective.h"
btBackwardEulerObjective::btBackwardEulerObjective(btAlignedObjectArray<btSoftBody *>& softBodies, const TVStack& backup_v)
: cg(50)
: cg(20)
, m_softBodies(softBodies)
, precondition(DefaultPreconditioner())
, projection(m_softBodies, m_dt)

29
src/BulletSoftBody/btCGProjection.h
View File

@@ -15,7 +15,7 @@ class btDeformableRigidDynamicsWorld;
struct Constraint
{
const btSoftBody::RContact* m_contact;
const btVector3 m_direction;
btVector3 m_direction;
btScalar m_value;
Constraint(const btSoftBody::RContact& rcontact)
@@ -36,7 +36,17 @@ struct Constraint
{
}
};
struct Friction
{
btVector3 m_dv;
btVector3 m_direction;
Friction()
{
m_dv.setZero();
m_direction.setZero();
}
};
class btCGProjection
@@ -49,11 +59,9 @@ public:
btAlignedObjectArray<btSoftBody *> m_softBodies;
btDeformableRigidDynamicsWorld* m_world;
std::unordered_map<btSoftBody::Node *, size_t> m_indices;
// TVArrayStack m_constrainedDirections;
// TArrayStack m_constrainedValues;
// btAlignedObjectArray<int> m_constrainedId;
const btScalar& m_dt;
std::unordered_map<btSoftBody::Node *, btAlignedObjectArray<Constraint> > m_constraints;
std::unordered_map<btSoftBody::Node *, Friction > m_frictions;
btCGProjection(btAlignedObjectArray<btSoftBody *>& softBodies, const btScalar& dt)
: m_softBodies(softBodies)
@@ -77,17 +85,8 @@ public:
{
if (nodeUpdated)
updateId();
// resize and clear the old constraints
// m_constrainedValues.resize(m_indices.size());
// m_constrainedDirections.resize(m_indices.size());
// for (int i = 0; i < m_constrainedDirections.size(); ++i)
// {
// m_constrainedDirections[i].clear();
// m_constrainedValues[i].clear();
// }
// m_constrainedId.clear();
m_constraints.clear();
m_frictions.clear();
}
void updateId()

56
src/BulletSoftBody/btContactProjection.cpp
View File

@@ -7,14 +7,16 @@
#include "btContactProjection.h"
#include "btDeformableRigidDynamicsWorld.h"
#include <algorithm>
void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocity)
{
///solve rigid body constraints
m_world->btMultiBodyDynamicsWorld::solveConstraints(m_world->getSolverInfo());
// loop through constraints to set constrained values
for (auto it : m_constraints)
for (auto& it : m_constraints)
{
Friction& friction = m_frictions[it.first];
btAlignedObjectArray<Constraint>& constraints = it.second;
for (int i = 0; i < constraints.size(); ++i)
{
@@ -66,11 +68,27 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit
const btVector3 vb = c->m_node->m_v * m_dt;
const btVector3 vr = vb - va;
const btScalar dn = btDot(vr, cti.m_normal);
if (1) // in the same CG solve, the set of constraits doesn't change
// if (dn <= SIMD_EPSILON)
btVector3 impulse = c->m_c0 * vr;
const btVector3 impulse_normal = c->m_c0 *(cti.m_normal * dn);
btVector3 impulse_tangent = impulse - impulse_normal;
if (dn < 0 && impulse_tangent.norm() > SIMD_EPSILON)
{
btScalar impulse_tangent_magnitude = std::min(impulse_normal.norm()*c->m_c3, impulse_tangent.norm());
impulse_tangent_magnitude = 0;
const btVector3 tangent_dir = impulse_tangent.normalized();
impulse_tangent = impulse_tangent_magnitude * tangent_dir;
friction.m_direction = impulse_tangent;
friction.m_dv = -impulse_tangent * c->m_c2/m_dt + (c->m_node->m_v - backupVelocity[m_indices[c->m_node]]).dot(tangent_dir)*tangent_dir;
}
impulse = impulse_normal + impulse_tangent;
// if (1) // in the same CG solve, the set of constraits doesn't change
if (dn <= SIMD_EPSILON)
{
// 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 *(cti.m_normal * dn);
// TODO: only contact is considered here, add friction later
// dv = new_impulse + accumulated velocity change in previous CG iterations
@@ -82,7 +100,7 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit
if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
{
if (rigidCol)
rigidCol->applyImpulse(impulse, c->m_c1);
rigidCol->applyImpulse(impulse_normal, c->m_c1);
}
else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
{
@@ -102,7 +120,6 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit
void btContactProjection::setConstraintDirections()
{
// set Dirichlet constraint
size_t counter = 0;
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
@@ -116,7 +133,6 @@ void btContactProjection::setConstraintDirections()
c.push_back(Constraint(btVector3(0,0,1)));
m_constraints[&(psb->m_nodes[j])] = c;
}
++counter;
}
}
@@ -181,6 +197,7 @@ void btContactProjection::setConstraintDirections()
btAlignedObjectArray<Constraint> constraints;
constraints.push_back(Constraint(c));
m_constraints[c.m_node] = constraints;
m_frictions[c.m_node] = Friction();
}
else
{
@@ -193,11 +210,10 @@ void btContactProjection::setConstraintDirections()
}
// for particles with more than three constrained directions, prune constrained directions so that there are at most three constrained directions
counter = 0;
const int dim = 3;
for (auto it : m_constraints)
for (auto& it : m_constraints)
{
const btAlignedObjectArray<Constraint>& c = it.second;
btAlignedObjectArray<Constraint>& c = it.second;
if (c.size() > dim)
{
btAlignedObjectArray<Constraint> prunedConstraints;
@@ -216,7 +232,7 @@ void btContactProjection::setConstraintDirections()
min_dotProductAbs = dotProductAbs;
}
}
if (std::abs(min_dotProductAbs-1) < SIMD_EPSILON)
if (std::abs(std::abs(min_dotProductAbs)-1) < SIMD_EPSILON)
{
it.second = prunedConstraints;
continue;
@@ -240,5 +256,23 @@ void btContactProjection::setConstraintDirections()
prunedConstraints.push_back(c[selected2]);
it.second = prunedConstraints;
}
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);
}
}
}
}

14
src/BulletSoftBody/btContactProjection.h
View File

@@ -30,18 +30,22 @@ public:
virtual void operator()(TVStack& x)
{
const int dim = 3;
for (auto it : m_constraints)
for (auto& it : m_constraints)
{
const btAlignedObjectArray<Constraint>& constraints = it.second;
size_t i = m_indices[it.first];
const Friction& friction = m_frictions[it.first];
btAssert(constraints.size() <= dim);
btAssert(constraints.size() > 0);
if (constraints.size() == 1)
{
x[i] -= x[i].dot(constraints[0].m_direction) * constraints[0].m_direction;
if (friction.m_direction.norm() > SIMD_EPSILON)
x[i] -= x[i].dot(friction.m_direction) * friction.m_direction;
}
else if (constraints.size() == 2)
{
// 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;
@@ -55,16 +59,22 @@ public:
virtual void enforceConstraint(TVStack& x)
{
const int dim = 3;
for (auto it : m_constraints)
for (auto& it : m_constraints)
{
const btAlignedObjectArray<Constraint>& constraints = it.second;
size_t i = m_indices[it.first];
const Friction& friction = m_frictions[it.first];
btAssert(constraints.size() <= dim);
btAssert(constraints.size() > 0);
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;
if (friction.m_direction.norm() > SIMD_EPSILON)
{
x[i] -= x[i].dot(friction.m_direction) * friction.m_direction;
x[i] += friction.m_dv;
}
}
else if (constraints.size() == 2)
{

1
src/BulletSoftBody/btDeformableBodySolver.cpp
View File

@@ -122,6 +122,7 @@ void btDeformableBodySolver::solveConstraints(float solverdt)
updateVelocity();
}
advect(solverdt);
// postStabilize();
}
void btDeformableBodySolver::reinitialize(bool nodeUpdated)

3
src/BulletSoftBody/btSoftBodyInternals.h
View File

@@ -891,7 +891,8 @@ struct btSoftColliders
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 = 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)