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

deformable code refactor

Browse Source
This commit is contained in:
Xuchen Han
2019-07-21 18:32:54 -07:00
parent dc10336d45
commit a90cad2a96
17 changed files with 1065 additions and 382 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h
View File

@@ -229,6 +229,16 @@ public:
{ {
return m_latencyMotionStateInterpolation; return m_latencyMotionStateInterpolation;
} }
btAlignedObjectArray<btRigidBody*>& getNonStaticRigidBodies()
{
return m_nonStaticRigidBodies;
}
const btAlignedObjectArray<btRigidBody*>& getNonStaticRigidBodies() const
{
return m_nonStaticRigidBodies;
}
}; };
#endif //BT_DISCRETE_DYNAMICS_WORLD_H #endif //BT_DISCRETE_DYNAMICS_WORLD_H

28
src/BulletSoftBody/btBackwardEulerObjective.h
View File

@@ -1,5 +1,5 @@
// //
// btBackwardEulerObjective.h // btDeformableBackwardEulerObjective.h
// BulletSoftBody // BulletSoftBody
// //
// Created by Xuchen Han on 7/1/19. // Created by Xuchen Han on 7/1/19.
@@ -11,27 +11,26 @@
#include "btConjugateGradient.h" #include "btConjugateGradient.h"
#include "btLagrangianForce.h" #include "btLagrangianForce.h"
#include "btMassSpring.h" #include "btMassSpring.h"
#include "btContactProjection.h" #include "btDeformableContactProjection.h"
#include "btPreconditioner.h" #include "btPreconditioner.h"
#include "btDeformableRigidDynamicsWorld.h" #include "btDeformableRigidDynamicsWorld.h"
class btDeformableRigidDynamicsWorld; class btDeformableRigidDynamicsWorld;
class btBackwardEulerObjective class btDeformableBackwardEulerObjective
{ {
public: public:
using TVStack = btAlignedObjectArray<btVector3>; using TVStack = btAlignedObjectArray<btVector3>;
btScalar m_dt; btScalar m_dt;
btConjugateGradient<btBackwardEulerObjective> cg;
btDeformableRigidDynamicsWorld* m_world; btDeformableRigidDynamicsWorld* m_world;
btAlignedObjectArray<btLagrangianForce*> m_lf; btAlignedObjectArray<btLagrangianForce*> m_lf;
btAlignedObjectArray<btSoftBody *>& m_softBodies; btAlignedObjectArray<btSoftBody *>& m_softBodies;
Preconditioner* m_preconditioner; Preconditioner* m_preconditioner;
btContactProjection projection; btDeformableContactProjection projection;
const TVStack& m_backupVelocity; const TVStack& m_backupVelocity;
btBackwardEulerObjective(btAlignedObjectArray<btSoftBody *>& softBodies, const TVStack& backup_v); btDeformableBackwardEulerObjective(btAlignedObjectArray<btSoftBody *>& softBodies, const TVStack& backup_v);
virtual ~btBackwardEulerObjective() {} virtual ~btDeformableBackwardEulerObjective() {}
void initialize(){} void initialize(){}
@@ -53,18 +52,14 @@ public:
// perform A*x = b // perform A*x = b
void multiply(const TVStack& x, TVStack& b) const; 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 // set initial guess for CG solve
void initialGuess(TVStack& dv, const TVStack& residual); void initialGuess(TVStack& dv, const TVStack& residual);
// reset data structure // reset data structure
void reinitialize(bool nodeUpdated); void reinitialize(bool nodeUpdated);
void setDt(btScalar dt);
// enforce constraints in CG solve // enforce constraints in CG solve
void enforceConstraint(TVStack& x) void enforceConstraint(TVStack& x)
{ {
@@ -82,10 +77,11 @@ public:
} }
// update the projections and project the residual // update the projections and project the residual
void project(TVStack& r, const TVStack& dv) void project(TVStack& r)
{ {
updateProjection(dv); projection.update();
projection(r); // TODO rename
projection.project(r);
} }
// perform precondition M^(-1) x = b // perform precondition M^(-1) x = b

51
src/BulletSoftBody/btCGProjection.h
View File

@@ -8,53 +8,61 @@
#define BT_CG_PROJECTION_H #define BT_CG_PROJECTION_H
#include "btSoftBody.h" #include "btSoftBody.h"
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
#include <unordered_map> #include <unordered_map>
class btDeformableRigidDynamicsWorld; class btDeformableRigidDynamicsWorld;
struct Constraint struct DeformableContactConstraint
{ {
btAlignedObjectArray<const btSoftBody::RContact*> m_contact; btAlignedObjectArray<const btSoftBody::RContact*> m_contact;
btAlignedObjectArray<btVector3> m_direction; btAlignedObjectArray<btVector3> m_direction;
btAlignedObjectArray<btScalar> m_value; btAlignedObjectArray<btScalar> m_value;
// the magnitude of the total impulse the node applied to the rb in the normal direction in the cg solve // 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; btAlignedObjectArray<btScalar> m_accumulated_normal_impulse;
btAlignedObjectArray<btMultiBodyJacobianData> m_normal_jacobian;
Constraint(const btSoftBody::RContact& rcontact) DeformableContactConstraint(const btSoftBody::RContact& rcontact, const btMultiBodyJacobianData& jacobian)
{ {
append(rcontact); append(rcontact, jacobian);
} }
Constraint(const btVector3 dir) DeformableContactConstraint(const btVector3 dir)
{ {
m_contact.push_back(nullptr); m_contact.push_back(nullptr);
m_direction.push_back(dir); m_direction.push_back(dir);
m_value.push_back(0); m_value.push_back(0);
m_accumulated_normal_impulse.push_back(0); m_accumulated_normal_impulse.push_back(0);
btMultiBodyJacobianData j;
m_normal_jacobian.push_back(j);
} }
Constraint() DeformableContactConstraint()
{ {
m_contact.push_back(nullptr); m_contact.push_back(nullptr);
m_direction.push_back(btVector3(0,0,0)); m_direction.push_back(btVector3(0,0,0));
m_value.push_back(0); m_value.push_back(0);
m_accumulated_normal_impulse.push_back(0); m_accumulated_normal_impulse.push_back(0);
btMultiBodyJacobianData j;
m_normal_jacobian.push_back(j);
} }
void append(const btSoftBody::RContact& rcontact) void append(const btSoftBody::RContact& rcontact, const btMultiBodyJacobianData& jacobian)
{ {
m_contact.push_back(&rcontact); m_contact.push_back(&rcontact);
m_direction.push_back(rcontact.m_cti.m_normal); m_direction.push_back(rcontact.m_cti.m_normal);
m_value.push_back(0); m_value.push_back(0);
m_accumulated_normal_impulse.push_back(0); m_accumulated_normal_impulse.push_back(0);
m_normal_jacobian.push_back(jacobian);
} }
~Constraint() ~DeformableContactConstraint()
{ {
} }
}; };
struct Friction struct DeformableFrictionConstraint
{ {
btAlignedObjectArray<bool> m_static; // whether the friction is static btAlignedObjectArray<bool> m_static; // whether the friction is static
@@ -69,16 +77,24 @@ struct Friction
btAlignedObjectArray<btVector3> m_direction_prev; btAlignedObjectArray<btVector3> m_direction_prev;
btAlignedObjectArray<bool> m_released; // whether the contact is released btAlignedObjectArray<bool> m_released; // whether the contact is released
btAlignedObjectArray<btMultiBodyJacobianData> m_complementary_jacobian;
btAlignedObjectArray<btVector3> m_complementaryDirection;
// the total impulse the node applied to the rb in the tangential direction in the cg solve // the total impulse the node applied to the rb in the tangential direction in the cg solve
btAlignedObjectArray<btVector3> m_accumulated_tangent_impulse; btAlignedObjectArray<btVector3> m_accumulated_tangent_impulse;
Friction()
DeformableFrictionConstraint()
{ {
append(); append();
} }
DeformableFrictionConstraint(const btVector3& complementaryDir, const btMultiBodyJacobianData& jacobian)
{
append();
addJacobian(complementaryDir, jacobian);
}
void append() void append()
{ {
m_static.push_back(false); m_static.push_back(false);
@@ -96,6 +112,13 @@ struct Friction
m_accumulated_tangent_impulse.push_back(btVector3(0,0,0)); m_accumulated_tangent_impulse.push_back(btVector3(0,0,0));
m_released.push_back(false); m_released.push_back(false);
} }
void addJacobian(const btVector3& complementaryDir, const btMultiBodyJacobianData& jacobian)
{
m_complementary_jacobian.push_back(jacobian);
m_complementaryDirection.push_back(complementaryDir);
}
}; };
class btCGProjection class btCGProjection
@@ -109,8 +132,6 @@ public:
btDeformableRigidDynamicsWorld* m_world; btDeformableRigidDynamicsWorld* m_world;
std::unordered_map<btSoftBody::Node *, size_t> m_indices; std::unordered_map<btSoftBody::Node *, size_t> m_indices;
const btScalar& m_dt; const btScalar& m_dt;
std::unordered_map<btSoftBody::Node *, btAlignedObjectArray<Constraint> > m_constraints;
std::unordered_map<btSoftBody::Node *, btAlignedObjectArray<Friction> > m_frictions;
btCGProjection(btAlignedObjectArray<btSoftBody *>& softBodies, const btScalar& dt) btCGProjection(btAlignedObjectArray<btSoftBody *>& softBodies, const btScalar& dt)
: m_softBodies(softBodies) : m_softBodies(softBodies)
@@ -123,19 +144,17 @@ public:
} }
// apply the constraints // apply the constraints
virtual void operator()(TVStack& x) = 0; virtual void project(TVStack& x) = 0;
virtual void setConstraints() = 0; virtual void setConstraints() = 0;
// update the constraints // update the constraints
virtual void update(const TVStack& dv, const TVStack& backup_v) = 0; virtual void update() = 0;
virtual void reinitialize(bool nodeUpdated) virtual void reinitialize(bool nodeUpdated)
{ {
if (nodeUpdated) if (nodeUpdated)
updateId(); updateId();
m_constraints.clear();
m_frictions.clear();
} }
void updateId() void updateId()

4
src/BulletSoftBody/btConjugateGradient.h
View File

@@ -36,7 +36,7 @@ public:
// r = b - A * x --with assigned dof zeroed out // r = b - A * x --with assigned dof zeroed out
A.multiply(x, temp); A.multiply(x, temp);
r = sub(b, temp); r = sub(b, temp);
A.project(r,x); A.project(r);
A.enforceConstraint(x); A.enforceConstraint(x);
btScalar r_norm = std::sqrt(squaredNorm(r)); btScalar r_norm = std::sqrt(squaredNorm(r));
@@ -62,7 +62,7 @@ public:
multAndAddTo(alpha, p, x); multAndAddTo(alpha, p, x);
multAndAddTo(-alpha, temp, r); multAndAddTo(-alpha, temp, r);
// zero out the dofs of r // zero out the dofs of r
A.project(r,x); A.project(r);
A.enforceConstraint(x); A.enforceConstraint(x);
r_norm = std::sqrt(squaredNorm(r)); r_norm = std::sqrt(squaredNorm(r));

156
src/BulletSoftBody/btContactProjection.cpp
View File

@@ -1,14 +1,47 @@
// //
// btContactProjection.cpp // btDeformableContactProjection.cpp
// BulletSoftBody // BulletSoftBody
// //
// Created by Xuchen Han on 7/4/19. // Created by Xuchen Han on 7/4/19.
// //
#include "btContactProjection.h" #include "btDeformableContactProjection.h"
#include "btDeformableRigidDynamicsWorld.h" #include "btDeformableRigidDynamicsWorld.h"
#include <algorithm> #include <algorithm>
void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocity) static void findJacobian(const btMultiBodyLinkCollider* multibodyLinkCol,
btMultiBodyJacobianData& jacobianData,
const btVector3& contact_point,
const btVector3& dir)
{
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, contact_point, dir, jac, jacobianData.scratch_r, jacobianData.scratch_v, jacobianData.scratch_m);
multibodyLinkCol->m_multiBody->calcAccelerationDeltasMultiDof(&jacobianData.m_jacobians[0], &jacobianData.m_deltaVelocitiesUnitImpulse[0], jacobianData.scratch_r, jacobianData.scratch_v);
}
static btVector3 generateUnitOrthogonalVector(const btVector3& u)
{
btScalar ux = u.getX();
btScalar uy = u.getY();
btScalar uz = u.getZ();
btScalar ax = std::abs(ux);
btScalar ay = std::abs(uy);
btScalar az = std::abs(uz);
btVector3 v;
if (ax <= ay && ax <= az)
v = btVector3(0, -uz, uy);
else if (ay <= ax && ay <= az)
v = btVector3(-uz, 0, ux);
else
v = btVector3(-uy, ux, 0);
v.normalize();
return v;
}
void btDeformableContactProjection::update()
{ {
///solve rigid body constraints ///solve rigid body constraints
m_world->getSolverInfo().m_numIterations = 10; m_world->getSolverInfo().m_numIterations = 10;
@@ -32,13 +65,17 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit
} }
const btSoftBody::RContact* c = constraint.m_contact[j]; const btSoftBody::RContact* c = constraint.m_contact[j];
const btSoftBody::sCti& cti = c->m_cti; const btSoftBody::sCti& cti = c->m_cti;
btMultiBodyJacobianData jacobianData;
// normal jacobian is precompute but tangent jacobian is not
const btMultiBodyJacobianData& jacobianData_normal = constraint.m_normal_jacobian[j];
const btMultiBodyJacobianData& jacobianData_complementary = friction.m_complementary_jacobian[j];
if (cti.m_colObj->hasContactResponse()) if (cti.m_colObj->hasContactResponse())
{ {
btVector3 va(0, 0, 0); btVector3 va(0, 0, 0);
btRigidBody* rigidCol = 0; btRigidBody* rigidCol = 0;
btMultiBodyLinkCollider* multibodyLinkCol = 0; btMultiBodyLinkCollider* multibodyLinkCol = 0;
btScalar* deltaV; const btScalar* deltaV_normal;
// grab the velocity of the rigid body // grab the velocity of the rigid body
if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
@@ -52,20 +89,25 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit
if (multibodyLinkCol) if (multibodyLinkCol)
{ {
const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
jacobianData.m_jacobians.resize(ndof); const btScalar* jac_normal = &jacobianData_normal.m_jacobians[0];
jacobianData.m_deltaVelocitiesUnitImpulse.resize(ndof); deltaV_normal = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
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);
// add in the normal component of the va
btScalar vel = 0.0; btScalar vel = 0.0;
for (int k = 0; k < ndof; ++k) for (int k = 0; k < ndof; ++k)
{ {
vel += multibodyLinkCol->m_multiBody->getVelocityVector()[k] * jac[k]; vel += multibodyLinkCol->m_multiBody->getVelocityVector()[k] * jac_normal[k];
} }
va = cti.m_normal * vel * m_dt; va = cti.m_normal * vel * m_dt;
// add in complementary direction of va
const btScalar* jac_complementary = &jacobianData_complementary.m_jacobians[0];
vel = 0.0;
for (int k = 0; k < ndof; ++k)
{
vel += multibodyLinkCol->m_multiBody->getVelocityVector()[k] * jac_complementary[k];
}
va += friction.m_complementaryDirection[j] * vel * m_dt;
} }
} }
@@ -109,7 +151,7 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit
else else
{ {
friction.m_static[j] = true; friction.m_static[j] = true;
friction.m_impulse[j] = local_tangent_norm; friction.m_impulse[j] = tangent_norm;
} }
} }
else else
@@ -125,15 +167,22 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit
// the incremental impulse applied to rb in the tangential direction // 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]); btVector3 incremental_tangent = (friction.m_impulse_prev[j] * friction.m_direction_prev[j])-(friction.m_impulse[j] * friction.m_direction[j]);
// TODO cleanup
if (1) // in the same CG solve, the set of constraits doesn't change if (1) // in the same CG solve, the set of constraits doesn't change
{ {
// c0 is the impulse matrix, c3 is 1 - the friction coefficient or 0, c4 is the contact hardness coefficient // 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 // dv = new_impulse + accumulated velocity change in previous CG iterations
// so we have the invariant node->m_v = backupVelocity + dv; // so we have the invariant node->m_v = backupVelocity + dv;
// 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; btScalar dvn = -accumulated_normal * c->m_c2/m_dt;
// the following is equivalent
/*
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);
*/
constraint.m_value[j] = dvn; constraint.m_value[j] = dvn;
// the incremental impulse: // the incremental impulse:
@@ -147,10 +196,20 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit
} }
else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
{ {
if (multibodyLinkCol) if (multibodyLinkCol)
{ {
double multiplier = 0.5; double multiplier = 1;
multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV, -impulse.length() * multiplier); multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_normal, -impulse_normal.length() * multiplier);
if (incremental_tangent.norm() > SIMD_EPSILON)
{
btMultiBodyJacobianData jacobian_tangent;
btVector3 tangent = incremental_tangent.normalized();
findJacobian(multibodyLinkCol, jacobian_tangent, c->m_node->m_x, tangent);
const btScalar* deltaV_tangent = &jacobian_tangent.m_deltaVelocitiesUnitImpulse[0];
multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_tangent, incremental_tangent.length() * multiplier);
}
} }
} }
} }
@@ -160,7 +219,7 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit
} }
} }
void btContactProjection::setConstraints() void btDeformableContactProjection::setConstraints()
{ {
// set Dirichlet constraint // set Dirichlet constraint
for (int i = 0; i < m_softBodies.size(); ++i) for (int i = 0; i < m_softBodies.size(); ++i)
@@ -188,8 +247,8 @@ void btContactProjection::setConstraints()
for (int i = 0; i < m_softBodies.size(); ++i) for (int i = 0; i < m_softBodies.size(); ++i)
{ {
btSoftBody* psb = m_softBodies[i]; btSoftBody* psb = m_softBodies[i];
btMultiBodyJacobianData jacobianData; btMultiBodyJacobianData jacobianData_normal;
btAlignedObjectArray<btScalar> jacobian; btMultiBodyJacobianData jacobianData_complementary;
for (int j = 0; j < psb->m_rcontacts.size(); ++j) for (int j = 0; j < psb->m_rcontacts.size(); ++j)
{ {
@@ -206,7 +265,6 @@ void btContactProjection::setConstraints()
btVector3 va(0, 0, 0); btVector3 va(0, 0, 0);
btRigidBody* rigidCol = 0; btRigidBody* rigidCol = 0;
btMultiBodyLinkCollider* multibodyLinkCol = 0; btMultiBodyLinkCollider* multibodyLinkCol = 0;
btScalar* deltaV;
// grab the velocity of the rigid body // grab the velocity of the rigid body
if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
@@ -216,25 +274,18 @@ void btContactProjection::setConstraints()
} }
else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
{ {
jacobian.clear();
multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
if (multibodyLinkCol) if (multibodyLinkCol)
{ {
const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; findJacobian(multibodyLinkCol, jacobianData_normal, c.m_node->m_x, cti.m_normal);
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; btScalar vel = 0.0;
jacobian.resize(ndof); const btScalar* jac = &jacobianData_normal.m_jacobians[0];
const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
for (int j = 0; j < ndof; ++j) for (int j = 0; j < ndof; ++j)
{ {
vel += multibodyLinkCol->m_multiBody->getVelocityVector()[j] * jac[j]; vel += multibodyLinkCol->m_multiBody->getVelocityVector()[j] * jac[j];
jacobian[j] = jac[j]; std::cout << multibodyLinkCol->m_multiBody->getVelocityVector()[j] << std::endl;
std::cout << jac[j] << std::endl;
} }
va = cti.m_normal * vel * m_dt; va = cti.m_normal * vel * m_dt;
} }
@@ -245,13 +296,25 @@ void btContactProjection::setConstraints()
const btScalar dn = btDot(vr, cti.m_normal); const btScalar dn = btDot(vr, cti.m_normal);
if (dn < SIMD_EPSILON) if (dn < SIMD_EPSILON)
{ {
// find complementary jacobian
btVector3 complementaryDirection;
if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
{
multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
if (multibodyLinkCol)
{
complementaryDirection = generateUnitOrthogonalVector(cti.m_normal);
findJacobian(multibodyLinkCol, jacobianData_complementary, c.m_node->m_x, complementaryDirection);
}
}
if (m_constraints.find(c.m_node) == m_constraints.end()) if (m_constraints.find(c.m_node) == m_constraints.end())
{ {
btAlignedObjectArray<Constraint> constraints; btAlignedObjectArray<Constraint> constraints;
constraints.push_back(Constraint(c)); constraints.push_back(Constraint(c, jacobianData_normal));
m_constraints[c.m_node] = constraints; m_constraints[c.m_node] = constraints;
btAlignedObjectArray<Friction> frictions; btAlignedObjectArray<Friction> frictions;
frictions.push_back(Friction()); frictions.push_back(Friction(complementaryDirection, jacobianData_complementary));
m_frictions[c.m_node] = frictions; m_frictions[c.m_node] = frictions;
} }
else else
@@ -268,9 +331,10 @@ void btContactProjection::setConstraints()
if (std::abs(std::abs(dot_prod) - 1) < angle_epsilon) if (std::abs(std::abs(dot_prod) - 1) < angle_epsilon)
{ {
// group the constraints // group the constraints
constraints[j].append(c); constraints[j].append(c, jacobianData_normal);
// push in an empty friction // push in an empty friction
frictions[j].append(); frictions[j].append();
frictions[j].addJacobian(complementaryDirection, jacobianData_complementary);
merged = true; merged = true;
break; break;
} }
@@ -279,8 +343,8 @@ void btContactProjection::setConstraints()
// hard coded no more than 3 constraint directions // hard coded no more than 3 constraint directions
if (!merged && constraints.size() < dim) if (!merged && constraints.size() < dim)
{ {
constraints.push_back(Constraint(c)); constraints.push_back(Constraint(c, jacobianData_normal));
frictions.push_back(Friction()); frictions.push_back(Friction(complementaryDirection, jacobianData_complementary));
} }
} }
} }
@@ -289,7 +353,7 @@ void btContactProjection::setConstraints()
} }
} }
void btContactProjection::enforceConstraint(TVStack& x) void btDeformableContactProjection::enforceConstraint(TVStack& x)
{ {
const int dim = 3; const int dim = 3;
for (auto& it : m_constraints) for (auto& it : m_constraints)
@@ -356,7 +420,7 @@ void btContactProjection::enforceConstraint(TVStack& x)
} }
} }
void btContactProjection::operator()(TVStack& x) void btDeformableContactProjection::project(TVStack& x)
{ {
const int dim = 3; const int dim = 3;
for (auto& it : m_constraints) for (auto& it : m_constraints)
@@ -412,3 +476,13 @@ void btContactProjection::operator()(TVStack& x)
} }
} }
} }
void btDeformableContactProjection::reinitialize(bool nodeUpdated)
{
btCGProjection::reinitialize(nodeUpdated);
m_constraints.clear();
m_frictions.clear();
}

43
src/BulletSoftBody/btBackwardEulerObjective.cpp → src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp
View File

@@ -1,26 +1,26 @@
// //
// btBackwardEulerObjective.cpp // btDeformableBackwardEulerObjective.cpp
// BulletSoftBody // BulletSoftBody
// //
// Created by Xuchen Han on 7/9/19. // Created by Xuchen Han on 7/9/19.
// //
#include "btBackwardEulerObjective.h" #include "btDeformableBackwardEulerObjective.h"
btBackwardEulerObjective::btBackwardEulerObjective(btAlignedObjectArray<btSoftBody *>& softBodies, const TVStack& backup_v) btDeformableBackwardEulerObjective::btDeformableBackwardEulerObjective(btAlignedObjectArray<btSoftBody *>& softBodies, const TVStack& backup_v)
: cg(10) : m_softBodies(softBodies)
, m_softBodies(softBodies)
, projection(m_softBodies, m_dt) , projection(m_softBodies, m_dt)
, m_backupVelocity(backup_v) , m_backupVelocity(backup_v)
{ {
// TODO: this should really be specified in initialization instead of here // TODO: this should really be specified in initialization instead of here
btMassSpring* mass_spring = new btMassSpring(m_softBodies); btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(m_softBodies);
// m_preconditioner = new MassPreconditioner(m_softBodies); btDeformableGravityForce* gravity = new btDeformableGravityForce(m_softBodies, btVector3(0,-10,0));
m_preconditioner = new DefaultPreconditioner(); m_preconditioner = new DefaultPreconditioner();
m_lf.push_back(mass_spring); m_lf.push_back(mass_spring);
m_lf.push_back(gravity);
} }
void btBackwardEulerObjective::reinitialize(bool nodeUpdated) void btDeformableBackwardEulerObjective::reinitialize(bool nodeUpdated)
{ {
if(nodeUpdated) if(nodeUpdated)
{ {
@@ -34,8 +34,12 @@ void btBackwardEulerObjective::reinitialize(bool nodeUpdated)
m_preconditioner->reinitialize(nodeUpdated); m_preconditioner->reinitialize(nodeUpdated);
} }
void btDeformableBackwardEulerObjective::setDt(btScalar dt)
{
m_dt = dt;
}
void btBackwardEulerObjective::multiply(const TVStack& x, TVStack& b) const void btDeformableBackwardEulerObjective::multiply(const TVStack& x, TVStack& b) const
{ {
for (int i = 0; i < b.size(); ++i) for (int i = 0; i < b.size(); ++i)
b[i].setZero(); b[i].setZero();
@@ -56,18 +60,11 @@ void btBackwardEulerObjective::multiply(const TVStack& x, TVStack& b) const
for (int i = 0; i < m_lf.size(); ++i) for (int i = 0; i < m_lf.size(); ++i)
{ {
// add damping matrix // add damping matrix
m_lf[i]->addScaledDampingForceDifferential(-m_dt, x, b); m_lf[i]->addScaledForceDifferential(-m_dt, x, b);
} }
} }
void btBackwardEulerObjective::computeStep(TVStack& dv, const TVStack& residual, const btScalar& dt) void btDeformableBackwardEulerObjective::updateVelocity(const TVStack& dv)
{
m_dt = dt;
btScalar tolerance = std::numeric_limits<float>::epsilon()* 1024 * computeNorm(residual);
cg.solve(*this, dv, residual, tolerance);
}
void btBackwardEulerObjective::updateVelocity(const TVStack& dv)
{ {
// only the velocity of the constrained nodes needs to be updated during CG solve // only the velocity of the constrained nodes needs to be updated during CG solve
for (auto it : projection.m_constraints) for (auto it : projection.m_constraints)
@@ -77,7 +74,7 @@ void btBackwardEulerObjective::updateVelocity(const TVStack& dv)
} }
} }
void btBackwardEulerObjective::applyForce(TVStack& force, bool setZero) void btDeformableBackwardEulerObjective::applyForce(TVStack& force, bool setZero)
{ {
size_t counter = 0; size_t counter = 0;
for (int i = 0; i < m_softBodies.size(); ++i) for (int i = 0; i < m_softBodies.size(); ++i)
@@ -96,7 +93,7 @@ void btBackwardEulerObjective::applyForce(TVStack& force, bool setZero)
} }
} }
void btBackwardEulerObjective::computeResidual(btScalar dt, TVStack &residual) const void btDeformableBackwardEulerObjective::computeResidual(btScalar dt, TVStack &residual) const
{ {
// add implicit force // add implicit force
for (int i = 0; i < m_lf.size(); ++i) for (int i = 0; i < m_lf.size(); ++i)
@@ -105,7 +102,7 @@ void btBackwardEulerObjective::computeResidual(btScalar dt, TVStack &residual) c
} }
} }
btScalar btBackwardEulerObjective::computeNorm(const TVStack& residual) const btScalar btDeformableBackwardEulerObjective::computeNorm(const TVStack& residual) const
{ {
btScalar norm_squared = 0; btScalar norm_squared = 0;
for (int i = 0; i < residual.size(); ++i) for (int i = 0; i < residual.size(); ++i)
@@ -115,14 +112,14 @@ btScalar btBackwardEulerObjective::computeNorm(const TVStack& residual) const
return std::sqrt(norm_squared+SIMD_EPSILON); return std::sqrt(norm_squared+SIMD_EPSILON);
} }
void btBackwardEulerObjective::applyExplicitForce(TVStack& force) void btDeformableBackwardEulerObjective::applyExplicitForce(TVStack& force)
{ {
for (int i = 0; i < m_lf.size(); ++i) for (int i = 0; i < m_lf.size(); ++i)
m_lf[i]->addScaledExplicitForce(m_dt, force); m_lf[i]->addScaledExplicitForce(m_dt, force);
applyForce(force, true); applyForce(force, true);
} }
void btBackwardEulerObjective::initialGuess(TVStack& dv, const TVStack& residual) void btDeformableBackwardEulerObjective::initialGuess(TVStack& dv, const TVStack& residual)
{ {
size_t counter = 0; size_t counter = 0;
for (int i = 0; i < m_softBodies.size(); ++i) for (int i = 0; i < m_softBodies.size(); ++i)

100
src/BulletSoftBody/btDeformableBackwardEulerObjective.h Normal file
View File

@@ -0,0 +1,100 @@
//
// btDeformableBackwardEulerObjective.h
// BulletSoftBody
//
// Created by Xuchen Han on 7/1/19.
//
#ifndef BT_BACKWARD_EULER_OBJECTIVE_H
#define BT_BACKWARD_EULER_OBJECTIVE_H
#include <functional>
#include "btConjugateGradient.h"
#include "btDeformableLagrangianForce.h"
#include "btDeformableMassSpringForce.h"
#include "btDeformableGravityForce.h"
#include "btDeformableContactProjection.h"
#include "btPreconditioner.h"
#include "btDeformableRigidDynamicsWorld.h"
class btDeformableRigidDynamicsWorld;
class btDeformableBackwardEulerObjective
{
public:
using TVStack = btAlignedObjectArray<btVector3>;
btScalar m_dt;
btDeformableRigidDynamicsWorld* m_world;
btAlignedObjectArray<btDeformableLagrangianForce*> m_lf;
btAlignedObjectArray<btSoftBody *>& m_softBodies;
Preconditioner* m_preconditioner;
btDeformableContactProjection projection;
const TVStack& m_backupVelocity;
btDeformableBackwardEulerObjective(btAlignedObjectArray<btSoftBody *>& softBodies, const TVStack& backup_v);
virtual ~btDeformableBackwardEulerObjective() {}
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;
// add explicit force to the velocity
void applyExplicitForce(TVStack& force);
// 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;
// set initial guess for CG solve
void initialGuess(TVStack& dv, const TVStack& residual);
// reset data structure
void reinitialize(bool nodeUpdated);
void setDt(btScalar dt);
// enforce constraints in CG solve
void enforceConstraint(TVStack& x)
{
projection.enforceConstraint(x);
updateVelocity(x);
}
// add dv to velocity
void updateVelocity(const TVStack& dv);
//set constraints as projections
void setConstraints()
{
projection.setConstraints();
}
// update the projections and project the residual
void project(TVStack& r)
{
projection.update();
projection.project(r);
}
// perform precondition M^(-1) x = b
void precondition(const TVStack& x, TVStack& b)
{
m_preconditioner->operator()(x,b);
}
virtual void setWorld(btDeformableRigidDynamicsWorld* world)
{
m_world = world;
projection.setWorld(world);
}
};
#endif /* btBackwardEulerObjective_h */

184
src/BulletSoftBody/btDeformableBodySolver.cpp
View File

@@ -10,11 +10,9 @@
btDeformableBodySolver::btDeformableBodySolver() btDeformableBodySolver::btDeformableBodySolver()
: m_numNodes(0) : m_numNodes(0)
, m_solveIterations(1) , cg(10)
, m_impulseIterations(1)
, m_world(nullptr)
{ {
m_objective = new btBackwardEulerObjective(m_softBodySet, m_backupVelocity); m_objective = new btDeformableBackwardEulerObjective(m_softBodySet, m_backupVelocity);
} }
btDeformableBodySolver::~btDeformableBodySolver() btDeformableBodySolver::~btDeformableBodySolver()
@@ -22,124 +20,31 @@ btDeformableBodySolver::~btDeformableBodySolver()
delete m_objective; delete m_objective;
} }
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);
btScalar dp = btMin((btDot(c.m_node->m_x, cti.m_normal) + cti.m_offset), mrg);
// dp += 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)
{
bool two_way = false;
if (two_way)
{
c.m_node->m_x -= impulse * c.m_c2;
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);
}
}
}
else
c.m_node->m_x -= dp * cti.m_normal * c.m_c4;
}
}
}
}
}
void btDeformableBodySolver::solveConstraints(float solverdt) void btDeformableBodySolver::solveConstraints(float solverdt)
{ {
m_dt = solverdt; m_objective->setDt(solverdt);
bool nodeUpdated = updateNodes();
reinitialize(nodeUpdated);
// apply explicit force
m_objective->applyExplicitForce(m_residual);
// add constraints to the solver // add constraints to the solver
setConstraints(); setConstraints();
// save v_{n+1}^* velocity after explicit forces
backupVelocity(); backupVelocity();
for (int i = 0; i < m_solveIterations; ++i)
{
m_objective->computeResidual(solverdt, m_residual); m_objective->computeResidual(solverdt, m_residual);
m_objective->initialGuess(m_dv, m_residual); // m_objective->initialGuess(m_dv, m_residual);
m_objective->computeStep(m_dv, m_residual, solverdt); computeStep(m_dv, m_residual);
updateVelocity(); updateVelocity();
} }
advect(solverdt);
postStabilize(); void btDeformableBodySolver::computeStep(TVStack& dv, const TVStack& residual)
{
btScalar tolerance = std::numeric_limits<float>::epsilon()* 1024 * m_objective->computeNorm(residual);
cg.solve(*m_objective, dv, residual, tolerance);
} }
void btDeformableBodySolver::reinitialize(bool nodeUpdated) void btDeformableBodySolver::reinitialize(const btAlignedObjectArray<btSoftBody *>& softBodies)
{ {
m_softBodySet.copyFromArray(softBodies);
bool nodeUpdated = updateNodes();
if (nodeUpdated) if (nodeUpdated)
{ {
m_dv.resize(m_numNodes); m_dv.resize(m_numNodes);
@@ -161,7 +66,6 @@ void btDeformableBodySolver::setConstraints()
void btDeformableBodySolver::setWorld(btDeformableRigidDynamicsWorld* world) void btDeformableBodySolver::setWorld(btDeformableRigidDynamicsWorld* world)
{ {
m_world = world;
m_objective->setWorld(world); m_objective->setWorld(world);
} }
@@ -180,20 +84,6 @@ void btDeformableBodySolver::updateVelocity()
} }
} }
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() void btDeformableBodySolver::backupVelocity()
{ {
// serial implementation // serial implementation
@@ -231,11 +121,53 @@ void btDeformableBodySolver::predictMotion(float solverdt)
if (psb->isActive()) if (psb->isActive())
{ {
psb->predictMotion(solverdt); // apply explicit forces to velocity
m_objective->applyExplicitForce(m_residual);
// predict motion for collision detection
predictDeformableMotion(psb, solverdt);
} }
} }
} }
void btDeformableBodySolver::predictDeformableMotion(btSoftBody* psb, btScalar dt)
{
int i, ni;
/* Prepare */
psb->m_sst.sdt = dt * psb->m_cfg.timescale;
psb->m_sst.isdt = 1 / psb->m_sst.sdt;
psb->m_sst.velmrg = psb->m_sst.sdt * 3;
psb->m_sst.radmrg = psb->getCollisionShape()->getMargin();
psb->m_sst.updmrg = psb->m_sst.radmrg * (btScalar)0.25;
/* Integrate */
for (i = 0, ni = psb->m_nodes.size(); i < ni; ++i)
{
btSoftBody::Node& n = psb->m_nodes[i];
n.m_q = n.m_x;
n.m_x += n.m_v * dt;
}
/* Bounds */
psb->updateBounds();
/* Nodes */
ATTRIBUTE_ALIGNED16(btDbvtVolume)
vol;
for (i = 0, ni = psb->m_nodes.size(); i < ni; ++i)
{
btSoftBody::Node& n = psb->m_nodes[i];
vol = btDbvtVolume::FromCR(n.m_x, psb->m_sst.radmrg);
psb->m_ndbvt.update(n.m_leaf,
vol,
n.m_v * psb->m_sst.velmrg,
psb->m_sst.updmrg);
}
/* Clear contacts */
psb->m_rcontacts.resize(0);
psb->m_scontacts.resize(0);
/* Optimize dbvt's */
psb->m_ndbvt.optimizeIncremental(1);
}
void btDeformableBodySolver::updateSoftBodies() void btDeformableBodySolver::updateSoftBodies()
{ {
for (int i = 0; i < m_softBodySet.size(); i++) for (int i = 0; i < m_softBodySet.size(); i++)
@@ -243,7 +175,7 @@ void btDeformableBodySolver::updateSoftBodies()
btSoftBody *psb = (btSoftBody *)m_softBodySet[i]; btSoftBody *psb = (btSoftBody *)m_softBodySet[i];
if (psb->isActive()) if (psb->isActive())
{ {
psb->integrateMotion(); // normal is updated here psb->updateNormals(); // normal is updated here
} }
} }
} }

35
src/BulletSoftBody/btDeformableBodySolver.h
View File

@@ -10,31 +10,27 @@
#include <iostream> #include <iostream>
#include "btSoftBodySolvers.h" #include "btSoftBodySolvers.h"
#include "btBackwardEulerObjective.h" #include "btDeformableBackwardEulerObjective.h"
#include "btDeformableRigidDynamicsWorld.h" #include "btDeformableRigidDynamicsWorld.h"
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h" #include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
struct btCollisionObjectWrapper; struct btCollisionObjectWrapper;
class btBackwardEulerObjective; class btDeformableBackwardEulerObjective;
class btDeformableRigidDynamicsWorld; class btDeformableRigidDynamicsWorld;
class btDeformableBodySolver : public btSoftBodySolver class btDeformableBodySolver : public btSoftBodySolver
{ {
using TVStack = btAlignedObjectArray<btVector3>; using TVStack = btAlignedObjectArray<btVector3>;
protected: protected:
/** Variable to define whether we need to update solver constants on the next iteration */
bool m_updateSolverConstants;
int m_numNodes; int m_numNodes;
TVStack m_dv; TVStack m_dv;
TVStack m_residual; TVStack m_residual;
btAlignedObjectArray<btSoftBody *> m_softBodySet; btAlignedObjectArray<btSoftBody *> m_softBodySet;
btBackwardEulerObjective* m_objective; btDeformableBackwardEulerObjective* m_objective;
int m_solveIterations;
int m_impulseIterations;
btDeformableRigidDynamicsWorld* m_world;
btAlignedObjectArray<btVector3> m_backupVelocity; btAlignedObjectArray<btVector3> m_backupVelocity;
btScalar m_dt; btScalar m_dt;
btConjugateGradient<btDeformableBackwardEulerObjective> cg;
public: public:
btDeformableBodySolver(); btDeformableBodySolver();
@@ -46,29 +42,19 @@ public:
return DEFORMABLE_SOLVER; return DEFORMABLE_SOLVER;
} }
virtual bool checkInitialized()
{
return true;
}
virtual void updateSoftBodies(); virtual void updateSoftBodies();
virtual void optimize(btAlignedObjectArray<btSoftBody *> &softBodies, bool forceUpdate = false)
{
m_softBodySet.copyFromArray(softBodies);
}
virtual void copyBackToSoftBodies(bool bMove = true) {} virtual void copyBackToSoftBodies(bool bMove = true) {}
void extracted(float solverdt);
virtual void solveConstraints(float solverdt); virtual void solveConstraints(float solverdt);
void postStabilize(); void reinitialize(const btAlignedObjectArray<btSoftBody *>& softBodies);
void reinitialize(bool nodeUpdated);
void setConstraints(); void setConstraints();
void advect(btScalar dt); void predictDeformableMotion(btSoftBody* psb, btScalar dt);
void backupVelocity(); void backupVelocity();
@@ -76,6 +62,8 @@ public:
bool updateNodes(); bool updateNodes();
void computeStep(TVStack& dv, const TVStack& residual);
virtual void predictMotion(float solverdt); virtual void predictMotion(float solverdt);
virtual void copySoftBodyToVertexBuffer(const btSoftBody *const softBody, btVertexBufferDescriptor *vertexBuffer) {} virtual void copySoftBodyToVertexBuffer(const btSoftBody *const softBody, btVertexBufferDescriptor *vertexBuffer) {}
@@ -88,7 +76,8 @@ public:
virtual void processCollision(btSoftBody * softBody, btSoftBody * otherSoftBody) { virtual void processCollision(btSoftBody * softBody, btSoftBody * otherSoftBody) {
softBody->defaultCollisionHandler(otherSoftBody); softBody->defaultCollisionHandler(otherSoftBody);
} }
virtual void optimize(btAlignedObjectArray<btSoftBody *> &softBodies, bool forceUpdate = false){}
virtual bool checkInitialized(){return true;}
virtual void setWorld(btDeformableRigidDynamicsWorld* world); virtual void setWorld(btDeformableRigidDynamicsWorld* world);
}; };

488
src/BulletSoftBody/btDeformableContactProjection.cpp Normal file
View File

@@ -0,0 +1,488 @@
//
// btDeformableContactProjection.cpp
// BulletSoftBody
//
// Created by Xuchen Han on 7/4/19.
//
#include "btDeformableContactProjection.h"
#include "btDeformableRigidDynamicsWorld.h"
#include <algorithm>
static void findJacobian(const btMultiBodyLinkCollider* multibodyLinkCol,
btMultiBodyJacobianData& jacobianData,
const btVector3& contact_point,
const btVector3& dir)
{
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, contact_point, dir, jac, jacobianData.scratch_r, jacobianData.scratch_v, jacobianData.scratch_m);
multibodyLinkCol->m_multiBody->calcAccelerationDeltasMultiDof(&jacobianData.m_jacobians[0], &jacobianData.m_deltaVelocitiesUnitImpulse[0], jacobianData.scratch_r, jacobianData.scratch_v);
}
static btVector3 generateUnitOrthogonalVector(const btVector3& u)
{
btScalar ux = u.getX();
btScalar uy = u.getY();
btScalar uz = u.getZ();
btScalar ax = std::abs(ux);
btScalar ay = std::abs(uy);
btScalar az = std::abs(uz);
btVector3 v;
if (ax <= ay && ax <= az)
v = btVector3(0, -uz, uy);
else if (ay <= ax && ay <= az)
v = btVector3(-uz, 0, ux);
else
v = btVector3(-uy, ux, 0);
v.normalize();
return v;
}
void btDeformableContactProjection::update()
{
///solve rigid body constraints
m_world->getSolverInfo().m_numIterations = 10;
m_world->btMultiBodyDynamicsWorld::solveConstraints(m_world->getSolverInfo());
// loop through constraints to set constrained values
for (auto& it : m_constraints)
{
btAlignedObjectArray<DeformableFrictionConstraint>& frictions = m_frictions[it.first];
btAlignedObjectArray<DeformableContactConstraint>& constraints = it.second;
for (int i = 0; i < constraints.size(); ++i)
{
DeformableContactConstraint& constraint = constraints[i];
DeformableFrictionConstraint& friction = frictions[i];
for (int j = 0; j < constraint.m_contact.size(); ++j)
{
if (constraint.m_contact[j] == nullptr)
{
// nothing needs to be done for dirichelet constraints
continue;
}
const btSoftBody::RContact* c = constraint.m_contact[j];
const btSoftBody::sCti& cti = c->m_cti;
// normal jacobian is precompute but tangent jacobian is not
const btMultiBodyJacobianData& jacobianData_normal = constraint.m_normal_jacobian[j];
const btMultiBodyJacobianData& jacobianData_complementary = friction.m_complementary_jacobian[j];
if (cti.m_colObj->hasContactResponse())
{
btVector3 va(0, 0, 0);
btRigidBody* rigidCol = 0;
btMultiBodyLinkCollider* multibodyLinkCol = 0;
const btScalar* deltaV_normal;
// 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;
const btScalar* jac_normal = &jacobianData_normal.m_jacobians[0];
deltaV_normal = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
// add in the normal component of the va
btScalar vel = 0.0;
for (int k = 0; k < ndof; ++k)
{
vel += multibodyLinkCol->m_multiBody->getVelocityVector()[k] * jac_normal[k];
}
va = cti.m_normal * vel * m_dt;
// add in complementary direction of va
const btScalar* jac_complementary = &jacobianData_complementary.m_jacobians[0];
vel = 0.0;
for (int k = 0; k < ndof; ++k)
{
vel += multibodyLinkCol->m_multiBody->getVelocityVector()[k] * jac_complementary[k];
}
va += friction.m_complementaryDirection[j] * 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);
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
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();
if (accumulated_normal < 0)
{
friction.m_direction[j] = -local_tangent_dir;
// 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;
}
else
{
friction.m_static[j] = true;
friction.m_impulse[j] = tangent_norm;
}
}
else
{
friction.m_released[j] = true;
friction.m_static[j] = false;
friction.m_impulse[j] = 0;
friction.m_direction[j] = btVector3(0,0,0);
}
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]);
// TODO cleanup
if (1) // in the same CG solve, the set of constraits doesn't change
{
// 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;
btScalar dvn = -accumulated_normal * c->m_c2/m_dt;
// the following is equivalent
/*
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);
*/
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 + incremental_tangent;
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 = 1;
multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_normal, -impulse_normal.length() * multiplier);
if (incremental_tangent.norm() > SIMD_EPSILON)
{
btMultiBodyJacobianData jacobian_tangent;
btVector3 tangent = incremental_tangent.normalized();
findJacobian(multibodyLinkCol, jacobian_tangent, c->m_node->m_x, tangent);
const btScalar* deltaV_tangent = &jacobian_tangent.m_deltaVelocitiesUnitImpulse[0];
multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_tangent, incremental_tangent.length() * multiplier);
}
}
}
}
}
}
}
}
}
void btDeformableContactProjection::setConstraints()
{
// set Dirichlet constraint
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
if (psb->m_nodes[j].m_im == 0)
{
btAlignedObjectArray<DeformableContactConstraint> c;
c.push_back(DeformableContactConstraint(btVector3(1,0,0)));
c.push_back(DeformableContactConstraint(btVector3(0,1,0)));
c.push_back(DeformableContactConstraint(btVector3(0,0,1)));
m_constraints[&(psb->m_nodes[j])] = c;
btAlignedObjectArray<DeformableFrictionConstraint> f;
f.push_back(DeformableFrictionConstraint());
f.push_back(DeformableFrictionConstraint());
f.push_back(DeformableFrictionConstraint());
m_frictions[&(psb->m_nodes[j])] = f;
}
}
}
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
btMultiBodyJacobianData jacobianData_normal;
btMultiBodyJacobianData jacobianData_complementary;
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;
// 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)
{
findJacobian(multibodyLinkCol, jacobianData_normal, c.m_node->m_x, cti.m_normal);
btScalar vel = 0.0;
const btScalar* jac = &jacobianData_normal.m_jacobians[0];
const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
for (int j = 0; j < ndof; ++j)
{
vel += multibodyLinkCol->m_multiBody->getVelocityVector()[j] * jac[j];
std::cout << multibodyLinkCol->m_multiBody->getVelocityVector()[j] << std::endl;
std::cout << jac[j] << std::endl;
}
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)
{
// find complementary jacobian
btVector3 complementaryDirection;
if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
{
multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
if (multibodyLinkCol)
{
complementaryDirection = generateUnitOrthogonalVector(cti.m_normal);
findJacobian(multibodyLinkCol, jacobianData_complementary, c.m_node->m_x, complementaryDirection);
}
}
if (m_constraints.find(c.m_node) == m_constraints.end())
{
btAlignedObjectArray<DeformableContactConstraint> constraints;
constraints.push_back(DeformableContactConstraint(c, jacobianData_normal));
m_constraints[c.m_node] = constraints;
btAlignedObjectArray<DeformableFrictionConstraint> frictions;
frictions.push_back(DeformableFrictionConstraint(complementaryDirection, jacobianData_complementary));
m_frictions[c.m_node] = frictions;
}
else
{
// group colinear constraints into one
const btScalar angle_epsilon = 0.015192247; // less than 10 degree
bool merged = false;
btAlignedObjectArray<DeformableContactConstraint>& constraints = m_constraints[c.m_node];
btAlignedObjectArray<DeformableFrictionConstraint>& frictions = m_frictions[c.m_node];
for (int j = 0; j < constraints.size(); ++j)
{
const btAlignedObjectArray<btVector3>& dirs = constraints[j].m_direction;
btScalar dot_prod = dirs[0].dot(cti.m_normal);
if (std::abs(std::abs(dot_prod) - 1) < angle_epsilon)
{
// group the constraints
constraints[j].append(c, jacobianData_normal);
// push in an empty friction
frictions[j].append();
frictions[j].addJacobian(complementaryDirection, jacobianData_complementary);
merged = true;
break;
}
}
const int dim = 3;
// hard coded no more than 3 constraint directions
if (!merged && constraints.size() < dim)
{
constraints.push_back(DeformableContactConstraint(c, jacobianData_normal));
frictions.push_back(DeformableFrictionConstraint(complementaryDirection, jacobianData_complementary));
}
}
}
}
}
}
}
void btDeformableContactProjection::enforceConstraint(TVStack& x)
{
const int dim = 3;
for (auto& it : m_constraints)
{
const btAlignedObjectArray<DeformableContactConstraint>& constraints = it.second;
size_t i = m_indices[it.first];
const btAlignedObjectArray<DeformableFrictionConstraint>& 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 DeformableFrictionConstraint& 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 btDeformableContactProjection::project(TVStack& x)
{
const int dim = 3;
for (auto& it : m_constraints)
{
const btAlignedObjectArray<DeformableContactConstraint>& constraints = it.second;
size_t i = m_indices[it.first];
btAlignedObjectArray<DeformableFrictionConstraint>& 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)
{
DeformableFrictionConstraint& 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)
{
DeformableFrictionConstraint& 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];
}
}
}
}
}
}
void btDeformableContactProjection::reinitialize(bool nodeUpdated)
{
btCGProjection::reinitialize(nodeUpdated);
m_constraints.clear();
m_frictions.clear();
}

17
src/BulletSoftBody/btContactProjection.h → src/BulletSoftBody/btDeformableContactProjection.h
View File

@@ -1,5 +1,5 @@
// //
// btContactProjection.h // btDeformableContactProjection.h
// BulletSoftBody // BulletSoftBody
// //
// Created by Xuchen Han on 7/4/19. // Created by Xuchen Han on 7/4/19.
@@ -12,27 +12,32 @@
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h" #include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
#include <iostream> #include <iostream>
class btContactProjection : public btCGProjection class btDeformableContactProjection : public btCGProjection
{ {
public: public:
btContactProjection(btAlignedObjectArray<btSoftBody *>& softBodies, const btScalar& dt) std::unordered_map<btSoftBody::Node *, btAlignedObjectArray<DeformableContactConstraint> > m_constraints;
std::unordered_map<btSoftBody::Node *, btAlignedObjectArray<DeformableFrictionConstraint> > m_frictions;
btDeformableContactProjection(btAlignedObjectArray<btSoftBody *>& softBodies, const btScalar& dt)
: btCGProjection(softBodies, dt) : btCGProjection(softBodies, dt)
{ {
} }
virtual ~btContactProjection() virtual ~btDeformableContactProjection()
{ {
} }
// apply the constraints to the rhs // apply the constraints to the rhs
virtual void operator()(TVStack& x); virtual void project(TVStack& x);
// apply constraints to x in Ax=b // apply constraints to x in Ax=b
virtual void enforceConstraint(TVStack& x); virtual void enforceConstraint(TVStack& x);
// update the constraints // update the constraints
virtual void update(const TVStack& dv, const TVStack& backupVelocity); virtual void update();
virtual void setConstraints(); virtual void setConstraints();
virtual void reinitialize(bool nodeUpdated);
}; };
#endif /* btContactProjection_h */ #endif /* btContactProjection_h */

60
src/BulletSoftBody/btDeformableGravityForce.h Normal file
View File

@@ -0,0 +1,60 @@
//
// btDeformableGravityForce.h
// BulletSoftBody
//
// Created by Xuchen Han on 7/21/19.
//
#ifndef BT_DEFORMABLE_GRAVITY_FORCE_H
#define BT_DEFORMABLE_GRAVITY_FORCE_H
#include "btDeformableLagrangianForce.h"
class btDeformableGravityForce : public btDeformableLagrangianForce
{
public:
using TVStack = btDeformableLagrangianForce::TVStack;
btVector3 m_gravity;
btDeformableGravityForce(const btAlignedObjectArray<btSoftBody *>& softBodies, const btVector3& g) : btDeformableLagrangianForce(softBodies), m_gravity(g)
{
}
virtual void addScaledImplicitForce(btScalar scale, TVStack& force)
{
}
virtual void addScaledExplicitForce(btScalar scale, TVStack& force)
{
addScaledGravityForce(scale, force);
}
virtual void addScaledForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
{
}
virtual void addScaledGravityForce(btScalar scale, TVStack& force)
{
int numNodes = getNumNodes();
btAssert(numNodes == force.size())
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
btSoftBody::Node& n = psb->m_nodes[j];
size_t id = m_indices[&n];
btScalar mass = (n.m_im == 0) ? 0 : 1. / n.m_im;
btVector3 scaled_force = scale * m_gravity * mass;
force[id] += scaled_force;
}
}
}
};
#endif /* BT_DEFORMABLE_GRAVITY_FORCE_H */

30
src/BulletSoftBody/btLagrangianForce.h → src/BulletSoftBody/btDeformableLagrangianForce.h
View File

@@ -1,35 +1,33 @@
// //
// btLagrangianForce.h // btDeformableLagrangianForce.h
// BulletSoftBody // BulletSoftBody
// //
// Created by Xuchen Han on 7/1/19. // Created by Xuchen Han on 7/1/19.
// //
#ifndef BT_LAGRANGIAN_FORCE_H #ifndef BT_DEFORMABLE_LAGRANGIAN_FORCE_H
#define BT_LAGRANGIAN_FORCE_H #define BT_DEFORMABLE_LAGRANGIAN_FORCE_H
#include "btSoftBody.h" #include "btSoftBody.h"
#include <unordered_map> #include <unordered_map>
class btLagrangianForce class btDeformableLagrangianForce
{ {
public: public:
using TVStack = btAlignedObjectArray<btVector3>; using TVStack = btAlignedObjectArray<btVector3>;
const btAlignedObjectArray<btSoftBody *>& m_softBodies; const btAlignedObjectArray<btSoftBody *>& m_softBodies;
std::unordered_map<btSoftBody::Node *, size_t> m_indices; std::unordered_map<btSoftBody::Node *, size_t> m_indices;
btLagrangianForce(const btAlignedObjectArray<btSoftBody *>& softBodies) btDeformableLagrangianForce(const btAlignedObjectArray<btSoftBody *>& softBodies)
: m_softBodies(softBodies) : m_softBodies(softBodies)
{ {
} }
virtual ~btLagrangianForce(){} virtual ~btDeformableLagrangianForce(){}
virtual void addScaledImplicitForce(btScalar scale, TVStack& force) = 0; virtual void addScaledImplicitForce(btScalar scale, TVStack& force) = 0;
virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) = 0; virtual void addScaledForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) = 0;
virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) = 0;
virtual void addScaledExplicitForce(btScalar scale, TVStack& force) = 0; virtual void addScaledExplicitForce(btScalar scale, TVStack& force) = 0;
@@ -39,7 +37,7 @@ public:
updateId(); updateId();
} }
void updateId() virtual void updateId()
{ {
size_t index = 0; size_t index = 0;
for (int i = 0; i < m_softBodies.size(); ++i) for (int i = 0; i < m_softBodies.size(); ++i)
@@ -51,5 +49,15 @@ public:
} }
} }
} }
virtual int getNumNodes()
{
int numNodes = 0;
for (int i = 0; i < m_softBodies.size(); ++i)
{
numNodes += m_softBodies[i]->m_nodes.size();
}
return numNodes;
}
}; };
#endif /* btLagrangianForce_h */ #endif /* BT_DEFORMABLE_LAGRANGIAN_FORCE */

54
src/BulletSoftBody/btMassSpring.h → src/BulletSoftBody/btDeformableMassSpringForce.h
View File

@@ -1,20 +1,20 @@
// //
// btMassSpring.h // btDeformableMassSpringForce.h
// BulletSoftBody // BulletSoftBody
// //
// Created by Chuyuan Fu on 7/1/19. // Created by Xuchen Gan on 7/1/19.
// //
#ifndef BT_MASS_SPRING_H #ifndef BT_MASS_SPRING_H
#define BT_MASS_SPRING_H #define BT_MASS_SPRING_H
#include "btLagrangianForce.h" #include "btDeformableLagrangianForce.h"
class btMassSpring : public btLagrangianForce class btDeformableMassSpringForce : public btDeformableLagrangianForce
{ {
public: public:
using TVStack = btLagrangianForce::TVStack; using TVStack = btDeformableLagrangianForce::TVStack;
btMassSpring(const btAlignedObjectArray<btSoftBody *>& softBodies) : btLagrangianForce(softBodies) btDeformableMassSpringForce(const btAlignedObjectArray<btSoftBody *>& softBodies) : btDeformableLagrangianForce(softBodies)
{ {
} }
@@ -72,10 +72,6 @@ public:
size_t id2 = m_indices[node2]; size_t id2 = m_indices[node2];
// elastic force // elastic force
// fully implicit
// btVector3 dir = (node2->m_x - node1->m_x);
// explicit elastic force // explicit elastic force
btVector3 dir = (node2->m_q - node1->m_q); btVector3 dir = (node2->m_q - node1->m_q);
btVector3 dir_normalized = dir.normalized(); btVector3 dir_normalized = dir.normalized();
@@ -86,33 +82,7 @@ public:
} }
} }
virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) virtual void addScaledForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
{
int numNodes = getNumNodes();
btAssert(numNodes == dx.size());
btAssert(numNodes == df.size());
// implicit elastic force differential
for (int i = 0; i < m_softBodies.size(); ++i)
{
const btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_links.size(); ++j)
{
const auto& link = psb->m_links[j];
const auto node1 = link.m_n[0];
const auto node2 = link.m_n[1];
btScalar kLST = link.Feature::m_material->m_kLST;
size_t id1 = m_indices[node1];
size_t id2 = m_indices[node2];
btVector3 local_scaled_df = scale * kLST * (dx[id2] - dx[id1]);
df[id1] += local_scaled_df;
df[id2] -= local_scaled_df;
}
}
}
virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
{ {
// implicit damping force differential // implicit damping force differential
for (int i = 0; i < m_softBodies.size(); ++i) for (int i = 0; i < m_softBodies.size(); ++i)
@@ -132,16 +102,6 @@ public:
} }
} }
} }
int getNumNodes()
{
int numNodes = 0;
for (int i = 0; i < m_softBodies.size(); ++i)
{
numNodes += m_softBodies[i]->m_nodes.size();
}
return numNodes;
}
}; };
#endif /* btMassSpring_h */ #endif /* btMassSpring_h */

148
src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp
View File

@@ -12,87 +12,78 @@
void btDeformableRigidDynamicsWorld::internalSingleStepSimulation(btScalar timeStep) void btDeformableRigidDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
{ {
m_internalTime += timeStep; reinitialize(timeStep);
// Let the solver grab the soft bodies and if necessary optimize for it
m_deformableBodySolver->optimize(m_softBodies);
if (!m_deformableBodySolver->checkInitialized()) // add gravity to velocity of rigid and multi bodys
{ applyRigidBodyGravity(timeStep);
btAssert("Solver initialization failed\n");
}
// from btDiscreteDynamicsWorld singleStepSimulation ///apply gravity and explicit force to velocity, predict motion
if (0 != m_internalPreTickCallback)
{
(*m_internalPreTickCallback)(this, timeStep);
}
///apply gravity, predict motion
predictUnconstraintMotion(timeStep); predictUnconstraintMotion(timeStep);
btDispatcherInfo& dispatchInfo = btMultiBodyDynamicsWorld::getDispatchInfo();
dispatchInfo.m_timeStep = timeStep;
dispatchInfo.m_stepCount = 0;
dispatchInfo.m_debugDraw = btMultiBodyDynamicsWorld::getDebugDrawer();
// only used in CCD
// createPredictiveContacts(timeStep);
///perform collision detection ///perform collision detection
btMultiBodyDynamicsWorld::performDiscreteCollisionDetection(); btMultiBodyDynamicsWorld::performDiscreteCollisionDetection();
btMultiBodyDynamicsWorld::calculateSimulationIslands(); btMultiBodyDynamicsWorld::calculateSimulationIslands();
btMultiBodyDynamicsWorld::getSolverInfo().m_timeStep = timeStep; beforeSolverCallbacks(timeStep);
if (0 != m_internalTickCallback)
{
(*m_internalTickCallback)(this, timeStep);
}
// TODO: This is an ugly hack to get the desired gravity behavior.
// gravity is applied in stepSimulation and then cleared here and then applied here and then cleared here again
// so that 1) gravity is applied to velocity before constraint solve and 2) gravity is applied in each substep
// when there are multiple substeps
clearForces();
clearMultiBodyForces();
btMultiBodyDynamicsWorld::applyGravity();
// integrate rigid body gravity
for (int i = 0; i < m_nonStaticRigidBodies.size(); ++i)
{
btRigidBody* rb = m_nonStaticRigidBodies[i];
rb->integrateVelocities(timeStep);
}
// integrate multibody gravity
btMultiBodyDynamicsWorld::solveExternalForces(btMultiBodyDynamicsWorld::getSolverInfo());
clearForces();
clearMultiBodyForces();
for (int i = 0; i < before_solver_callbacks.size(); ++i)
before_solver_callbacks[i](m_internalTime, this);
///solve deformable bodies constraints ///solve deformable bodies constraints
solveDeformableBodiesConstraints(timeStep); solveDeformableBodiesConstraints(timeStep);
positionCorrection();
integrateTransforms(timeStep);
//integrate transforms
btMultiBodyDynamicsWorld::integrateTransforms(timeStep);
///update vehicle simulation ///update vehicle simulation
btMultiBodyDynamicsWorld::updateActions(timeStep); btMultiBodyDynamicsWorld::updateActions(timeStep);
btMultiBodyDynamicsWorld::updateActivationState(timeStep); btMultiBodyDynamicsWorld::updateActivationState(timeStep);
///update soft bodies
m_deformableBodySolver->updateSoftBodies();
clearForces();
// End solver-wise simulation step // End solver-wise simulation step
// /////////////////////////////// // ///////////////////////////////
} }
void btDeformableRigidDynamicsWorld::positionCorrection()
{
// perform position correction for all geometric collisions
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
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())
{
btScalar dp = btMin((btDot(c.m_node->m_x, cti.m_normal) + cti.m_offset), mrg);
if (dp < 0)
{
// m_c4 is the collision hardness
c.m_node->m_q -= dp * cti.m_normal * c.m_c4;
}
}
}
}
}
void btDeformableRigidDynamicsWorld::integrateTransforms(btScalar dt)
{
btMultiBodyDynamicsWorld::integrateTransforms(dt);
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[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 btDeformableRigidDynamicsWorld::solveDeformableBodiesConstraints(btScalar timeStep) void btDeformableRigidDynamicsWorld::solveDeformableBodiesConstraints(btScalar timeStep)
{ {
m_deformableBodySolver->solveConstraints(timeStep); m_deformableBodySolver->solveConstraints(timeStep);
@@ -117,4 +108,45 @@ void btDeformableRigidDynamicsWorld::predictUnconstraintMotion(btScalar timeStep
m_deformableBodySolver->predictMotion(float(timeStep)); m_deformableBodySolver->predictMotion(float(timeStep));
} }
void btDeformableRigidDynamicsWorld::reinitialize(btScalar timeStep)
{
m_internalTime += timeStep;
m_deformableBodySolver->reinitialize(m_softBodies);
btDispatcherInfo& dispatchInfo = btMultiBodyDynamicsWorld::getDispatchInfo();
dispatchInfo.m_timeStep = timeStep;
dispatchInfo.m_stepCount = 0;
dispatchInfo.m_debugDraw = btMultiBodyDynamicsWorld::getDebugDrawer();
btMultiBodyDynamicsWorld::getSolverInfo().m_timeStep = timeStep;
}
void btDeformableRigidDynamicsWorld::applyRigidBodyGravity(btScalar timeStep)
{
// TODO: This is an ugly hack to get the desired gravity behavior.
// gravity is applied in stepSimulation and then cleared here and then applied here and then cleared here again
// so that 1) gravity is applied to velocity before constraint solve and 2) gravity is applied in each substep
// when there are multiple substeps
clearForces();
clearMultiBodyForces();
btMultiBodyDynamicsWorld::applyGravity();
// integrate rigid body gravity
for (int i = 0; i < m_nonStaticRigidBodies.size(); ++i)
{
btRigidBody* rb = m_nonStaticRigidBodies[i];
rb->integrateVelocities(timeStep);
}
// integrate multibody gravity
btMultiBodyDynamicsWorld::solveExternalForces(btMultiBodyDynamicsWorld::getSolverInfo());
clearForces();
clearMultiBodyForces();
}
void btDeformableRigidDynamicsWorld::beforeSolverCallbacks(btScalar timeStep)
{
if (0 != m_internalTickCallback)
{
(*m_internalTickCallback)(this, timeStep);
}
for (int i = 0; i < m_beforeSolverCallbacks.size(); ++i)
m_beforeSolverCallbacks[i](m_internalTime, this);
}

20
src/BulletSoftBody/btDeformableRigidDynamicsWorld.h
View File

@@ -17,15 +17,15 @@
#define BT_DEFORMABLE_RIGID_DYNAMICS_WORLD_H #define BT_DEFORMABLE_RIGID_DYNAMICS_WORLD_H
#include "btSoftMultiBodyDynamicsWorld.h" #include "btSoftMultiBodyDynamicsWorld.h"
#include "btLagrangianForce.h" #include "btDeformableLagrangianForce.h"
#include "btMassSpring.h" #include "btDeformableMassSpringForce.h"
#include "btDeformableBodySolver.h" #include "btDeformableBodySolver.h"
#include "btSoftBodyHelpers.h" #include "btSoftBodyHelpers.h"
#include <functional> #include <functional>
typedef btAlignedObjectArray<btSoftBody*> btSoftBodyArray; typedef btAlignedObjectArray<btSoftBody*> btSoftBodyArray;
class btDeformableBodySolver; class btDeformableBodySolver;
class btLagrangianForce; class btDeformableLagrangianForce;
typedef btAlignedObjectArray<btSoftBody*> btSoftBodyArray; typedef btAlignedObjectArray<btSoftBody*> btSoftBodyArray;
class btDeformableRigidDynamicsWorld : public btMultiBodyDynamicsWorld class btDeformableRigidDynamicsWorld : public btMultiBodyDynamicsWorld
@@ -45,6 +45,10 @@ class btDeformableRigidDynamicsWorld : public btMultiBodyDynamicsWorld
protected: protected:
virtual void internalSingleStepSimulation(btScalar timeStep); virtual void internalSingleStepSimulation(btScalar timeStep);
virtual void integrateTransforms(btScalar timeStep);
void positionCorrection();
void solveDeformableBodiesConstraints(btScalar timeStep); void solveDeformableBodiesConstraints(btScalar timeStep);
public: public:
@@ -70,7 +74,7 @@ public:
m_sbi.m_sparsesdf.Initialize(); m_sbi.m_sparsesdf.Initialize();
m_internalTime = 0.0; m_internalTime = 0.0;
} }
btAlignedObjectArray<std::function<void(btScalar, btDeformableRigidDynamicsWorld*)> > before_solver_callbacks; btAlignedObjectArray<std::function<void(btScalar, btDeformableRigidDynamicsWorld*)> > m_beforeSolverCallbacks;
virtual ~btDeformableRigidDynamicsWorld() virtual ~btDeformableRigidDynamicsWorld()
{ {
} }
@@ -108,10 +112,18 @@ public:
{ {
return m_sbi; return m_sbi;
} }
const btSoftBodyWorldInfo& getWorldInfo() const const btSoftBodyWorldInfo& getWorldInfo() const
{ {
return m_sbi; return m_sbi;
} }
void reinitialize(btScalar timeStep);
void applyRigidBodyGravity(btScalar timeStep);
void beforeSolverCallbacks(btScalar timeStep);
int getDrawFlags() const { return (m_drawFlags); } int getDrawFlags() const { return (m_drawFlags); }
void setDrawFlags(int f) { m_drawFlags = f; } void setDrawFlags(int f) { m_drawFlags = f; }
}; };

11
src/BulletSoftBody/btSoftBody.cpp
View File

@@ -1782,7 +1782,7 @@ void btSoftBody::predictMotion(btScalar dt)
m_sst.radmrg = getCollisionShape()->getMargin(); m_sst.radmrg = getCollisionShape()->getMargin();
m_sst.updmrg = m_sst.radmrg * (btScalar)0.25; m_sst.updmrg = m_sst.radmrg * (btScalar)0.25;
/* Forces */ /* Forces */
addVelocity(m_worldInfo->m_gravity * m_sst.sdt); // addVelocity(m_worldInfo->m_gravity * m_sst.sdt);
applyForces(); applyForces();
/* Integrate */ /* Integrate */
for (i = 0, ni = m_nodes.size(); i < ni; ++i) for (i = 0, ni = m_nodes.size(); i < ni; ++i)
@@ -2270,10 +2270,11 @@ bool btSoftBody::checkContact(const btCollisionObjectWrapper* colObjWrap,
{ {
btVector3 nrm; btVector3 nrm;
const btCollisionShape* shp = colObjWrap->getCollisionShape(); const btCollisionShape* shp = colObjWrap->getCollisionShape();
// const btRigidBody *tmpRigid = btRigidBody::upcast(colObjWrap->getCollisionObject()); const btRigidBody *tmpRigid = btRigidBody::upcast(colObjWrap->getCollisionObject());
//const btTransform &wtr = tmpRigid ? tmpRigid->getWorldTransform() : colObjWrap->getWorldTransform();
const btTransform& wtr = colObjWrap->getWorldTransform(); // get the position x_{n+1}^* = x_n + dt * v_{n+1}^* where v_{n+1}^* = v_n + dtg
//todo: check which transform is needed here const btTransform &wtr = tmpRigid ? tmpRigid->getInterpolationWorldTransform() : colObjWrap->getWorldTransform();
// TODO: get the correct transform for multibody
btScalar dst = btScalar dst =
m_worldInfo->m_sparsesdf.Evaluate( m_worldInfo->m_sparsesdf.Evaluate(