AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-15 22:20:12 +00:00
Code Issues Releases Wiki Activity

bug fix in friction; accumulate friction impulses in cg; forbid switching from static to dynamic friction

Browse Source
This commit is contained in:
Xuchen Han 2019-07-16 15:28:33 -07:00
parent befab4eab6
commit 2fc376e8f5
5 changed files with 124 additions and 56 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
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)
, projection(m_softBodies, m_dt)
, m_backupVelocity(backup_v)
@ -79,20 +79,4 @@ void btBackwardEulerObjective::updateVelocity(const TVStack& dv)
it.first->m_v = m_backupVelocity[i] + dv[i];
}
}
// for (int i = 0; i < m_softBodies.size(); ++i)
// {
// int counter = 0;
// for (int i = 0; i < m_softBodies.size(); ++i)
// {
// btSoftBody* psb = m_softBodies[i];
// for (int j = 0; j < psb->m_nodes.size(); ++j)
// {
// // only the velocity of the constrained nodes needs to be updated during CG solve
// if (projection.m_constraints[&(psb->m_nodes[j])].size() > 0)
// psb->m_nodes[j].m_v = m_backupVelocity[counter] + dv[counter];
// ++counter;
// }
// }
// }

28
src/BulletSoftBody/btCGProjection.h
View File

@ -39,17 +39,27 @@ struct Constraint
struct Friction
{
bool m_static;
btScalar m_value;
btVector3 m_direction;
btAlignedObjectArray<bool> m_static;
btAlignedObjectArray<btScalar> m_value;
btAlignedObjectArray<btVector3> m_direction;
bool m_static_prev;
btScalar m_value_prev;
btVector3 m_direction_prev;
btAlignedObjectArray<bool> m_static_prev;
btAlignedObjectArray<btScalar> m_value_prev;
btAlignedObjectArray<btVector3> m_direction_prev;
btAlignedObjectArray<btVector3> m_accumulated_impulse;
Friction()
{
m_direction_prev.setZero();
m_direction.setZero();
m_static.push_back(false);
m_static_prev.push_back(false);
m_direction.push_back(btVector3(0,0,0));
m_direction_prev.push_back(btVector3(0,0,0));
m_value.push_back(0);
m_value_prev.push_back(0);
m_accumulated_impulse.push_back(btVector3(0,0,0));
}
};
@ -65,7 +75,7 @@ public:
std::unordered_map<btSoftBody::Node *, size_t> m_indices;
const btScalar& m_dt;
std::unordered_map<btSoftBody::Node *, btAlignedObjectArray<Constraint> > m_constraints;
std::unordered_map<btSoftBody::Node *, Friction > m_frictions;
std::unordered_map<btSoftBody::Node *, btAlignedObjectArray<Friction> > m_frictions;
btCGProjection(btAlignedObjectArray<btSoftBody *>& softBodies, const btScalar& dt)
: m_softBodies(softBodies)

2
src/BulletSoftBody/btConjugateGradient.h
View File

@ -63,11 +63,9 @@ public:
// r -= alpha * temp;
multAndAddTo(alpha, p, x);
multAndAddTo(-alpha, temp, r);
// zero out the dofs of r
A.project(r,x);
A.enforceConstraint(x);
r_norm = std::sqrt(squaredNorm(r));
if (r_norm < tolerance) {

89
src/BulletSoftBody/btContactProjection.cpp
View File

@ -17,11 +17,12 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit
// loop through constraints to set constrained values
for (auto& it : m_constraints)
{
Friction& friction = m_frictions[it.first];
btAlignedObjectArray<Friction>& frictions = m_frictions[it.first];
btAlignedObjectArray<Constraint>& constraints = it.second;
for (int i = 0; i < constraints.size(); ++i)
{
Constraint& constraint = constraints[i];
Friction& friction = frictions[i];
for (int j = 0; j < constraint.m_contact.size(); ++j)
{
if (constraint.m_contact[j] == nullptr)
@ -75,36 +76,64 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit
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*1000, 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]]);
}
impulse = impulse_normal + impulse_tangent;
// accumulated impulse on the rb in this and all prev cg iterations
friction.m_accumulated_impulse[j] += impulse;
btScalar accumulated_normal = friction.m_accumulated_impulse[j].dot(cti.m_normal);
btVector3 accumulated_tangent = friction.m_accumulated_impulse[j] - accumulated_normal * cti.m_normal;
// if (1) // in the same CG solve, the set of constraits doesn't change
if (dn <= SIMD_EPSILON)
// start friction handling
// copy old data
friction.m_direction_prev[j] = friction.m_direction[j];
friction.m_value_prev[j] = friction.m_value[j];
friction.m_static_prev[j] = friction.m_static[j];
if (accumulated_normal < 0 && accumulated_tangent.norm() > SIMD_EPSILON)
{
// do not allow switching from static friction to dynamic friction
// it causes cg to explode
if (-accumulated_normal*c->m_c3 < accumulated_tangent.norm() && friction.m_static_prev[j] == false)
{
friction.m_static[j] = false;
friction.m_value[j] = -accumulated_normal*c->m_c3;
}
else
{
friction.m_static[j] = true;
friction.m_value[j] = accumulated_tangent.norm();
}
const btVector3 tangent_dir = accumulated_tangent.normalized();
impulse_tangent = friction.m_value[j] * tangent_dir;
friction.m_direction[j] = -tangent_dir;
}
else
{
friction.m_static[j] = false;
friction.m_value[j] = 0;
impulse_tangent.setZero();
}
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
// TODO: only contact is considered here, add friction later
// 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);
constraint.m_value[j] = dvn;
// the incremental impulse:
// in the normal direction it's the normal component of "impulse"
// in the tangent direction it's the difference between the frictional impulse in the iteration and the previous iteration
impulse = impulse_normal + (friction.m_value_prev[j] * friction.m_direction_prev[j]) - (friction.m_value[j] * friction.m_direction[j]);
if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
{
if (rigidCol)
rigidCol->applyImpulse(impulse_normal, c->m_c1);
rigidCol->applyImpulse(impulse, c->m_c1);
}
else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
{
@ -137,6 +166,13 @@ void btContactProjection::setConstraintDirections()
c.push_back(Constraint(btVector3(0,1,0)));
c.push_back(Constraint(btVector3(0,0,1)));
m_constraints[&(psb->m_nodes[j])] = c;
btAlignedObjectArray<Friction> f;
f.push_back(Friction());
f.push_back(Friction());
f.push_back(Friction());
m_frictions[&(psb->m_nodes[j])] = f;
// no friction constraints for dirichlet
}
}
}
@ -202,7 +238,9 @@ void btContactProjection::setConstraintDirections()
btAlignedObjectArray<Constraint> constraints;
constraints.push_back(Constraint(c));
m_constraints[c.m_node] = constraints;
m_frictions[c.m_node] = Friction();
btAlignedObjectArray<Friction> frictions;
frictions.push_back(Friction());
m_frictions[c.m_node] = frictions;
}
else
{
@ -210,6 +248,7 @@ void btContactProjection::setConstraintDirections()
const btScalar angle_epsilon = 0.015192247; // less than 10 degree
bool merged = false;
btAlignedObjectArray<Constraint>& constraints = m_constraints[c.m_node];
btAlignedObjectArray<Friction>& frictions = m_frictions[c.m_node];
for (int j = 0; j < constraints.size(); ++j)
{
const btAlignedObjectArray<btVector3>& dirs = constraints[j].m_direction;
@ -219,6 +258,15 @@ void btContactProjection::setConstraintDirections()
constraints[j].m_contact.push_back(&c);
constraints[j].m_direction.push_back(cti.m_normal);
constraints[j].m_value.push_back(0);
// push in an empty friction
frictions[j].m_direction.push_back(btVector3(0,0,0));
frictions[j].m_direction_prev.push_back(btVector3(0,0,0));
frictions[j].m_value.push_back(0);
frictions[j].m_value_prev.push_back(0);
frictions[j].m_static.push_back(false);
frictions[j].m_static_prev.push_back(false);
frictions[j].m_accumulated_impulse.push_back(btVector3(0,0,0));
merged = true;
break;
}
@ -226,7 +274,10 @@ void btContactProjection::setConstraintDirections()
const int dim = 3;
// hard coded no more than 3 constraint directions
if (!merged && constraints.size() < dim)
{
constraints.push_back(Constraint(c));
frictions.push_back(Friction());
}
}
}
}

43
src/BulletSoftBody/btContactProjection.h
View File

@ -34,16 +34,31 @@ public:
{
const btAlignedObjectArray<Constraint>& constraints = it.second;
size_t i = m_indices[it.first];
const Friction& friction = m_frictions[it.first];
btAlignedObjectArray<Friction>& frictions = 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[0]) * constraints[0].m_direction[0];
if (friction.m_direction.norm() > SIMD_EPSILON)
Friction& friction= frictions[0];
bool has_static_constraint = false;
for (int j = 0; j < friction.m_static.size(); ++j)
has_static_constraint = has_static_constraint || friction.m_static[j];
for (int j = 0; j < friction.m_direction.size(); ++j)
{
btVector3 dir = friction.m_direction.normalized();
x[i] -= x[i].dot(dir) * dir;
// clear the old friction force
if (friction.m_static_prev[j] == false)
{
x[i] -= friction.m_direction_prev[j] * friction.m_value_prev[j];
}
// only add to the rhs if there is no static friction constraint on the node
if (friction.m_static[j] == false && !has_static_constraint)
{
x[i] += friction.m_direction[j] * friction.m_value[j];
}
}
}
else if (constraints.size() == 2)
@ -67,7 +82,7 @@ public:
{
const btAlignedObjectArray<Constraint>& constraints = it.second;
size_t i = m_indices[it.first];
const Friction& friction = m_frictions[it.first];
const btAlignedObjectArray<Friction>& frictions = m_frictions[it.first];
btAssert(constraints.size() <= dim);
btAssert(constraints.size() > 0);
if (constraints.size() == 1)
@ -75,15 +90,25 @@ public:
x[i] -= x[i].dot(constraints[0].m_direction[0]) * constraints[0].m_direction[0];
for (int j = 0; j < constraints[0].m_direction.size(); ++j)
x[i] += constraints[0].m_value[j] * constraints[0].m_direction[j];
if (friction.m_direction.norm() > SIMD_EPSILON)
const Friction& friction= frictions[0];
for (int j = 0; j < friction.m_direction.size(); ++j)
{
btVector3 dir = friction.m_direction.normalized();
x[i] -= x[i].dot(dir) * dir;
x[i] += friction.m_dv;
// clear the old constraint
if (friction.m_static_prev[j] == true)
{
x[i] -= friction.m_direction_prev[j] * friction.m_value_prev[j];
}
// add the new constraint
if (friction.m_static[j] == true)
{
x[i] += friction.m_direction[j] * friction.m_value[j];
}
}
}
else if (constraints.size() == 2)
{
// TODO: friction
btVector3 free_dir = btCross(constraints[0].m_direction[0], constraints[1].m_direction[0]);
btAssert(free_dir.norm() > SIMD_EPSILON)
free_dir.normalize();