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

add newton solver

Browse Source
This commit is contained in:
Xuchen Han
2019-08-27 20:54:40 -07:00
parent c722630fc7
commit d4a15e016e
12 changed files with 566 additions and 62 deletions
Download Patch File Download Diff File Expand all files Collapse all files

21
src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp
View File

@@ -20,6 +20,7 @@ btDeformableBackwardEulerObjective::btDeformableBackwardEulerObjective(btAligned
: m_softBodies(softBodies)
, projection(m_softBodies, m_dt)
, m_backupVelocity(backup_v)
, m_implicit(false)
{
m_preconditioner = new DefaultPreconditioner();
}
@@ -72,7 +73,11 @@ void btDeformableBackwardEulerObjective::multiply(const TVStack& x, TVStack& b)
for (int i = 0; i < m_lf.size(); ++i)
{
// add damping matrix
m_lf[i]->addScaledForceDifferential(-m_dt, x, b);
m_lf[i]->addScaledDampingForceDifferential(-m_dt, x, b);
if (m_implicit)
{
m_lf[i]->addScaledElasticForceDifferential(-m_dt*m_dt, x, b);
}
}
}
@@ -105,14 +110,22 @@ void btDeformableBackwardEulerObjective::applyForce(TVStack& force, bool setZero
}
}
void btDeformableBackwardEulerObjective::computeResidual(btScalar dt, TVStack &residual) const
void btDeformableBackwardEulerObjective::computeResidual(btScalar dt, TVStack &residual)
{
BT_PROFILE("computeResidual");
// add implicit force
for (int i = 0; i < m_lf.size(); ++i)
{
m_lf[i]->addScaledImplicitForce(dt, residual);
if (m_implicit)
{
m_lf[i]->addScaledForces(dt, residual);
}
else
{
m_lf[i]->addScaledDampingForce(dt, residual);
}
}
projection.project(residual);
}
btScalar btDeformableBackwardEulerObjective::computeNorm(const TVStack& residual) const
@@ -122,7 +135,7 @@ btScalar btDeformableBackwardEulerObjective::computeNorm(const TVStack& residual
{
norm_squared += residual[i].length2();
}
return std::sqrt(norm_squared+SIMD_EPSILON);
return std::sqrt(norm_squared);
}
void btDeformableBackwardEulerObjective::applyExplicitForce(TVStack& force)

9
src/BulletSoftBody/btDeformableBackwardEulerObjective.h
View File

@@ -37,6 +37,8 @@ public:
btDeformableContactProjection projection;
const TVStack& m_backupVelocity;
btAlignedObjectArray<btSoftBody::Node* > m_nodes;
bool m_implicit;
btDeformableBackwardEulerObjective(btAlignedObjectArray<btSoftBody *>& softBodies, const TVStack& backup_v);
virtual ~btDeformableBackwardEulerObjective();
@@ -44,7 +46,7 @@ public:
void initialize(){}
// compute the rhs for CG solve, i.e, add the dt scaled implicit force to residual
void computeResidual(btScalar dt, TVStack& residual) const;
void computeResidual(btScalar dt, TVStack& residual);
// add explicit force to the velocity
void applyExplicitForce(TVStack& force);
@@ -117,6 +119,11 @@ public:
{
return &m_nodes;
}
void setImplicit(bool implicit)
{
m_implicit = implicit;
}
};
#endif /* btBackwardEulerObjective_h */

89
src/BulletSoftBody/btDeformableBodySolver.cpp
View File

@@ -21,6 +21,8 @@
btDeformableBodySolver::btDeformableBodySolver()
: m_numNodes(0)
, m_cg(50)
, m_maxNewtonIterations(5)
, m_newtonTolerance(1e-10)
{
m_objective = new btDeformableBackwardEulerObjective(m_softBodySet, m_backupVelocity);
}
@@ -33,17 +35,70 @@ btDeformableBodySolver::~btDeformableBodySolver()
void btDeformableBodySolver::solveDeformableConstraints(btScalar solverdt)
{
BT_PROFILE("solveConstraints");
if (!m_implicit)
{
m_objective->computeResidual(solverdt, m_residual);
m_objective->applyDynamicFriction(m_residual);
computeStep(m_dv, m_residual);
updateVelocity();
}
void btDeformableBodySolver::computeStep(TVStack& dv, const TVStack& residual)
else
{
btScalar tolerance = std::numeric_limits<float>::epsilon() * 16 * m_objective->computeNorm(residual);
m_cg.solve(*m_objective, dv, residual, tolerance);
for (int i = 0; i < m_maxNewtonIterations; ++i)
{
updateState();
// add the inertia term in the residual
int counter = 0;
for (int k = 0; k < m_softBodySet.size(); ++k)
{
btSoftBody* psb = m_softBodySet[k];
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
if (psb->m_nodes[j].m_im > 0)
{
m_residual[counter] = (-1./psb->m_nodes[j].m_im) * m_dv[counter];
}
++counter;
}
}
m_objective->computeResidual(solverdt, m_residual);
if (m_objective->computeNorm(m_residual) < m_newtonTolerance)
{
break;
}
// m_objective->applyDynamicFriction(m_residual);
computeStep(m_ddv, m_residual);
updateDv();
for (int j = 0; j < m_numNodes; ++j)
{
m_ddv[j].setZero();
m_residual[j].setZero();
}
}
}
}
void btDeformableBodySolver::updateState()
{
updateVelocity();
updateTempPosition();
}
void btDeformableBodySolver::updateDv()
{
for (int i = 0; i < m_numNodes; ++i)
{
m_dv[i] += m_ddv[i];
}
}
void btDeformableBodySolver::computeStep(TVStack& ddv, const TVStack& residual)
{
// btScalar tolerance = std::numeric_limits<btScalar>::epsilon() * m_objective->computeNorm(residual);
btScalar tolerance = std::numeric_limits<btScalar>::epsilon();
m_cg.solve(*m_objective, ddv, residual, tolerance);
}
void btDeformableBodySolver::reinitialize(const btAlignedObjectArray<btSoftBody *>& softBodies, btScalar dt)
@@ -54,6 +109,7 @@ void btDeformableBodySolver::reinitialize(const btAlignedObjectArray<btSoftBody
if (nodeUpdated)
{
m_dv.resize(m_numNodes, btVector3(0,0,0));
m_ddv.resize(m_numNodes, btVector3(0,0,0));
m_residual.resize(m_numNodes, btVector3(0,0,0));
m_backupVelocity.resize(m_numNodes, btVector3(0,0,0));
}
@@ -62,9 +118,11 @@ void btDeformableBodySolver::reinitialize(const btAlignedObjectArray<btSoftBody
for (int i = 0; i < m_numNodes; ++i)
{
m_dv[i].setZero();
m_ddv[i].setZero();
m_residual[i].setZero();
}
m_dt = dt;
m_objective->reinitialize(nodeUpdated, dt);
}
@@ -103,6 +161,21 @@ void btDeformableBodySolver::updateVelocity()
}
}
void btDeformableBodySolver::updateTempPosition()
{
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)
{
psb->m_nodes[j].m_q = psb->m_nodes[j].m_x + m_dt * psb->m_nodes[j].m_v;
++counter;
}
psb->updateDeformation();
}
}
void btDeformableBodySolver::backupVelocity()
{
int counter = 0;
@@ -209,3 +282,9 @@ void btDeformableBodySolver::updateSoftBodies()
}
}
}
void btDeformableBodySolver::setImplicit(bool implicit)
{
m_implicit = implicit;
m_objective->setImplicit(implicit);
}

12
src/BulletSoftBody/btDeformableBodySolver.h
View File

@@ -34,6 +34,7 @@ class btDeformableBodySolver : public btSoftBodySolver
protected:
int m_numNodes;
TVStack m_dv;
TVStack m_ddv;
TVStack m_residual;
btAlignedObjectArray<btSoftBody *> m_softBodySet;
@@ -41,7 +42,9 @@ protected:
btScalar m_dt;
btScalar m_contact_iterations;
btConjugateGradient<btDeformableBackwardEulerObjective> m_cg;
bool m_implicit;
int m_maxNewtonIterations;
btScalar m_newtonTolerance;
public:
btDeformableBackwardEulerObjective* m_objective;
@@ -95,6 +98,13 @@ public:
virtual bool checkInitialized(){return true;}
void setImplicit(bool implicit);
void updateState();
void updateDv();
void updateTempPosition();
};
#endif /* btDeformableBodySolver_h */

9
src/BulletSoftBody/btDeformableCorotatedForce.h
View File

@@ -39,8 +39,9 @@ public:
{
}
virtual void addScaledImplicitForce(btScalar scale, TVStack& force)
virtual void addScaledForces(btScalar scale, TVStack& force)
{
addScaledElasticForce(scale, force);
}
virtual void addScaledExplicitForce(btScalar scale, TVStack& force)
@@ -105,7 +106,11 @@ public:
}
}
virtual void addScaledForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df)
{
}
virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
{
}

12
src/BulletSoftBody/btDeformableGravityForce.h
View File

@@ -28,8 +28,9 @@ public:
{
}
virtual void addScaledImplicitForce(btScalar scale, TVStack& force)
virtual void addScaledForces(btScalar scale, TVStack& force)
{
addScaledGravityForce(scale, force);
}
virtual void addScaledExplicitForce(btScalar scale, TVStack& force)
@@ -37,9 +38,16 @@ public:
addScaledGravityForce(scale, force);
}
virtual void addScaledForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
virtual void addScaledDampingForce(btScalar scale, TVStack& force)
{
}
virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df)
{
}
virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
{
}
virtual void addScaledGravityForce(btScalar scale, TVStack& force)

270
src/BulletSoftBody/btDeformableLagrangianForce.h
View File

@@ -18,6 +18,7 @@
#include "btSoftBody.h"
#include <LinearMath/btHashMap.h>
#include <iostream>
enum btDeformableLagrangianForceType
{
@@ -40,12 +41,21 @@ public:
virtual ~btDeformableLagrangianForce(){}
virtual void addScaledImplicitForce(btScalar scale, TVStack& force) = 0;
// add all forces
virtual void addScaledForces(btScalar scale, TVStack& force) = 0;
virtual void addScaledForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) = 0;
// add damping df
virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) = 0;
// add elastic df
virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) = 0;
// add all forces that are explicit in explicit solve
virtual void addScaledExplicitForce(btScalar scale, TVStack& force) = 0;
// add all damping forces
virtual void addScaledDampingForce(btScalar scale, TVStack& force) = 0;
virtual btDeformableLagrangianForceType getForceType() = 0;
virtual void reinitialize(bool nodeUpdated)
@@ -71,5 +81,261 @@ public:
{
m_nodes = nodes;
}
virtual btMatrix3x3 Ds(int id0, int id1, int id2, int id3, const TVStack& dx)
{
btVector3 c1 = dx[id1] - dx[id0];
btVector3 c2 = dx[id2] - dx[id0];
btVector3 c3 = dx[id3] - dx[id0];
btMatrix3x3 dF(c1.getX(), c2.getX(), c3.getX(),
c1.getY(), c2.getY(), c3.getY(),
c1.getZ(), c2.getZ(), c3.getZ());
return dF;
}
virtual void testDerivative()
{
for (int i = 0; i<m_softBodies.size();++i)
{
btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
psb->m_nodes[j].m_q += btVector3(randomDouble(-.1, .1), randomDouble(-.1, .1), randomDouble(-.1, .1));
}
psb->updateDeformation();
}
TVStack dx;
dx.resize(getNumNodes());
TVStack dphi_dx;
dphi_dx.resize(dx.size());
for (int i =0; i < dphi_dx.size();++i)
{
dphi_dx[i].setZero();
}
addScaledForces(-1, dphi_dx);
// write down the current position
TVStack x;
x.resize(dx.size());
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)
{
x[counter] = psb->m_nodes[j].m_q;
counter++;
}
}
counter = 0;
// populate dx with random vectors
for (int i = 0; i < dx.size(); ++i)
{
dx[i].setX(randomDouble(-1, 1));
dx[i].setY(randomDouble(-1, 1));
dx[i].setZ(randomDouble(-1, 1));
}
btAlignedObjectArray<double> errors;
double h = 1;
for (int it = 0; it < 10; ++it)
{
for (int i = 0; i < dx.size(); ++i)
{
dx[i] *= 0.5;
}
// get dphi/dx * dx
double dphi = 0;
for (int i = 0; i < dx.size(); ++i)
{
dphi += dphi_dx[i].dot(dx[i]);
}
for (int i = 0; i<m_softBodies.size();++i)
{
btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
psb->m_nodes[j].m_q = x[counter] + dx[counter];
counter++;
}
psb->updateDeformation();
}
counter = 0;
double f1 = totalElasticEnergy();
for (int i = 0; i<m_softBodies.size();++i)
{
btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
psb->m_nodes[j].m_q = x[counter] - dx[counter];
counter++;
}
psb->updateDeformation();
}
counter = 0;
double f2 = totalElasticEnergy();
//restore m_q
for (int i = 0; i<m_softBodies.size();++i)
{
btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
psb->m_nodes[j].m_q = x[counter];
counter++;
}
psb->updateDeformation();
}
counter = 0;
double error = f1-f2-2*dphi;
errors.push_back(error);
std::cout << "Iteration = " << it <<", f1 = " << f1 << ", f2 = " << f2 << ", error = " << error << std::endl;
}
for (int i = 1; i < errors.size(); ++i)
{
std::cout << "Iteration = " << i << ", ratio = " << errors[i-1]/errors[i] << std::endl;
}
}
virtual void testHessian()
{
for (int i = 0; i<m_softBodies.size();++i)
{
btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
psb->m_nodes[j].m_q += btVector3(randomDouble(-.1, .1), randomDouble(-.1, .1), randomDouble(-.1, .1));
}
psb->updateDeformation();
}
TVStack dx;
dx.resize(getNumNodes());
TVStack df;
df.resize(dx.size());
TVStack f1;
f1.resize(dx.size());
TVStack f2;
f2.resize(dx.size());
// write down the current position
TVStack x;
x.resize(dx.size());
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)
{
x[counter] = psb->m_nodes[j].m_q;
counter++;
}
}
counter = 0;
// populate dx with random vectors
for (int i = 0; i < dx.size(); ++i)
{
dx[i].setX(randomDouble(-1, 1));
dx[i].setY(randomDouble(-1, 1));
dx[i].setZ(randomDouble(-1, 1));
}
btAlignedObjectArray<double> errors;
for (int it = 0; it < 10; ++it)
{
for (int i = 0; i < dx.size(); ++i)
{
dx[i] *= 0.5;
}
// get df
for (int i =0; i < df.size();++i)
{
df[i].setZero();
f1[i].setZero();
f2[i].setZero();
}
//set df
addScaledElasticForceDifferential(-1, dx, df);
for (int i = 0; i<m_softBodies.size();++i)
{
btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
psb->m_nodes[j].m_q = x[counter] + dx[counter];
counter++;
}
psb->updateDeformation();
}
counter = 0;
//set f1
addScaledForces(-1, f1);
for (int i = 0; i<m_softBodies.size();++i)
{
btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
psb->m_nodes[j].m_q = x[counter] - dx[counter];
counter++;
}
psb->updateDeformation();
}
counter = 0;
//set f2
addScaledForces(-1, f2);
//restore m_q
for (int i = 0; i<m_softBodies.size();++i)
{
btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
psb->m_nodes[j].m_q = x[counter];
counter++;
}
psb->updateDeformation();
}
counter = 0;
double error = 0;
for (int i = 0; i < df.size();++i)
{
btVector3 error_vector = f1[i]-f2[i]-2*df[i];
error += error_vector.length2();
}
error = btSqrt(error);
errors.push_back(error);
std::cout << "Iteration = " << it << ", error = " << error << std::endl;
}
for (int i = 1; i < errors.size(); ++i)
{
std::cout << "Iteration = " << i << ", ratio = " << errors[i-1]/errors[i] << std::endl;
}
}
virtual double totalElasticEnergy()
{
return 0;
}
double randomDouble(double low, double high)
{
return low + static_cast<double>(rand()) / RAND_MAX * (high - low);
}
};
#endif /* BT_DEFORMABLE_LAGRANGIAN_FORCE */

43
src/BulletSoftBody/btDeformableMassSpringForce.h
View File

@@ -31,9 +31,10 @@ public:
{
}
virtual void addScaledImplicitForce(btScalar scale, TVStack& force)
virtual void addScaledForces(btScalar scale, TVStack& force)
{
addScaledDampingForce(scale, force);
addScaledElasticForce(scale, force);
}
virtual void addScaledExplicitForce(btScalar scale, TVStack& force)
@@ -61,9 +62,9 @@ public:
btVector3 scaled_force = scale * m_dampingStiffness * v_diff;
if (m_momentum_conserving)
{
if ((node2->m_x - node1->m_x).norm() > SIMD_EPSILON)
if ((node2->m_q - node1->m_q).norm() > SIMD_EPSILON)
{
btVector3 dir = (node2->m_x - node1->m_x).normalized();
btVector3 dir = (node2->m_q - node1->m_q).normalized();
scaled_force = scale * m_dampingStiffness * v_diff.dot(dir) * dir;
}
}
@@ -91,7 +92,7 @@ public:
// elastic force
// explicit elastic force
btVector3 dir = (node2->m_x - node1->m_x);
btVector3 dir = (node2->m_q - node1->m_q);
btVector3 dir_normalized = (dir.norm() > SIMD_EPSILON) ? dir.normalized() : btVector3(0,0,0);
btVector3 scaled_force = scale * m_elasticStiffness * (dir - dir_normalized * r);
force[id1] += scaled_force;
@@ -100,7 +101,7 @@ public:
}
}
virtual void addScaledForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
{
// implicit damping force differential
for (int i = 0; i < m_softBodies.size(); ++i)
@@ -118,9 +119,9 @@ public:
btVector3 local_scaled_df = scaled_k_damp * (dv[id2] - dv[id1]);
if (m_momentum_conserving)
{
if ((node2->m_x - node1->m_x).norm() > SIMD_EPSILON)
if ((node2->m_q - node1->m_q).norm() > SIMD_EPSILON)
{
btVector3 dir = (node2->m_x - node1->m_x).normalized();
btVector3 dir = (node2->m_q - node1->m_q).normalized();
local_scaled_df= scaled_k_damp * (dv[id2] - dv[id1]).dot(dir) * dir;
}
}
@@ -130,6 +131,34 @@ public:
}
}
virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df)
{
// implicit damping force differential
for (int i = 0; i < m_softBodies.size(); ++i)
{
const btSoftBody* psb = m_softBodies[i];
btScalar scaled_k_damp = m_dampingStiffness * scale;
for (int j = 0; j < psb->m_links.size(); ++j)
{
const btSoftBody::Link& link = psb->m_links[j];
btSoftBody::Node* node1 = link.m_n[0];
btSoftBody::Node* node2 = link.m_n[1];
size_t id1 = node1->index;
size_t id2 = node2->index;
btScalar r = link.m_rl;
// todo @xuchenhan: find df
btVector3 dir = (node2->m_q - node1->m_q);
btVector3 dir_normalized = (dir.norm() > SIMD_EPSILON) ? dir.normalized() : btVector3(0,0,0);
btVector3 scaled_force = scale * m_elasticStiffness * (dir - dir_normalized * r);
btVector3 local_scaled_df = btVector3(0,0,0);
df[id1] += local_scaled_df;
df[id2] -= local_scaled_df;
}
}
}
virtual btDeformableLagrangianForceType getForceType()
{
return BT_MASSSPRING_FORCE;

19
src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp
View File

@@ -167,19 +167,31 @@ void btDeformableMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
}
}
node.m_x = node.m_x + timeStep * node.m_v;
node.m_q = node.m_x;
}
}
m_deformableBodySolver->revertVelocity();
}
void btDeformableMultiBodyDynamicsWorld::solveConstraints(btScalar timeStep)
{
if (!m_implicit)
{
// save v_{n+1}^* velocity after explicit forces
m_deformableBodySolver->backupVelocity();
}
// set up constraints among multibodies and between multibodies and deformable bodies
setupConstraints();
solveMultiBodyRelatedConstraints();
if (m_implicit)
{
// revert to v_n after collisions are registered
m_deformableBodySolver->revertVelocity();
// todo @xuchenhan : think about whether contact solve should be done with v_n velocity or v_{n+1}^* velocity. It's using v_n for implicit and v_{n+1}^* for non-implicit now.
}
// At this point, dv is golden for nodes in contact
m_deformableBodySolver->solveDeformableConstraints(timeStep);
}
@@ -273,7 +285,6 @@ void btDeformableMultiBodyDynamicsWorld::solveMultiBodyRelatedConstraints()
}
}
// todo : may not be necessary
for (int i = 0; i < this->m_multiBodies.size(); i++)
{
btMultiBody* bod = m_multiBodies[i];
@@ -304,7 +315,13 @@ void btDeformableMultiBodyDynamicsWorld::predictUnconstraintMotion(btScalar time
void btDeformableMultiBodyDynamicsWorld::reinitialize(btScalar timeStep)
{
m_internalTime += timeStep;
m_deformableBodySolver->setImplicit(m_implicit);
m_deformableBodySolver->reinitialize(m_softBodies, timeStep);
if (m_implicit)
{
// backup v_n velocity
m_deformableBodySolver->backupVelocity();
}
btDispatcherInfo& dispatchInfo = btMultiBodyDynamicsWorld::getDispatchInfo();
dispatchInfo.m_timeStep = timeStep;
dispatchInfo.m_stepCount = 0;

9
src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h
View File

@@ -47,6 +47,7 @@ class btDeformableMultiBodyDynamicsWorld : public btMultiBodyDynamicsWorld
btSoftBodyWorldInfo m_sbi;
btScalar m_internalTime;
int m_contact_iterations;
bool m_implicit;
typedef void (*btSolverCallback)(btScalar time, btDeformableMultiBodyDynamicsWorld* world);
btSolverCallback m_solverCallback;
@@ -72,7 +73,7 @@ public:
m_sbi.m_broadphase = pairCache;
m_sbi.m_dispatcher = dispatcher;
m_sbi.m_sparsesdf.Initialize();
m_sbi.m_sparsesdf.setDefaultVoxelsz(0.025);
m_sbi.m_sparsesdf.setDefaultVoxelsz(0.0025);
m_sbi.m_sparsesdf.Reset();
m_sbi.air_density = (btScalar)1.2;
@@ -81,6 +82,7 @@ public:
m_sbi.water_normal = btVector3(0, 0, 0);
m_sbi.m_gravity.setValue(0, -10, 0);
m_internalTime = 0.0;
m_implicit = false;
}
void setSolverCallback(btSolverCallback cb)
@@ -153,6 +155,11 @@ public:
void solveMultiBodyRelatedConstraints();
void sortConstraints();
void setImplicit(bool implicit)
{
m_implicit = implicit;
}
};
#endif //BT_DEFORMABLE_RIGID_DYNAMICS_WORLD_H

113
src/BulletSoftBody/btDeformableNeoHookeanForce.h
View File

@@ -25,16 +25,16 @@ public:
btScalar m_mu, m_lambda;
btDeformableNeoHookeanForce(): m_mu(1), m_lambda(1)
{
}
btDeformableNeoHookeanForce(btScalar mu, btScalar lambda): m_mu(mu), m_lambda(lambda)
{
}
virtual void addScaledImplicitForce(btScalar scale, TVStack& force)
virtual void addScaledForces(btScalar scale, TVStack& force)
{
addScaledDampingForce(scale, force);
addScaledElasticForce(scale, force);
}
virtual void addScaledExplicitForce(btScalar scale, TVStack& force)
@@ -46,6 +46,34 @@ public:
{
}
virtual double totalElasticEnergy()
{
double energy = 0;
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_tetras.size(); ++j)
{
btSoftBody::Tetra& tetra = psb->m_tetras[j];
energy += tetra.m_element_measure * elasticEnergyDensity(tetra);
}
}
return energy;
}
double elasticEnergyDensity(const btSoftBody::Tetra& t)
{
double density = 0;
btMatrix3x3 F = t.m_F;
btMatrix3x3 C = F.transpose()*F;
double J = F.determinant();
double trace = C[0].getX() + C[1].getY() + C[2].getZ();
density += m_mu * 0.5 * (trace - 3.);
density += m_lambda * 0.5 * (J - 1. - 0.75 * m_mu / m_lambda)* (J - 1. - 0.75 * m_mu / m_lambda);
density -= m_mu * 0.5 * log(trace+1);
return density;
}
virtual void addScaledElasticForce(btScalar scale, TVStack& force)
{
int numNodes = getNumNodes();
@@ -72,7 +100,6 @@ public:
size_t id3 = node3->index;
// elastic force
// explicit elastic force
btScalar scale1 = scale * tetra.m_element_measure;
force[id0] -= scale1 * force_on_node0;
force[id1] -= scale1 * force_on_node123.getColumn(0);
@@ -82,25 +109,62 @@ public:
}
}
virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
{
}
virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df)
{
int numNodes = getNumNodes();
btAssert(numNodes <= df.size())
btVector3 grad_N_hat_1st_col = btVector3(-1,-1,-1);
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_tetras.size(); ++j)
{
btSoftBody::Tetra& tetra = psb->m_tetras[j];
btSoftBody::Node* node0 = tetra.m_n[0];
btSoftBody::Node* node1 = tetra.m_n[1];
btSoftBody::Node* node2 = tetra.m_n[2];
btSoftBody::Node* node3 = tetra.m_n[3];
size_t id0 = node0->index;
size_t id1 = node1->index;
size_t id2 = node2->index;
size_t id3 = node3->index;
btMatrix3x3 dF = Ds(id0, id1, id2, id3, dx) * tetra.m_Dm_inverse;
btMatrix3x3 dP;
firstPiolaDifferential(tetra.m_F, dF, dP);
btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col);
btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose();
// elastic force differential
btScalar scale1 = scale * tetra.m_element_measure;
df[id0] -= scale1 * df_on_node0;
df[id1] -= scale1 * df_on_node123.getColumn(0);
df[id2] -= scale1 * df_on_node123.getColumn(1);
df[id3] -= scale1 * df_on_node123.getColumn(2);
}
}
}
void firstPiola(const btMatrix3x3& F, btMatrix3x3& P)
{
btMatrix3x3 C = F.transpose()*F;
btScalar J = F.determinant();
btScalar trace = C[0].getX() + C[1].getY() + C[2].getZ();
P = F * m_mu * ( 1. - 1. / (trace + 1.)) + F.adjoint().transpose() * (m_lambda * (J - 1) - 0.75 * m_mu);
}
virtual void addScaledForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
{
P = F * m_mu * ( 1. - 1. / (trace + 1.)) + F.adjoint().transpose() * (m_lambda * (J - 1.) - 0.75 * m_mu);
}
void firstPiolaDifferential(const btMatrix3x3& F, const btMatrix3x3& dF, btMatrix3x3& dP)
{
btScalar J = F.determinant();
addScaledCofactorMatrixDifferential(F, dF, m_lambda*(J-1) - 0.75*m_mu, dP);
dP += F.adjoint().transpose() * m_lambda * DotProduct(F.adjoint().transpose(), dF);
btMatrix3x3 C = F.transpose()*F;
btScalar trace = C[0].getX() + C[1].getY() + C[2].getZ();
dP = dF * m_mu * ( 1. - 1. / (trace + 1.)) + F * (2*m_mu) * DotProduct(F, dF) * (1./((1.+trace)*(1.+trace)));
//todo @xuchenhan: add the dP of the m_mu term.
addScaledCofactorMatrixDifferential(F, dF, m_lambda*(J-1.) - 0.75*m_mu, dP);
dP += F.adjoint().transpose() * m_lambda * DotProduct(F.adjoint().transpose(), dF);
}
btScalar DotProduct(const btMatrix3x3& A, const btMatrix3x3& B)
@@ -108,25 +172,26 @@ public:
btScalar ans = 0;
for (int i = 0; i < 3; ++i)
{
for (int j = 0; j < 3; ++j)
{
ans += A[i][j] * B[i][j];
}
ans += A[i].dot(B[i]);
// for (int j = 0; j < 3; ++j)
// {
// ans += A[i][j] * B[i][j];
// }
}
return ans;
}
void addScaledCofactorMatrixDifferential(const btMatrix3x3& F, const btMatrix3x3& dF, btScalar scale, btMatrix3x3& M)
{
M[0][0] = scale * (dF[1][1] * F[2][2] + F[1][1] * dF[2][2] - dF[2][1] * F[1][2] - F[2][1] * dF[1][2]);
M[1][0] = scale * (dF[2][1] * F[0][2] + F[2][1] * dF[0][2] - dF[0][1] * F[2][2] - F[0][1] * dF[2][2]);
M[2][0] = scale * (dF[0][1] * F[1][2] + F[0][1] * dF[1][2] - dF[1][1] * F[0][2] - F[1][1] * dF[0][2]);
M[0][1] = scale * (dF[2][0] * F[1][2] + F[2][0] * dF[1][2] - dF[1][0] * F[2][2] - F[1][0] * dF[2][2]);
M[1][1] = scale * (dF[0][0] * F[2][2] + F[0][0] * dF[2][2] - dF[2][0] * F[0][2] - F[2][0] * dF[0][2]);
M[2][1] = scale * (dF[1][0] * F[0][2] + F[1][0] * dF[0][2] - dF[0][0] * F[1][2] - F[0][0] * dF[1][2]);
M[0][2] = scale * (dF[1][0] * F[2][1] + F[1][0] * dF[2][1] - dF[2][0] * F[1][1] - F[2][0] * dF[1][1]);
M[1][2] = scale * (dF[2][0] * F[0][1] + F[2][0] * dF[0][1] - dF[0][0] * F[2][1] - F[0][0] * dF[2][1]);
M[2][2] = scale * (dF[0][0] * F[1][1] + F[0][0] * dF[1][1] - dF[1][0] * F[0][1] - F[1][0] * dF[0][1]);
M[0][0] += scale * (dF[1][1] * F[2][2] + F[1][1] * dF[2][2] - dF[2][1] * F[1][2] - F[2][1] * dF[1][2]);
M[1][0] += scale * (dF[2][1] * F[0][2] + F[2][1] * dF[0][2] - dF[0][1] * F[2][2] - F[0][1] * dF[2][2]);
M[2][0] += scale * (dF[0][1] * F[1][2] + F[0][1] * dF[1][2] - dF[1][1] * F[0][2] - F[1][1] * dF[0][2]);
M[0][1] += scale * (dF[2][0] * F[1][2] + F[2][0] * dF[1][2] - dF[1][0] * F[2][2] - F[1][0] * dF[2][2]);
M[1][1] += scale * (dF[0][0] * F[2][2] + F[0][0] * dF[2][2] - dF[2][0] * F[0][2] - F[2][0] * dF[0][2]);
M[2][1] += scale * (dF[1][0] * F[0][2] + F[1][0] * dF[0][2] - dF[0][0] * F[1][2] - F[0][0] * dF[1][2]);
M[0][2] += scale * (dF[1][0] * F[2][1] + F[1][0] * dF[2][1] - dF[2][0] * F[1][1] - F[2][0] * dF[1][1]);
M[1][2] += scale * (dF[2][0] * F[0][1] + F[2][0] * dF[0][1] - dF[0][0] * F[2][1] - F[0][0] * dF[2][1]);
M[2][2] += scale * (dF[0][0] * F[1][1] + F[0][0] * dF[1][1] - dF[1][0] * F[0][1] - F[1][0] * dF[0][1]);
}
virtual btDeformableLagrangianForceType getForceType()

8
src/BulletSoftBody/btSoftBody.cpp
View File

@@ -2834,12 +2834,10 @@ void btSoftBody::updateDeformation()
{
for (int i = 0; i < m_tetras.size(); ++i)
{
// updateDeformation is called before predictMotion where m_q is sync'd.
// So m_x is the current position of the node.
btSoftBody::Tetra& t = m_tetras[i];
btVector3 c1 = t.m_n[1]->m_x - t.m_n[0]->m_x;
btVector3 c2 = t.m_n[2]->m_x - t.m_n[0]->m_x;
btVector3 c3 = t.m_n[3]->m_x - t.m_n[0]->m_x;
btVector3 c1 = t.m_n[1]->m_q - t.m_n[0]->m_q;
btVector3 c2 = t.m_n[2]->m_q - t.m_n[0]->m_q;
btVector3 c3 = t.m_n[3]->m_q - t.m_n[0]->m_q;
btMatrix3x3 Ds(c1.getX(), c2.getX(), c3.getX(),
c1.getY(), c2.getY(), c3.getY(),
c1.getZ(), c2.getZ(), c3.getZ());