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

bug fixes in constraints projections; cpplized various functions

Browse Source
This commit is contained in:
Xuchen Han
2019-07-09 14:26:04 -07:00
parent 786b0436ec
commit 13d4e1cc2b
12 changed files with 450 additions and 263 deletions
Download Patch File Download Diff File Expand all files Collapse all files

88
src/BulletSoftBody/btBackwardEulerObjective.cpp Normal file
View File

@@ -0,0 +1,88 @@
//
// btBackwardEulerObjective.cpp
// BulletSoftBody
//
// Created by Xuchen Han on 7/9/19.
//
#include "btBackwardEulerObjective.h"
btBackwardEulerObjective::btBackwardEulerObjective(btAlignedObjectArray<btSoftBody *>& softBodies, const TVStack& backup_v)
: cg(20)
, m_softBodies(softBodies)
, precondition(DefaultPreconditioner())
, projection(m_softBodies, m_dt)
, m_backupVelocity(backup_v)
{
// TODO: this should really be specified in initialization instead of here
btMassSpring* mass_spring = new btMassSpring(m_softBodies);
m_lf.push_back(mass_spring);
}
void btBackwardEulerObjective::reinitialize(bool nodeUpdated)
{
if(nodeUpdated)
{
projection.setSoftBodies(m_softBodies);
}
for (int i = 0; i < m_lf.size(); ++i)
{
m_lf[i]->reinitialize(nodeUpdated);
projection.reinitialize(nodeUpdated);
}
}
void btBackwardEulerObjective::multiply(const TVStack& x, TVStack& b) const
{
for (int i = 0; i < b.size(); ++i)
b[i].setZero();
// add in the mass term
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)
{
const auto& node = psb->m_nodes[j];
b[counter] += (node.m_im == 0) ? btVector3(0,0,0) : x[counter] / node.m_im;
++counter;
}
}
for (int i = 0; i < m_lf.size(); ++i)
{
// add damping matrix
m_lf[i]->addScaledDampingForceDifferential(-m_dt, x, b);
// add stiffness matrix when fully implicity
m_lf[i]->addScaledElasticForceDifferential(-m_dt*m_dt, x, b);
}
}
void btBackwardEulerObjective::computeStep(TVStack& dv, const TVStack& residual, const btScalar& dt)
{
m_dt = dt;
btScalar tolerance = std::numeric_limits<float>::epsilon()* 16 * computeNorm(residual);
cg.solve(*this, dv, residual, tolerance);
}
void btBackwardEulerObjective::updateVelocity(const TVStack& dv)
{
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_constrainedDirections.size() > 0)
psb->m_nodes[j].m_v = m_backupVelocity[counter] + dv[counter];
++counter;
}
}
}
}

68
src/BulletSoftBody/btBackwardEulerObjective.h
View File

@@ -35,17 +35,9 @@ public:
btAlignedObjectArray<btSoftBody *>& m_softBodies;
std::function<void(const TVStack&, TVStack&)> precondition;
btContactProjection projection;
const TVStack& m_backupVelocity;
btBackwardEulerObjective(btAlignedObjectArray<btSoftBody *>& softBodies, const TVStack& backup_v)
: cg(20)
, m_softBodies(softBodies)
, precondition(DefaultPreconditioner())
, projection(m_softBodies)
{
// TODO: this should really be specified in initialization instead of here
btMassSpring* mass_spring = new btMassSpring(m_softBodies);
m_lf.push_back(mass_spring);
}
btBackwardEulerObjective(btAlignedObjectArray<btSoftBody *>& softBodies, const TVStack& backup_v);
virtual ~btBackwardEulerObjective() {}
@@ -54,7 +46,6 @@ public:
void computeResidual(btScalar dt, TVStack& residual) const
{
// gravity is treated explicitly in predictUnconstraintMotion
// add force
for (int i = 0; i < m_lf.size(); ++i)
{
@@ -72,62 +63,29 @@ public:
return std::sqrt(norm_squared);
}
void computeStep(TVStack& dv, const TVStack& residual, const btScalar& dt)
{
m_dt = dt;
btScalar tolerance = std::numeric_limits<float>::epsilon()*16 * computeNorm(residual);
cg.solve(*this, dv, residual, tolerance);
}
void computeStep(TVStack& dv, const TVStack& residual, const btScalar& dt);
void multiply(const TVStack& x, TVStack& b) const
{
for (int i = 0; i < b.size(); ++i)
b[i].setZero();
// add in the mass term
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)
{
const auto& node = psb->m_nodes[j];
b[counter] += (node.m_im == 0) ? btVector3(0,0,0) : x[counter] / node.m_im;
++counter;
}
}
for (int i = 0; i < m_lf.size(); ++i)
{
// damping force is implicit and elastic force is explicit
m_lf[i]->addScaledDampingForceDifferential(-m_dt, x, b);
// m_lf[i]->addScaledElasticForceDifferential(-m_dt*m_dt, x, b);
}
}
void multiply(const TVStack& x, TVStack& b) const;
void updateProjection(const TVStack& dv)
{
projection.update(m_dt, dv);
projection.update(dv, m_backupVelocity);
}
void reinitialize(bool nodeUpdated)
{
if(nodeUpdated)
{
projection.setSoftBodies(m_softBodies);
}
for (int i = 0; i < m_lf.size(); ++i)
{
m_lf[i]->reinitialize(nodeUpdated);
projection.reinitialize(nodeUpdated);
}
}
void reinitialize(bool nodeUpdated);
void enforceConstraint(TVStack& x)
{
projection.enforceConstraint(x);
updateVelocity(x);
}
void updateVelocity(const TVStack& dv);
void setConstraintDirections()
{
projection.setConstraintDirections();
}
void project(TVStack& r, const TVStack& dv)
{
updateProjection(dv);

17
src/BulletSoftBody/btCGProjection.h
View File

@@ -23,30 +23,39 @@ public:
std::unordered_map<btSoftBody::Node *, size_t> m_indices;
TVArrayStack m_constrainedDirections;
TArrayStack m_constrainedValues;
const btScalar& m_dt;
btCGProjection(btAlignedObjectArray<btSoftBody *>& softBodies)
btCGProjection(btAlignedObjectArray<btSoftBody *>& softBodies, const btScalar& dt)
: m_softBodies(softBodies)
, m_dt(dt)
{
}
virtual ~btCGProjection()
{
}
// apply the constraints
virtual void operator()(TVStack& x) = 0;
virtual void setConstraintDirections() = 0;
// update the constraints
virtual void update(btScalar dt, const TVStack& dv) = 0;
virtual void update(const TVStack& dv, const TVStack& backup_v) = 0;
virtual void reinitialize(bool nodeUpdated)
{
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();
}
}
void updateId()

68
src/BulletSoftBody/btConjugateGradient.h
View File

@@ -8,6 +8,10 @@
#ifndef BT_CONJUGATE_GRADIENT_H
#define BT_CONJUGATE_GRADIENT_H
#include <iostream>
#include <cmath>
#include <LinearMath/btAlignedObjectArray.h>
#include <LinearMath/btVector3.h>
template <class TM>
class btConjugateGradient
{
@@ -25,69 +29,6 @@ public:
virtual ~btConjugateGradient(){}
// // return the number of iterations taken
// int solve(const TM& A, TVStack& x, const TVStack& b, btScalar tolerance)
// {
// btAssert(x.size() == b.size());
// reinitialize(b);
//
// // r = M * (b - A * x) --with assigned dof zeroed out
// A.multiply(x, temp);
// temp = sub(b, temp);
// A.project(temp);
// A.precondition(temp, r);
//
// btScalar r_dot_r = squaredNorm(r), r_dot_r_new;
// btScalar r_norm = std::sqrt(r_dot_r);
// if (r_norm < tolerance) {
// std::cout << "Iteration = 0" << std::endl;
// std::cout << "Two norm of the residual = " << r_norm << std::endl;
// return 0;
// }
//
// p = r;
// // q = M * A * q;
// A.multiply(p, temp);
// A.precondition(temp, q);
//
// // alpha = |r|^2 / (p^T * A * p)
// btScalar alpha = r_dot_r / dot(p, q), beta;
//
// for (int k = 1; k < max_iterations; k++) {
//// x += alpha * p;
//// r -= alpha * q;
// multAndAddTo(alpha, p, x);
// multAndAddTo(-alpha, q, r);
//
// // zero out the dofs of r
// A.project(r);
//
// r_dot_r_new = squaredNorm(r);
// r_norm = std::sqrt(r_dot_r_new);
//
// if (r_norm < tolerance) {
// std::cout << "ConjugateGradient iterations " << k << std::endl;
// return k;
//
// beta = r_dot_r_new / r_dot_r;
// r_dot_r = r_dot_r_new;
//// p = r + beta * p;
// p = multAndAdd(beta, p, r);
//
// // q = M * A * q;
// A.multiply(p, temp);
// A.precondition(temp, q);
//
// alpha = r_dot_r / dot(p, q);
// }
//
// setZero(q);
// setZero(r);
// }
// std::cout << "ConjugateGradient max iterations reached " << max_iterations << std::endl;
// return max_iterations;
// }
// return the number of iterations taken
int solve(TM& A, TVStack& x, const TVStack& b, btScalar tolerance)
{
@@ -109,7 +50,6 @@ public:
// z = M^(-1) * r
A.precondition(r, z);
// p = z;
p = z;
// temp = A*p
A.multiply(p, temp);

151
src/BulletSoftBody/btContactProjection.cpp
View File

@@ -7,44 +7,16 @@
#include "btContactProjection.h"
#include "btDeformableRigidDynamicsWorld.h"
void btContactProjection::update(btScalar dt, const TVStack& dv)
void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocity)
{
///solve rigid body constraints
m_world->btSoftRigidDynamicsWorld::btDiscreteDynamicsWorld::solveConstraints(m_world->getSolverInfo());
// clear the old constraints
for (int i = 0; i < m_constrainedDirections.size(); ++i)
{
m_constrainedDirections[i].clear();
m_constrainedValues[i].clear();
}
// Set dirichlet constraints
size_t counter = 0;
for (int i = 0; i < m_softBodies.size(); ++i)
{
const btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
if (psb->m_nodes[j].m_im == 0)
{
m_constrainedDirections[counter].push_back(btVector3(1,0,0));
m_constrainedDirections[counter].push_back(btVector3(0,1,0));
m_constrainedDirections[counter].push_back(btVector3(0,0,1));
m_constrainedValues[counter].push_back(0);
m_constrainedValues[counter].push_back(0);
m_constrainedValues[counter].push_back(0);
}
++counter;
}
}
// loop through contacts to create contact constraints
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
btMultiBodyJacobianData jacobianData;
const btScalar mrg = psb->getCollisionShape()->getMargin();
for (int i = 0, ni = psb->m_rcontacts.size(); i < ni; ++i)
{
const btSoftBody::RContact& c = psb->m_rcontacts[i];
@@ -65,7 +37,7 @@ void btContactProjection::update(btScalar dt, const TVStack& dv)
if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
{
rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
va = rigidCol ? (rigidCol->getVelocityInLocalPoint(c.m_c1)) * dt : btVector3(0, 0, 0);
va = rigidCol ? (rigidCol->getVelocityInLocalPoint(c.m_c1)) * m_dt : btVector3(0, 0, 0);
}
else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
{
@@ -86,29 +58,25 @@ void btContactProjection::update(btScalar dt, const TVStack& dv)
{
vel += multibodyLinkCol->m_multiBody->getVelocityVector()[j] * jac[j];
}
va = cti.m_normal * vel * dt;
va = cti.m_normal * vel * m_dt;
}
}
// TODO: rethink what the velocity of the soft body node should be
// const btVector3 vb = c.m_node->m_x - c.m_node->m_q;
const btVector3 vb = c.m_node->m_v * dt;
const btVector3 vb = c.m_node->m_v * m_dt;
const btVector3 vr = vb - va;
const btScalar dn = btDot(vr, cti.m_normal);
if (dn <= SIMD_EPSILON)
if (1) // in the same CG solve, the set of constraits doesn't change
// if (dn <= SIMD_EPSILON)
{
const btScalar dp = btMin((btDot(c.m_node->m_x, cti.m_normal) + cti.m_offset), mrg);
const btVector3 fv = vr - (cti.m_normal * dn);
// 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 * ((vr - (fv * c.m_c3)));
const btVector3 impulse = c.m_c0 * ((vr - (fv * c.m_c3))+ (cti.m_normal * (dp * c.m_c4)));
const btVector3 impulse = c.m_c0 *(cti.m_normal * dn);
// TODO: only contact is considered here, add friction later
btVector3 normal = cti.m_normal.normalized();
btVector3 dv = -impulse * c.m_c2/dt;
btScalar dvn = dv.dot(normal);
m_constrainedDirections[m_indices[c.m_node]].push_back(normal);
m_constrainedValues[m_indices[c.m_node]].push_back(dvn);
// 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);
m_constrainedValues[m_indices[c.m_node]][0]=(dvn);
if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
{
@@ -128,3 +96,94 @@ void btContactProjection::update(btScalar dt, const TVStack& dv)
}
}
}
void btContactProjection::setConstraintDirections()
{
// set Dirichlet constraint
size_t counter = 0;
for (int i = 0; i < m_softBodies.size(); ++i)
{
const btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
if (psb->m_nodes[j].m_im == 0)
{
m_constrainedDirections[counter].push_back(btVector3(1,0,0));
m_constrainedDirections[counter].push_back(btVector3(0,1,0));
m_constrainedDirections[counter].push_back(btVector3(0,0,1));
m_constrainedValues[counter].push_back(0);
m_constrainedValues[counter].push_back(0);
m_constrainedValues[counter].push_back(0);
}
++counter;
}
}
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
btMultiBodyJacobianData jacobianData;
int j = 0;
while (j < psb->m_rcontacts.size())
{
const btSoftBody::RContact& c = psb->m_rcontacts[j];
// skip anchor points
if (c.m_node->m_im == 0)
{
psb->m_rcontacts.removeAtIndex(j);
continue;
}
const btSoftBody::sCti& cti = c.m_cti;
if (cti.m_colObj->hasContactResponse())
{
btVector3 va(0, 0, 0);
btRigidBody* rigidCol = 0;
btMultiBodyLinkCollider* multibodyLinkCol = 0;
btScalar* deltaV;
// grab the velocity of the rigid body
if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
{
rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
va = rigidCol ? (rigidCol->getVelocityInLocalPoint(c.m_c1)) * m_dt : btVector3(0, 0, 0);
}
else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
{
multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
if (multibodyLinkCol)
{
const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
jacobianData.m_jacobians.resize(ndof);
jacobianData.m_deltaVelocitiesUnitImpulse.resize(ndof);
btScalar* jac = &jacobianData.m_jacobians[0];
multibodyLinkCol->m_multiBody->fillContactJacobianMultiDof(multibodyLinkCol->m_link, c.m_node->m_x, cti.m_normal, jac, jacobianData.scratch_r, jacobianData.scratch_v, jacobianData.scratch_m);
deltaV = &jacobianData.m_deltaVelocitiesUnitImpulse[0];
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)
{
vel += multibodyLinkCol->m_multiBody->getVelocityVector()[j] * jac[j];
}
va = cti.m_normal * vel * m_dt;
}
}
const btVector3 vb = c.m_node->m_v * m_dt;
const btVector3 vr = vb - va;
const btScalar dn = btDot(vr, cti.m_normal);
if (dn < SIMD_EPSILON)
{
++j;
m_constrainedDirections[m_indices[c.m_node]].push_back(cti.m_normal);
m_constrainedValues[m_indices[c.m_node]].resize(m_constrainedValues[m_indices[c.m_node]].size()+1);
continue;
}
}
psb->m_rcontacts.removeAtIndex(j);
}
}
}

8
src/BulletSoftBody/btContactProjection.h
View File

@@ -15,8 +15,8 @@
class btContactProjection : public btCGProjection
{
public:
btContactProjection(btAlignedObjectArray<btSoftBody *>& softBodies)
: btCGProjection(softBodies)
btContactProjection(btAlignedObjectArray<btSoftBody *>& softBodies, const btScalar& dt)
: btCGProjection(softBodies, dt)
{
}
@@ -51,6 +51,8 @@ public:
}
// update the constraints
virtual void update(btScalar dt, const TVStack& dv);
virtual void update(const TVStack& dv, const TVStack& backupVelocity);
virtual void setConstraintDirections();
};
#endif /* btContactProjection_h */

157
src/BulletSoftBody/btDeformableBodySolver.cpp Normal file
View File

@@ -0,0 +1,157 @@
//
// btDeformableBodySolver.cpp
// BulletSoftBody
//
// Created by Xuchen Han on 7/9/19.
//
#include <stdio.h>
#include "btDeformableBodySolver.h"
void btDeformableBodySolver::postStabilize()
{
for (int i = 0; i < m_softBodySet.size(); ++i)
{
btSoftBody* psb = m_softBodySet[i];
btMultiBodyJacobianData jacobianData;
const btScalar mrg = psb->getCollisionShape()->getMargin();
for (int j = 0; j < psb->m_rcontacts.size(); ++j)
{
const btSoftBody::RContact& c = psb->m_rcontacts[j];
// skip anchor points
if (c.m_node->m_im == 0)
continue;
const btSoftBody::sCti& cti = c.m_cti;
if (cti.m_colObj->hasContactResponse())
{
btVector3 va(0, 0, 0);
btRigidBody* rigidCol = 0;
btMultiBodyLinkCollider* multibodyLinkCol = 0;
btScalar* deltaV;
// grab the velocity of the rigid body
if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
{
rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
va = rigidCol ? (rigidCol->getVelocityInLocalPoint(c.m_c1)) * m_dt : btVector3(0, 0, 0);
}
else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
{
multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
if (multibodyLinkCol)
{
const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
jacobianData.m_jacobians.resize(ndof);
jacobianData.m_deltaVelocitiesUnitImpulse.resize(ndof);
btScalar* jac = &jacobianData.m_jacobians[0];
multibodyLinkCol->m_multiBody->fillContactJacobianMultiDof(multibodyLinkCol->m_link, c.m_node->m_x, cti.m_normal, jac, jacobianData.scratch_r, jacobianData.scratch_v, jacobianData.scratch_m);
deltaV = &jacobianData.m_deltaVelocitiesUnitImpulse[0];
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)
{
vel += multibodyLinkCol->m_multiBody->getVelocityVector()[j] * jac[j];
}
va = cti.m_normal * vel * m_dt;
}
}
const btVector3 vb = c.m_node->m_v * m_dt;
const btVector3 vr = vb - va;
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);
// 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;
const btVector3 impulse = c.m_c0 * ((cti.m_normal * (dn * c.m_c4)));
// TODO: only contact is considered here, add friction later
if (dp < 0)
{
c.m_node->m_x -= dp * cti.m_normal * c.m_c4;
////
// if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
// {
// if (rigidCol)
// 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);
// }
// }
}
}
}
}
void btDeformableBodySolver::solveConstraints(float solverdt)
{
m_dt = solverdt;
bool nodeUpdated = updateNodes();
reinitialize(nodeUpdated);
backupVelocity();
postStabilize();
for (int i = 0; i < m_solveIterations; ++i)
{
m_objective->computeResidual(solverdt, m_residual);
m_objective->computeStep(m_dv, m_residual, solverdt);
updateVelocity();
}
advect(solverdt);
}
void btDeformableBodySolver::reinitialize(bool nodeUpdated)
{
if (nodeUpdated)
{
m_dv.resize(m_numNodes);
m_residual.resize(m_numNodes);
}
for (int i = 0; i < m_dv.size(); ++i)
{
m_dv[i].setZero();
m_residual[i].setZero();
}
m_objective->reinitialize(nodeUpdated);
// remove contact constraints with separating velocity
setConstraintDirections();
}
void btDeformableBodySolver::setConstraintDirections()
{
m_objective->setConstraintDirections();
}
void btDeformableBodySolver::setWorld(btDeformableRigidDynamicsWorld* world)
{
m_world = world;
m_objective->setWorld(world);
}
void btDeformableBodySolver::updateVelocity()
{
// 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)
{
// psb->m_nodes[j].m_v += m_dv[counter];
psb->m_nodes[j].m_v = m_backupVelocity[counter]+m_dv[counter];
++counter;
}
}
}

89
src/BulletSoftBody/btDeformableBodySolver.h
View File

@@ -16,7 +16,7 @@
#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
struct btCollisionObjectWrapper;
class btBackwardEulerObjective;
class btDeformableRigidDynamicsWorld;
class btDeformableBodySolver : public btSoftBodySolver
@@ -29,25 +29,17 @@ protected:
TVStack m_dv;
TVStack m_residual;
btAlignedObjectArray<btSoftBody *> m_softBodySet;
btBackwardEulerObjective m_objective;
btBackwardEulerObjective* m_objective;
int m_solveIterations;
int m_impulseIterations;
btDeformableRigidDynamicsWorld* m_world;
btAlignedObjectArray<btVector3> m_backupVelocity;
btScalar m_dt;
public:
btDeformableBodySolver()
: m_numNodes(0)
, m_objective(m_softBodySet, m_backupVelocity)
, m_solveIterations(1)
, m_impulseIterations(1)
, m_world(nullptr)
{
}
btDeformableBodySolver();
virtual ~btDeformableBodySolver()
{
}
virtual ~btDeformableBodySolver();
virtual SolverTypes getSolverType() const
{
@@ -78,21 +70,9 @@ public:
virtual void copyBackToSoftBodies(bool bMove = true) {}
virtual void solveConstraints(float solverdt)
{
bool nodeUpdated = updateNodes();
reinitialize(nodeUpdated);
for (int i = 0; i < m_solveIterations; ++i)
{
// only need to advect x here if elastic force is implicit
// prepareSolve(solverdt);
m_objective.computeResidual(solverdt, m_residual);
m_objective.computeStep(m_dv, m_residual, solverdt);
updateVelocity();
}
advect(solverdt);
}
virtual void solveConstraints(float solverdt);
void postStabilize();
void moveTempVelocity(btScalar dt, const TVStack& f)
{
@@ -108,34 +88,10 @@ public:
}
}
void reinitialize(bool nodeUpdated)
{
if (nodeUpdated)
{
m_dv.resize(m_numNodes);
m_residual.resize(m_numNodes);
}
for (int i = 0; i < m_dv.size(); ++i)
{
m_dv[i].setZero();
m_residual[i].setZero();
}
m_objective.reinitialize(nodeUpdated);
}
void reinitialize(bool nodeUpdated);
void setConstraintDirections();
void prepareSolve(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 advect(btScalar dt)
{
size_t counter = 0;
@@ -145,13 +101,13 @@ public:
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
auto& node = psb->m_nodes[j];
// node.m_x += dt * m_dv[counter++];
node.m_x += dt * node.m_v;
node.m_x += dt * m_dv[counter++];
// node.m_x = node.m_q + dt * node.m_v;
}
}
}
void updateVelocity()
void backupVelocity()
{
// serial implementation
int counter = 0;
@@ -160,13 +116,13 @@ public:
btSoftBody* psb = m_softBodySet[i];
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
psb->m_nodes[j].m_v += m_dv[counter];
++counter;
m_backupVelocity[counter++] = psb->m_nodes[j].m_v;
}
}
}
void updateVelocity();
bool updateNodes()
{
int numNodes = 0;
@@ -175,6 +131,7 @@ public:
if (numNodes != m_numNodes)
{
m_numNodes = numNodes;
m_backupVelocity.resize(numNodes);
return true;
}
return false;
@@ -200,15 +157,11 @@ public:
softBody->defaultCollisionHandler(collisionObjectWrap);
}
virtual void processCollision(btSoftBody *, btSoftBody *) {
// TODO
virtual void processCollision(btSoftBody * softBody, btSoftBody * otherSoftBody) {
softBody->defaultCollisionHandler(otherSoftBody);
}
virtual void setWorld(btDeformableRigidDynamicsWorld* world)
{
m_world = world;
m_objective.setWorld(world);
}
virtual void setWorld(btDeformableRigidDynamicsWorld* world);
};
#endif /* btDeformableBodySolver_h */

39
src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp
View File

@@ -9,6 +9,20 @@
#include "btDeformableRigidDynamicsWorld.h"
#include "btDeformableBodySolver.h"
btDeformableBodySolver::btDeformableBodySolver()
: m_numNodes(0)
, m_solveIterations(1)
, m_impulseIterations(1)
, m_world(nullptr)
{
m_objective = new btBackwardEulerObjective(m_softBodySet, m_backupVelocity);
}
btDeformableBodySolver::~btDeformableBodySolver()
{
delete m_objective;
}
void btDeformableRigidDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
{
// Let the solver grab the soft bodies and if necessary optimize for it
@@ -50,9 +64,6 @@ void btDeformableRigidDynamicsWorld::internalSingleStepSimulation(btScalar timeS
(*m_internalTickCallback)(this, timeStep);
}
// btSoftRigidDynamicsWorld::btDiscreteDynamicsWorld::internalSingleStepSimulation(timeStep);
// incorporate gravity into velocity and clear force
for (int i = 0; i < m_nonStaticRigidBodies.size(); ++i)
{
@@ -64,7 +75,6 @@ void btDeformableRigidDynamicsWorld::internalSingleStepSimulation(btScalar timeS
///solve deformable bodies constraints
solveDeformableBodiesConstraints(timeStep);
// predictUnconstraintMotion(timeStep);
//integrate transforms
btSoftRigidDynamicsWorld::btDiscreteDynamicsWorld::integrateTransforms(timeStep);
@@ -76,6 +86,7 @@ void btDeformableRigidDynamicsWorld::internalSingleStepSimulation(btScalar timeS
///update soft bodies
m_deformableBodySolver->updateSoftBodies();
clearForces();
// End solver-wise simulation step
// ///////////////////////////////
}
@@ -85,3 +96,23 @@ void btDeformableRigidDynamicsWorld::solveDeformableBodiesConstraints(btScalar t
m_deformableBodySolver->solveConstraints(timeStep);
}
void btDeformableRigidDynamicsWorld::addSoftBody(btSoftBody* body, int collisionFilterGroup, int collisionFilterMask)
{
getSoftDynamicsWorld()->getSoftBodyArray().push_back(body);
// Set the soft body solver that will deal with this body
// to be the world's solver
body->setSoftBodySolver(m_deformableBodySolver);
btCollisionWorld::addCollisionObject(body,
collisionFilterGroup,
collisionFilterMask);
}
void btDeformableRigidDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
{
btDiscreteDynamicsWorld::predictUnconstraintMotion(timeStep);
m_deformableBodySolver->predictMotion(float(timeStep));
}

19
src/BulletSoftBody/btDeformableRigidDynamicsWorld.h
View File

@@ -62,27 +62,12 @@ public:
return BT_DEFORMABLE_RIGID_DYNAMICS_WORLD;
}
virtual void predictUnconstraintMotion(btScalar timeStep)
{
btDiscreteDynamicsWorld::predictUnconstraintMotion(timeStep);
m_deformableBodySolver->predictMotion(float(timeStep));
}
virtual void predictUnconstraintMotion(btScalar timeStep);
// virtual void internalStepSingleStepSimulation(btScalar timeStep);
// virtual void solveDeformableBodiesConstraints(btScalar timeStep);
virtual void addSoftBody(btSoftBody* body, int collisionFilterGroup = btBroadphaseProxy::DefaultFilter, int collisionFilterMask = btBroadphaseProxy::AllFilter)
{
getSoftDynamicsWorld()->getSoftBodyArray().push_back(body);
// Set the soft body solver that will deal with this body
// to be the world's solver
body->setSoftBodySolver(m_deformableBodySolver);
btCollisionWorld::addCollisionObject(body,
collisionFilterGroup,
collisionFilterMask);
}
virtual void addSoftBody(btSoftBody* body, int collisionFilterGroup = btBroadphaseProxy::DefaultFilter, int collisionFilterMask = btBroadphaseProxy::AllFilter);
};
#endif //BT_DEFORMABLE_RIGID_DYNAMICS_WORLD_H

7
src/BulletSoftBody/btMassSpring.h
View File

@@ -37,7 +37,12 @@ public:
size_t id2 = m_indices[node2];
// elastic force
// fully implicit
btVector3 dir = (node2->m_x - node1->m_x);
// explicit elastic force
// btVector3 dir = (node2->m_q - node1->m_q);
btVector3 dir_normalized = dir.normalized();
btVector3 scaled_force = scale * kLST * (dir - dir_normalized * r);
force[id1] += scaled_force;
@@ -89,7 +94,7 @@ public:
const auto& link = psb->m_links[j];
const auto node1 = link.m_n[0];
const auto node2 = link.m_n[1];
btScalar k_damp = psb->m_dampingCoefficient; // TODO: FIX THIS HACK and set k_damp properly
btScalar k_damp = psb->m_dampingCoefficient;
size_t id1 = m_indices[node1];
size_t id2 = m_indices[node2];
btVector3 local_scaled_df = scale * k_damp * (dv[id2] - dv[id1]);

2
src/BulletSoftBody/btSoftBodyInternals.h
View File

@@ -880,7 +880,7 @@ struct btSoftColliders
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 btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic;
const btVector3 ra = n.m_x - wtr.getOrigin();
const btVector3 ra = n.m_q - wtr.getOrigin();
const btVector3 va = m_rigidBody ? m_rigidBody->getVelocityInLocalPoint(ra) * psb->m_sst.sdt : btVector3(0, 0, 0);
const btVector3 vb = n.m_x - n.m_q;
const btVector3 vr = vb - va;