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

code clean up + check in examples

Browse Source
This commit is contained in:
Xuchen Han
2019-07-18 11:34:06 -07:00
parent 3430192db7
commit dc10336d45
18 changed files with 1552 additions and 412 deletions
Download Patch File Download Diff File Expand all files Collapse all files

47
src/BulletSoftBody/btBackwardEulerObjective.cpp
View File

@@ -77,6 +77,51 @@ void btBackwardEulerObjective::updateVelocity(const TVStack& dv)
}
}
void btBackwardEulerObjective::applyForce(TVStack& force, bool setZero)
{
size_t 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)
{
btScalar one_over_mass = (psb->m_nodes[j].m_im == 0) ? 0 : psb->m_nodes[j].m_im;
psb->m_nodes[j].m_v += one_over_mass * force[counter++];
}
}
if (setZero)
{
for (int i = 0; i < force.size(); ++i)
force[i].setZero();
}
}
void btBackwardEulerObjective::computeResidual(btScalar dt, TVStack &residual) const
{
// add implicit force
for (int i = 0; i < m_lf.size(); ++i)
{
m_lf[i]->addScaledImplicitForce(dt, residual);
}
}
btScalar btBackwardEulerObjective::computeNorm(const TVStack& residual) const
{
btScalar norm_squared = 0;
for (int i = 0; i < residual.size(); ++i)
{
norm_squared += residual[i].length2();
}
return std::sqrt(norm_squared+SIMD_EPSILON);
}
void btBackwardEulerObjective::applyExplicitForce(TVStack& force)
{
for (int i = 0; i < m_lf.size(); ++i)
m_lf[i]->addScaledExplicitForce(m_dt, force);
applyForce(force, true);
}
void btBackwardEulerObjective::initialGuess(TVStack& dv, const TVStack& residual)
{
size_t counter = 0;
@@ -85,7 +130,7 @@ void btBackwardEulerObjective::initialGuess(TVStack& dv, const TVStack& residual
btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
dv[counter] = psb->m_nodes[j].m_im * residual[counter] * 1;
dv[counter] = psb->m_nodes[j].m_im * residual[counter];
++counter;
}
}

118
src/BulletSoftBody/btBackwardEulerObjective.h
View File

@@ -12,66 +12,10 @@
#include "btLagrangianForce.h"
#include "btMassSpring.h"
#include "btContactProjection.h"
#include "btPreconditioner.h"
#include "btDeformableRigidDynamicsWorld.h"
class btDeformableRigidDynamicsWorld;
class Preconditioner
{
public:
using TVStack = btAlignedObjectArray<btVector3>;
virtual void operator()(const TVStack& x, TVStack& b) = 0;
virtual void reinitialize(bool nodeUpdated) = 0;
};
class DefaultPreconditioner : public Preconditioner
{
public:
virtual void operator()(const TVStack& x, TVStack& b)
{
btAssert(b.size() == x.size());
for (int i = 0; i < b.size(); ++i)
b[i] = x[i];
}
virtual void reinitialize(bool nodeUpdated)
{
}
};
class MassPreconditioner : public Preconditioner
{
btAlignedObjectArray<btScalar> m_inv_mass;
const btAlignedObjectArray<btSoftBody *>& m_softBodies;
public:
MassPreconditioner(const btAlignedObjectArray<btSoftBody *>& softBodies)
: m_softBodies(softBodies)
{
}
virtual void reinitialize(bool nodeUpdated)
{
if (nodeUpdated)
{
m_inv_mass.clear();
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_nodes.size(); ++j)
m_inv_mass.push_back(psb->m_nodes[j].m_im);
}
}
}
virtual void operator()(const TVStack& x, TVStack& b)
{
btAssert(b.size() == x.size());
btAssert(m_inv_mass.size() == x.size());
for (int i = 0; i < b.size(); ++i)
b[i] = x[i] * m_inv_mass[i];
}
};
class btBackwardEulerObjective
{
public:
@@ -91,74 +35,60 @@ public:
void initialize(){}
void computeResidual(btScalar dt, TVStack& residual) const
{
// add implicit force
for (int i = 0; i < m_lf.size(); ++i)
{
m_lf[i]->addScaledImplicitForce(dt, residual);
}
}
// compute the rhs for CG solve, i.e, add the dt scaled implicit force to residual
void computeResidual(btScalar dt, TVStack& residual) const;
void applyExplicitForce(TVStack& force)
{
for (int i = 0; i < m_lf.size(); ++i)
m_lf[i]->addScaledExplicitForce(m_dt, force);
size_t 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)
{
btScalar one_over_mass = (psb->m_nodes[j].m_im == 0) ? 0 : psb->m_nodes[j].m_im;
psb->m_nodes[j].m_v += one_over_mass * force[counter];
force[counter].setZero();
counter++;
}
}
}
// add explicit force to the velocity
void applyExplicitForce(TVStack& force);
btScalar computeNorm(const TVStack& residual) const
{
btScalar norm_squared = 0;
for (int i = 0; i < residual.size(); ++i)
{
norm_squared += residual[i].length2();
}
return std::sqrt(norm_squared+SIMD_EPSILON);
}
// apply force to velocity and optionally reset the force to zero
void applyForce(TVStack& force, bool setZero);
// compute the norm of the residual
btScalar computeNorm(const TVStack& residual) const;
// compute one step of the solve (there is only one solve if the system is linear)
void computeStep(TVStack& dv, const TVStack& residual, const btScalar& dt);
// perform A*x = b
void multiply(const TVStack& x, TVStack& b) const;
// update the constraints treated as projections
void updateProjection(const TVStack& dv)
{
projection.update(dv, m_backupVelocity);
}
// set initial guess for CG solve
void initialGuess(TVStack& dv, const TVStack& residual);
// reset data structure
void reinitialize(bool nodeUpdated);
// enforce constraints in CG solve
void enforceConstraint(TVStack& x)
{
projection.enforceConstraint(x);
updateVelocity(x);
}
// add dv to velocity
void updateVelocity(const TVStack& dv);
void setConstraintDirections()
//set constraints as projections
void setConstraints()
{
projection.setConstraintDirections();
projection.setConstraints();
}
// update the projections and project the residual
void project(TVStack& r, const TVStack& dv)
{
updateProjection(dv);
projection(r);
}
// perform precondition M^(-1) x = b
void precondition(const TVStack& x, TVStack& b)
{
m_preconditioner->operator()(x,b);

49
src/BulletSoftBody/btCGProjection.h
View File

@@ -17,12 +17,12 @@ struct Constraint
btAlignedObjectArray<const btSoftBody::RContact*> m_contact;
btAlignedObjectArray<btVector3> m_direction;
btAlignedObjectArray<btScalar> m_value;
// the magnitude of the total impulse the node applied to the rb in the normal direction in the cg solve
btAlignedObjectArray<btScalar> m_accumulated_normal_impulse;
Constraint(const btSoftBody::RContact& rcontact)
{
m_contact.push_back(&rcontact);
m_direction.push_back(rcontact.m_cti.m_normal);
m_value.push_back(0);
append(rcontact);
}
Constraint(const btVector3 dir)
@@ -30,30 +30,56 @@ struct Constraint
m_contact.push_back(nullptr);
m_direction.push_back(dir);
m_value.push_back(0);
m_accumulated_normal_impulse.push_back(0);
}
Constraint()
{
m_contact.push_back(nullptr);
m_direction.push_back(btVector3(0,0,0));
m_value.push_back(0);
m_accumulated_normal_impulse.push_back(0);
}
void append(const btSoftBody::RContact& rcontact)
{
m_contact.push_back(&rcontact);
m_direction.push_back(rcontact.m_cti.m_normal);
m_value.push_back(0);
m_accumulated_normal_impulse.push_back(0);
}
~Constraint()
{
}
};
struct Friction
{
btAlignedObjectArray<bool> m_static;
btAlignedObjectArray<btScalar> m_impulse;
btAlignedObjectArray<btScalar> m_dv;
btAlignedObjectArray<btVector3> m_direction;
btAlignedObjectArray<btVector3> m_direction_prev;
btAlignedObjectArray<bool> m_static; // whether the friction is static
btAlignedObjectArray<btScalar> m_impulse; // the impulse magnitude the node feels
btAlignedObjectArray<btScalar> m_dv; // the dv magnitude of the node
btAlignedObjectArray<btVector3> m_direction; // the direction of the friction for the node
btAlignedObjectArray<bool> m_static_prev;
btAlignedObjectArray<btScalar> m_impulse_prev;
btAlignedObjectArray<btScalar> m_dv_prev;
btAlignedObjectArray<btVector3> m_direction_prev;
btAlignedObjectArray<bool> m_released;
btAlignedObjectArray<bool> m_released; // whether the contact is released
btAlignedObjectArray<btScalar> m_accumulated_normal_impulse;
// the total impulse the node applied to the rb in the tangential direction in the cg solve
btAlignedObjectArray<btVector3> m_accumulated_tangent_impulse;
Friction()
{
append();
}
void append()
{
m_static.push_back(false);
m_static_prev.push_back(false);
@@ -67,7 +93,6 @@ struct Friction
m_dv.push_back(0);
m_dv_prev.push_back(0);
m_accumulated_normal_impulse.push_back(0);
m_accumulated_tangent_impulse.push_back(btVector3(0,0,0));
m_released.push_back(false);
}
@@ -100,7 +125,7 @@ public:
// apply the constraints
virtual void operator()(TVStack& x) = 0;
virtual void setConstraintDirections() = 0;
virtual void setConstraints() = 0;
// update the constraints
virtual void update(const TVStack& dv, const TVStack& backup_v) = 0;

8
src/BulletSoftBody/btConjugateGradient.h
View File

@@ -18,12 +18,10 @@ class btConjugateGradient
using TVStack = btAlignedObjectArray<btVector3>;
TVStack r,p,z,temp;
int max_iterations;
btScalar tolerance;
public:
btConjugateGradient(const int max_it_in)
: max_iterations(max_it_in)
, tolerance(std::numeric_limits<float>::epsilon() * 16)
{
}
@@ -121,12 +119,6 @@ public:
return ans;
}
void setZero(TVStack& a)
{
for (int i = 0; i < a.size(); ++i)
a[i].setZero();
}
void multAndAddTo(btScalar s, const TVStack& a, TVStack& result)
{
// result += s*a

201
src/BulletSoftBody/btContactProjection.cpp
View File

@@ -61,9 +61,9 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit
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)
for (int k = 0; k < ndof; ++k)
{
vel += multibodyLinkCol->m_multiBody->getVelocityVector()[j] * jac[j];
vel += multibodyLinkCol->m_multiBody->getVelocityVector()[k] * jac[k];
}
va = cti.m_normal * vel * m_dt;
}
@@ -75,41 +75,41 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit
btVector3 impulse = c->m_c0 * vr;
const btVector3 impulse_normal = c->m_c0 * (cti.m_normal * dn);
const btVector3 impulse_tangent = impulse - impulse_normal;
// start friction handling
// copy old data
friction.m_impulse_prev[j] = friction.m_impulse[j];
friction.m_dv_prev[j] = friction.m_dv[j];
friction.m_static_prev[j] = friction.m_static[j];
// get the current tangent direction
btScalar local_tangent_norm = impulse_tangent.norm();
btVector3 local_tangent_dir = btVector3(0,0,0);
if (local_tangent_norm > SIMD_EPSILON)
local_tangent_dir = impulse_tangent.normalized();
// accumulated impulse on the rb in this and all prev cg iterations
friction.m_accumulated_normal_impulse[j] += impulse_normal.dot(cti.m_normal);
btScalar accumulated_normal = friction.m_accumulated_normal_impulse[j];
constraint.m_accumulated_normal_impulse[j] += impulse_normal.dot(cti.m_normal);
const btScalar& accumulated_normal = constraint.m_accumulated_normal_impulse[j];
// the total tangential impulse required to stop sliding
btVector3 tangent = friction.m_accumulated_tangent_impulse[j] + impulse_tangent;
btScalar tangent_norm = tangent.norm();
// start friction handling
// copy old data
friction.m_impulse_prev[j] = friction.m_impulse[j];
friction.m_dv_prev[j] = friction.m_dv[j];
friction.m_static_prev[j] = friction.m_static[j];
if (accumulated_normal < 0 && tangent_norm > SIMD_EPSILON)
if (accumulated_normal < 0)
{
friction.m_direction[j] = -local_tangent_dir;
btScalar compare1 = -accumulated_normal*c->m_c3;
btScalar compare2 = tangent_norm;
// do not allow switching from static friction to dynamic friction
// it causes cg to explode
if (-accumulated_normal*c->m_c3 < tangent_norm && friction.m_static_prev[j] == false && friction.m_released[j] == false)
{
friction.m_static[j] = false;
friction.m_impulse[j] = -accumulated_normal*c->m_c3;
friction.m_dv[j] = 0;
impulse = impulse_normal + (friction.m_impulse_prev[j] * friction.m_direction_prev[j])-(friction.m_impulse[j] * friction.m_direction[j]);
}
else
{
friction.m_static[j] = true;
friction.m_impulse[j] = 0;
friction.m_dv[j] = local_tangent_norm * c->m_c2/m_dt;
impulse = impulse_normal + impulse_tangent;
friction.m_impulse[j] = local_tangent_norm;
}
}
else
@@ -117,27 +117,29 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit
friction.m_released[j] = true;
friction.m_static[j] = false;
friction.m_impulse[j] = 0;
friction.m_dv[j] = 0;
friction.m_direction[j] = btVector3(0,0,0);
impulse = impulse_normal;
}
friction.m_dv[j] = friction.m_impulse[j] * c->m_c2/m_dt;
friction.m_accumulated_tangent_impulse[j] = -friction.m_impulse[j] * friction.m_direction[j];
// the incremental impulse applied to rb in the tangential direction
btVector3 incremental_tangent = (friction.m_impulse_prev[j] * friction.m_direction_prev[j])-(friction.m_impulse[j] * friction.m_direction[j]);
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
// 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);
// btVector3 dv = -impulse_normal * c->m_c2/m_dt + c->m_node->m_v - backupVelocity[m_indices[c->m_node]];
// btScalar dvn = dv.dot(cti.m_normal);
btScalar dvn = -accumulated_normal * c->m_c2/m_dt;
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_impulse_prev[j] * friction.m_direction_prev[j])-(friction.m_impulse[j] * friction.m_direction[j]);
// impulse = impulse_normal;
impulse = impulse_normal + incremental_tangent;
if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
{
if (rigidCol)
@@ -147,7 +149,7 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit
{
if (multibodyLinkCol)
{
double multiplier = 1;
double multiplier = 0.5;
multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV, -impulse.length() * multiplier);
}
}
@@ -158,8 +160,7 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit
}
}
void btContactProjection::setConstraintDirections()
void btContactProjection::setConstraints()
{
// set Dirichlet constraint
for (int i = 0; i < m_softBodies.size(); ++i)
@@ -188,6 +189,7 @@ void btContactProjection::setConstraintDirections()
{
btSoftBody* psb = m_softBodies[i];
btMultiBodyJacobianData jacobianData;
btAlignedObjectArray<btScalar> jacobian;
for (int j = 0; j < psb->m_rcontacts.size(); ++j)
{
@@ -214,6 +216,7 @@ void btContactProjection::setConstraintDirections()
}
else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
{
jacobian.clear();
multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
if (multibodyLinkCol)
{
@@ -227,9 +230,11 @@ void btContactProjection::setConstraintDirections()
multibodyLinkCol->m_multiBody->calcAccelerationDeltasMultiDof(&jacobianData.m_jacobians[0], deltaV, jacobianData.scratch_r, jacobianData.scratch_v);
btScalar vel = 0.0;
jacobian.resize(ndof);
for (int j = 0; j < ndof; ++j)
{
vel += multibodyLinkCol->m_multiBody->getVelocityVector()[j] * jac[j];
jacobian[j] = jac[j];
}
va = cti.m_normal * vel * m_dt;
}
@@ -262,22 +267,10 @@ void btContactProjection::setConstraintDirections()
btScalar dot_prod = dirs[0].dot(cti.m_normal);
if (std::abs(std::abs(dot_prod) - 1) < angle_epsilon)
{
constraints[j].m_contact.push_back(&c);
constraints[j].m_direction.push_back(cti.m_normal);
constraints[j].m_value.push_back(0);
// group the constraints
constraints[j].append(c);
// 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_impulse.push_back(0);
frictions[j].m_impulse_prev.push_back(0);
frictions[j].m_dv.push_back(0);
frictions[j].m_dv_prev.push_back(0);
frictions[j].m_static.push_back(false);
frictions[j].m_static_prev.push_back(false);
frictions[j].m_accumulated_normal_impulse.push_back(0);
frictions[j].m_accumulated_tangent_impulse.push_back(btVector3(0,0,0));
frictions[j].m_released.push_back(false);
frictions[j].append();
merged = true;
break;
}
@@ -295,3 +288,127 @@ void btContactProjection::setConstraintDirections()
}
}
}
void btContactProjection::enforceConstraint(TVStack& x)
{
const int dim = 3;
for (auto& it : m_constraints)
{
const btAlignedObjectArray<Constraint>& constraints = it.second;
size_t i = m_indices[it.first];
const 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];
for (int j = 0; j < constraints[0].m_direction.size(); ++j)
x[i] += constraints[0].m_value[j] * constraints[0].m_direction[j];
}
else if (constraints.size() == 2)
{
btVector3 free_dir = btCross(constraints[0].m_direction[0], constraints[1].m_direction[0]);
btAssert(free_dir.norm() > SIMD_EPSILON)
free_dir.normalize();
x[i] = x[i].dot(free_dir) * free_dir;
for (int j = 0; j < constraints.size(); ++j)
{
for (int k = 0; k < constraints[j].m_direction.size(); ++k)
{
x[i] += constraints[j].m_value[k] * constraints[j].m_direction[k];
}
}
}
else
{
x[i].setZero();
for (int j = 0; j < constraints.size(); ++j)
{
for (int k = 0; k < constraints[j].m_direction.size(); ++k)
{
x[i] += constraints[j].m_value[k] * constraints[j].m_direction[k];
}
}
}
// apply friction if the node is not constrained in all directions
if (constraints.size() < 3)
{
for (int f = 0; f < frictions.size(); ++f)
{
const Friction& friction= frictions[f];
for (int j = 0; j < friction.m_direction.size(); ++j)
{
// clear the old constraint
if (friction.m_static_prev[j] == true)
{
x[i] -= friction.m_direction_prev[j] * friction.m_dv_prev[j];
}
// add the new constraint
if (friction.m_static[j] == true)
{
x[i] += friction.m_direction[j] * friction.m_dv[j];
}
}
}
}
}
}
void btContactProjection::operator()(TVStack& x)
{
const int dim = 3;
for (auto& it : m_constraints)
{
const btAlignedObjectArray<Constraint>& constraints = it.second;
size_t i = m_indices[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];
}
else if (constraints.size() == 2)
{
btVector3 free_dir = btCross(constraints[0].m_direction[0], constraints[1].m_direction[0]);
btAssert(free_dir.norm() > SIMD_EPSILON)
free_dir.normalize();
x[i] = x[i].dot(free_dir) * free_dir;
}
else
x[i].setZero();
// apply friction if the node is not constrained in all directions
if (constraints.size() < 3)
{
bool has_static_constraint = false;
for (int f = 0; f < frictions.size(); ++f)
{
Friction& friction= frictions[f];
for (int j = 0; j < friction.m_static.size(); ++j)
has_static_constraint = has_static_constraint || friction.m_static[j];
}
for (int f = 0; f < frictions.size(); ++f)
{
Friction& friction= frictions[f];
for (int j = 0; j < friction.m_direction.size(); ++j)
{
// clear the old friction force
if (friction.m_static_prev[j] == false)
{
x[i] -= friction.m_direction_prev[j] * friction.m_impulse_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_impulse[j];
}
}
}
}
}
}

160
src/BulletSoftBody/btContactProjection.h
View File

@@ -18,171 +18,21 @@ public:
btContactProjection(btAlignedObjectArray<btSoftBody *>& softBodies, const btScalar& dt)
: btCGProjection(softBodies, dt)
{
}
virtual ~btContactProjection()
{
}
// apply the constraints
virtual void operator()(TVStack& x)
{
const int dim = 3;
for (auto& it : m_constraints)
{
const btAlignedObjectArray<Constraint>& constraints = it.second;
size_t i = m_indices[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];
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)
{
// clear the old friction force
if (friction.m_static_prev[j] == false)
{
x[i] -= friction.m_direction_prev[j] * friction.m_impulse_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_impulse[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();
x[i] = x[i].dot(free_dir) * free_dir;
bool has_static_constraint = false;
for (int f = 0; f < 2; ++f)
{
Friction& friction= frictions[f];
for (int j = 0; j < friction.m_static.size(); ++j)
has_static_constraint = has_static_constraint || friction.m_static[j];
}
for (int f = 0; f < 2; ++f)
{
Friction& friction= frictions[f];
for (int j = 0; j < friction.m_direction.size(); ++j)
{
// clear the old friction force
if (friction.m_static_prev[j] == false)
{
x[i] -= friction.m_direction_prev[j] * friction.m_impulse_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_impulse[j];
}
}
}
}
else
x[i].setZero();
}
}
// apply the constraints to the rhs
virtual void operator()(TVStack& x);
virtual void enforceConstraint(TVStack& x)
{
const int dim = 3;
for (auto& it : m_constraints)
{
const btAlignedObjectArray<Constraint>& constraints = it.second;
size_t i = m_indices[it.first];
const 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];
for (int j = 0; j < constraints[0].m_direction.size(); ++j)
x[i] += constraints[0].m_value[j] * constraints[0].m_direction[j];
const Friction& friction= frictions[0];
for (int j = 0; j < friction.m_direction.size(); ++j)
{
// clear the old constraint
if (friction.m_static_prev[j] == true)
{
// x[i] -= friction.m_direction_prev[j] * friction.m_dv_prev[j];
}
// add the new constraint
if (friction.m_static[j] == true)
{
x[i] += friction.m_direction[j] * friction.m_dv[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();
x[i] = x[i].dot(free_dir) * free_dir;
for (int j = 0; j < constraints.size(); ++j)
{
for (int k = 0; k < constraints[j].m_direction.size(); ++k)
{
x[i] += constraints[j].m_value[k] * constraints[j].m_direction[k];
}
}
for (int f = 0; f < 2; ++f)
{
const Friction& friction= frictions[f];
for (int j = 0; j < friction.m_direction.size(); ++j)
{
// clear the old constraint
if (friction.m_static_prev[j] == true)
{
// x[i] -= friction.m_direction_prev[j] * friction.m_dv_prev[j];
}
// add the new constraint
if (friction.m_static[j] == true)
{
x[i] += friction.m_direction[j] * friction.m_dv[j];
}
}
}
}
else
{
x[i].setZero();
for (int j = 0; j < constraints.size(); ++j)
{
for (int k = 0; k < constraints[j].m_direction.size(); ++k)
{
x[i] += constraints[j].m_value[k] * constraints[j].m_direction[k];
}
}
}
}
}
// apply constraints to x in Ax=b
virtual void enforceConstraint(TVStack& x);
// update the constraints
virtual void update(const TVStack& dv, const TVStack& backupVelocity);
virtual void setConstraintDirections();
virtual void setConstraints();
};
#endif /* btContactProjection_h */

78
src/BulletSoftBody/btDeformableBodySolver.cpp
View File

@@ -122,14 +122,15 @@ void btDeformableBodySolver::solveConstraints(float solverdt)
// apply explicit force
m_objective->applyExplicitForce(m_residual);
// remove contact constraints with separating velocity
setConstraintDirections();
// add constraints to the solver
setConstraints();
backupVelocity();
for (int i = 0; i < m_solveIterations; ++i)
{
m_objective->computeResidual(solverdt, m_residual);
m_objective->initialGuess(m_dv, m_residual);
m_objective->computeStep(m_dv, m_residual, solverdt);
updateVelocity();
}
@@ -153,9 +154,9 @@ void btDeformableBodySolver::reinitialize(bool nodeUpdated)
m_objective->reinitialize(nodeUpdated);
}
void btDeformableBodySolver::setConstraintDirections()
void btDeformableBodySolver::setConstraints()
{
m_objective->setConstraintDirections();
m_objective->setConstraints();
}
void btDeformableBodySolver::setWorld(btDeformableRigidDynamicsWorld* world)
@@ -173,9 +174,76 @@ void btDeformableBodySolver::updateVelocity()
btSoftBody* psb = m_softBodySet[i];
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
// psb->m_nodes[j].m_v += m_dv[counter];
psb->m_nodes[j].m_v = m_backupVelocity[counter]+m_dv[counter];
++counter;
}
}
}
void btDeformableBodySolver::advect(btScalar dt)
{
for (int i = 0; i < m_softBodySet.size(); ++i)
{
btSoftBody* psb = m_softBodySet[i];
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
auto& node = psb->m_nodes[j];
node.m_x = node.m_q + dt * node.m_v;
}
}
}
void btDeformableBodySolver::backupVelocity()
{
// serial implementation
int counter = 0;
for (int i = 0; i < m_softBodySet.size(); ++i)
{
btSoftBody* psb = m_softBodySet[i];
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
m_backupVelocity[counter++] = psb->m_nodes[j].m_v;
}
}
}
bool btDeformableBodySolver::updateNodes()
{
int numNodes = 0;
for (int i = 0; i < m_softBodySet.size(); ++i)
numNodes += m_softBodySet[i]->m_nodes.size();
if (numNodes != m_numNodes)
{
m_numNodes = numNodes;
m_backupVelocity.resize(numNodes);
return true;
}
return false;
}
void btDeformableBodySolver::predictMotion(float solverdt)
{
for (int i = 0; i < m_softBodySet.size(); ++i)
{
btSoftBody *psb = m_softBodySet[i];
if (psb->isActive())
{
psb->predictMotion(solverdt);
}
}
}
void btDeformableBodySolver::updateSoftBodies()
{
for (int i = 0; i < m_softBodySet.size(); i++)
{
btSoftBody *psb = (btSoftBody *)m_softBodySet[i];
if (psb->isActive())
{
psb->integrateMotion(); // normal is updated here
}
}
}

69
src/BulletSoftBody/btDeformableBodySolver.h
View File

@@ -51,17 +51,7 @@ public:
return true;
}
virtual void updateSoftBodies()
{
for (int i = 0; i < m_softBodySet.size(); i++)
{
btSoftBody *psb = (btSoftBody *)m_softBodySet[i];
if (psb->isActive())
{
psb->integrateMotion(); // normal is updated here
}
}
}
virtual void updateSoftBodies();
virtual void optimize(btAlignedObjectArray<btSoftBody *> &softBodies, bool forceUpdate = false)
{
@@ -76,64 +66,17 @@ public:
void reinitialize(bool nodeUpdated);
void setConstraintDirections();
void setConstraints();
void advect(btScalar dt)
{
size_t counter = 0;
for (int i = 0; i < m_softBodySet.size(); ++i)
{
btSoftBody* psb = m_softBodySet[i];
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
auto& node = psb->m_nodes[j];
node.m_x = node.m_q + dt * node.m_v;
}
}
}
void advect(btScalar dt);
void backupVelocity()
{
// serial implementation
int counter = 0;
for (int i = 0; i < m_softBodySet.size(); ++i)
{
btSoftBody* psb = m_softBodySet[i];
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
m_backupVelocity[counter++] = psb->m_nodes[j].m_v;
}
}
}
void backupVelocity();
void updateVelocity();
bool updateNodes()
{
int numNodes = 0;
for (int i = 0; i < m_softBodySet.size(); ++i)
numNodes += m_softBodySet[i]->m_nodes.size();
if (numNodes != m_numNodes)
{
m_numNodes = numNodes;
m_backupVelocity.resize(numNodes);
return true;
}
return false;
}
bool updateNodes();
virtual void predictMotion(float solverdt)
{
for (int i = 0; i < m_softBodySet.size(); ++i)
{
btSoftBody *psb = m_softBodySet[i];
if (psb->isActive())
{
psb->predictMotion(solverdt);
}
}
}
virtual void predictMotion(float solverdt);
virtual void copySoftBodyToVertexBuffer(const btSoftBody *const softBody, btVertexBufferDescriptor *vertexBuffer) {}

67
src/BulletSoftBody/btPreconditioner.h Normal file
View File

@@ -0,0 +1,67 @@
//
// btPreconditioner.h
// BulletSoftBody
//
// Created by Xuchen Han on 7/18/19.
//
#ifndef BT_PRECONDITIONER_H
#define BT_PRECONDITIONER_H
class Preconditioner
{
public:
using TVStack = btAlignedObjectArray<btVector3>;
virtual void operator()(const TVStack& x, TVStack& b) = 0;
virtual void reinitialize(bool nodeUpdated) = 0;
};
class DefaultPreconditioner : public Preconditioner
{
public:
virtual void operator()(const TVStack& x, TVStack& b)
{
btAssert(b.size() == x.size());
for (int i = 0; i < b.size(); ++i)
b[i] = x[i];
}
virtual void reinitialize(bool nodeUpdated)
{
}
};
class MassPreconditioner : public Preconditioner
{
btAlignedObjectArray<btScalar> m_inv_mass;
const btAlignedObjectArray<btSoftBody *>& m_softBodies;
public:
MassPreconditioner(const btAlignedObjectArray<btSoftBody *>& softBodies)
: m_softBodies(softBodies)
{
}
virtual void reinitialize(bool nodeUpdated)
{
if (nodeUpdated)
{
m_inv_mass.clear();
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_nodes.size(); ++j)
m_inv_mass.push_back(psb->m_nodes[j].m_im);
}
}
}
virtual void operator()(const TVStack& x, TVStack& b)
{
btAssert(b.size() == x.size());
btAssert(m_inv_mass.size() == x.size());
for (int i = 0; i < b.size(); ++i)
b[i] = x[i] * m_inv_mass[i];
}
};
#endif /* BT_PRECONDITIONER_H */