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

factor out force; now btDeformableLagrangianceForce can be specified at configuration time and to specific softbody

Browse Source
This commit is contained in:
Xuchen Han
2019-07-25 13:51:44 -07:00
parent 233a381e7c
commit ec403f790d
18 changed files with 150 additions and 204 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
examples/DeformableContact/DeformableContact.cpp
View File

@@ -127,8 +127,9 @@ void DeformableContact::initPhysics()
m_dynamicsWorld = new btDeformableRigidDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver);
deformableBodySolver->setWorld(getDeformableDynamicsWorld());
m_dynamicsWorld->setGravity(btVector3(0, -10, 0));
getDeformableDynamicsWorld()->getWorldInfo().m_gravity.setValue(0, -10, 0);
btVector3 gravity = btVector3(0, -10, 0);
m_dynamicsWorld->setGravity(gravity);
getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity;
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
{
@@ -169,8 +170,8 @@ void DeformableContact::initPhysics()
bool spherical = false; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals
bool canSleep = false;
bool selfCollide = true;
btVector3 linkHalfExtents(1, 1, 1);
btVector3 baseHalfExtents(1, 1, 1);
btVector3 linkHalfExtents(.4, 1, .4);
btVector3 baseHalfExtents(.4, 1, .4);
btMultiBody* mbC = createFeatherstoneMultiBody_testMultiDof(m_dynamicsWorld, numLinks, btVector3(0.f, 10.f,0.f), linkHalfExtents, baseHalfExtents, spherical, g_floatingBase);
@@ -228,6 +229,8 @@ void DeformableContact::initPhysics()
psb->m_cfg.kCHR = 1; // collision hardness with rigid body
psb->m_cfg.kDF = .1;
getDeformableDynamicsWorld()->addSoftBody(psb);
getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce());
getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity));
}
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);

7
examples/DeformableDemo/DeformableDemo.cpp
View File

@@ -168,8 +168,9 @@ void DeformableDemo::initPhysics()
m_dynamicsWorld = new btDeformableRigidDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver);
deformableBodySolver->setWorld(getDeformableDynamicsWorld());
// m_dynamicsWorld->getSolverInfo().m_singleAxisDeformableThreshold = 0.f;//faster but lower quality
m_dynamicsWorld->setGravity(btVector3(0, -10, 0));
getDeformableDynamicsWorld()->getWorldInfo().m_gravity.setValue(0, -10, 0);
btVector3 gravity = btVector3(0, -10, 0);
m_dynamicsWorld->setGravity(gravity);
getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity;
// getDeformableDynamicsWorld()->before_solver_callbacks.push_back(dynamics);
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
@@ -235,6 +236,8 @@ void DeformableDemo::initPhysics()
psb->m_cfg.kCHR = 1; // collision hardness with rigid body
psb->m_cfg.kDF = 1;
getDeformableDynamicsWorld()->addSoftBody(psb);
getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce());
getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity));
// add a few rigid bodies
Ctor_RbUpStack(1);

9
examples/Pinch/Pinch.cpp
View File

@@ -252,8 +252,9 @@ void Pinch::initPhysics()
m_dynamicsWorld = new btDeformableRigidDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver);
deformableBodySolver->setWorld(getDeformableDynamicsWorld());
// m_dynamicsWorld->getSolverInfo().m_singleAxisDeformableThreshold = 0.f;//faster but lower quality
m_dynamicsWorld->setGravity(btVector3(0, -10, 0));
getDeformableDynamicsWorld()->getWorldInfo().m_gravity.setValue(0, -10, 0);
btVector3 gravity = btVector3(0, -10, 0);
m_dynamicsWorld->setGravity(gravity);
getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity;
getDeformableDynamicsWorld()->m_beforeSolverCallbacks.push_back(dynamics);
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
@@ -336,12 +337,14 @@ void Pinch::initPhysics()
// psb->translate(btVector3(-2.5, 4, -2.5));
// psb->getCollisionShape()->setMargin(0.1);
// psb->setTotalMass(1);
psb->setSpringStiffness(4);
psb->setSpringStiffness(2);
psb->setDampingCoefficient(0.02);
psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
psb->m_cfg.kCHR = 1; // collision hardness with rigid body
psb->m_cfg.kDF = 2;
getDeformableDynamicsWorld()->addSoftBody(psb);
getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce());
getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity));
// add a grippers
createGrip();
}

7
examples/VolumetricDeformable/VolumetricDeformable.cpp
View File

@@ -186,8 +186,9 @@ void VolumetricDeformable::initPhysics()
m_dynamicsWorld = new btDeformableRigidDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver);
deformableBodySolver->setWorld(getDeformableDynamicsWorld());
// m_dynamicsWorld->getSolverInfo().m_singleAxisDeformableThreshold = 0.f;//faster but lower quality
m_dynamicsWorld->setGravity(btVector3(0, -10, 0));
getDeformableDynamicsWorld()->getWorldInfo().m_gravity.setValue(0, -10, 0);
btVector3 gravity = btVector3(0, -10, 0);
m_dynamicsWorld->setGravity(gravity);
getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity;
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
{
@@ -240,6 +241,8 @@ void VolumetricDeformable::initPhysics()
psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
psb->m_cfg.kCHR = 1; // collision hardness with rigid body
psb->m_cfg.kDF = 0.5;
getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce());
getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity));
}
// add a few rigid bodies
Ctor_RbUpStack(4);

100
src/BulletSoftBody/btBackwardEulerObjective.h
View File

@@ -1,100 +0,0 @@
//
// 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 "btLagrangianForce.h"
#include "btMassSpring.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<btLagrangianForce*> 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();
// TODO rename
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 */

21
src/BulletSoftBody/btCGProjection.h
View File

@@ -109,12 +109,13 @@ public:
using TArrayStack = btAlignedObjectArray<btAlignedObjectArray<btScalar> >;
btAlignedObjectArray<btSoftBody *> m_softBodies;
btDeformableRigidDynamicsWorld* m_world;
std::unordered_map<btSoftBody::Node *, size_t> m_indices;
const std::unordered_map<btSoftBody::Node *, size_t>* m_indices;
const btScalar& m_dt;
btCGProjection(btAlignedObjectArray<btSoftBody *>& softBodies, const btScalar& dt)
btCGProjection(btAlignedObjectArray<btSoftBody *>& softBodies, const btScalar& dt, const std::unordered_map<btSoftBody::Node *, size_t>* indices)
: m_softBodies(softBodies)
, m_dt(dt)
, m_indices(indices)
{
}
@@ -132,22 +133,6 @@ public:
virtual void reinitialize(bool nodeUpdated)
{
if (nodeUpdated)
updateId();
}
void updateId()
{
size_t index = 0;
m_indices.clear();
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
m_indices[&(psb->m_nodes[j])] = index++;
}
}
}
void setSoftBodies(btAlignedObjectArray<btSoftBody* > softBodies)

13
src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp
View File

@@ -9,21 +9,22 @@
btDeformableBackwardEulerObjective::btDeformableBackwardEulerObjective(btAlignedObjectArray<btSoftBody *>& softBodies, const TVStack& backup_v)
: m_softBodies(softBodies)
, projection(m_softBodies, m_dt)
, projection(m_softBodies, m_dt, &m_indices)
, m_backupVelocity(backup_v)
{
// TODO: this should really be specified in initialization instead of here
btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(m_softBodies);
btDeformableGravityForce* gravity = new btDeformableGravityForce(m_softBodies, btVector3(0,-10,0));
// btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(m_softBodies);
// btDeformableGravityForce* gravity = new btDeformableGravityForce(m_softBodies, btVector3(0,-10,0));
m_preconditioner = new DefaultPreconditioner();
m_lf.push_back(mass_spring);
m_lf.push_back(gravity);
// m_lf.push_back(mass_spring);
// m_lf.push_back(gravity);
}
void btDeformableBackwardEulerObjective::reinitialize(bool nodeUpdated)
{
if(nodeUpdated)
{
updateId();
projection.setSoftBodies(m_softBodies);
}
for (int i = 0; i < m_lf.size(); ++i)
@@ -69,7 +70,7 @@ void btDeformableBackwardEulerObjective::updateVelocity(const TVStack& dv)
// only the velocity of the constrained nodes needs to be updated during CG solve
for (auto it : projection.m_constraints)
{
int i = projection.m_indices[it.first];
int i = m_indices[it.first];
it.first->m_v = m_backupVelocity[i] + dv[i];
}
}

19
src/BulletSoftBody/btDeformableBackwardEulerObjective.h
View File

@@ -28,6 +28,7 @@ public:
Preconditioner* m_preconditioner;
btDeformableContactProjection projection;
const TVStack& m_backupVelocity;
std::unordered_map<btSoftBody::Node *, size_t> m_indices;
btDeformableBackwardEulerObjective(btAlignedObjectArray<btSoftBody *>& softBodies, const TVStack& backup_v);
@@ -95,6 +96,24 @@ public:
m_world = world;
projection.setWorld(world);
}
virtual void updateId()
{
size_t index = 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)
{
m_indices[&(psb->m_nodes[j])] = index++;
}
}
}
std::unordered_map<btSoftBody::Node *, size_t>* getIndices()
{
return &m_indices;
}
};
#endif /* btBackwardEulerObjective_h */

17
src/BulletSoftBody/btDeformableContactProjection.cpp
View File

@@ -88,26 +88,27 @@ void btDeformableContactProjection::update()
const btScalar* J_n = &c->jacobianData_normal.m_jacobians[0];
const btScalar* J_t1 = &c->jacobianData_t1.m_jacobians[0];
const btScalar* J_t2 = &c->jacobianData_t2.m_jacobians[0];
const btScalar* local_v = multibodyLinkCol->m_multiBody->getVelocityVector();
deltaV_normal = &c->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] * J_n[k];
vel += local_v[k] * J_n[k];
}
va = cti.m_normal * vel * m_dt;
// add in the tangential components of the va
vel = 0.0;
for (int k = 0; k < ndof; ++k)
{
vel += multibodyLinkCol->m_multiBody->getVelocityVector()[k] * J_t1[k];
vel += local_v[k] * J_t1[k];
}
va += c->t1 * vel * m_dt;
vel = 0.0;
for (int k = 0; k < ndof; ++k)
{
vel += multibodyLinkCol->m_multiBody->getVelocityVector()[k] * J_t2[k];
vel += local_v[k] * J_t2[k];
}
va += c->t2 * vel * m_dt;
}
@@ -177,7 +178,7 @@ void btDeformableContactProjection::update()
// the following is equivalent
/*
btVector3 dv = -impulse_normal * c->m_c2/m_dt + c->m_node->m_v - backupVelocity[m_indices[c->m_node]];
btVector3 dv = -impulse_normal * c->m_c2/m_dt + c->m_node->m_v - backupVelocity[m_indices->at(c->m_node)];
btScalar dvn = dv.dot(cti.m_normal);
*/
@@ -196,9 +197,11 @@ void btDeformableContactProjection::update()
{
if (multibodyLinkCol)
{
// apply normal component of the impulse
multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_normal, impulse.dot(cti.m_normal));
if (incremental_tangent.norm() > SIMD_EPSILON)
{
// apply tangential component of the impulse
const btScalar* deltaV_t1 = &c->jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_t1, impulse.dot(c->t1));
const btScalar* deltaV_t2 = &c->jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
@@ -336,7 +339,7 @@ void btDeformableContactProjection::enforceConstraint(TVStack& x)
for (auto& it : m_constraints)
{
const btAlignedObjectArray<DeformableContactConstraint>& constraints = it.second;
size_t i = m_indices[it.first];
size_t i = m_indices->at(it.first);
const btAlignedObjectArray<DeformableFrictionConstraint>& frictions = m_frictions[it.first];
btAssert(constraints.size() <= dim);
btAssert(constraints.size() > 0);
@@ -399,7 +402,7 @@ void btDeformableContactProjection::project(TVStack& x)
for (auto& it : m_constraints)
{
const btAlignedObjectArray<DeformableContactConstraint>& constraints = it.second;
size_t i = m_indices[it.first];
size_t i = m_indices->at(it.first);
btAlignedObjectArray<DeformableFrictionConstraint>& frictions = m_frictions[it.first];
btAssert(constraints.size() <= dim);
btAssert(constraints.size() > 0);

4
src/BulletSoftBody/btDeformableContactProjection.h
View File

@@ -18,8 +18,8 @@ public:
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)
btDeformableContactProjection(btAlignedObjectArray<btSoftBody *>& softBodies, const btScalar& dt, const std::unordered_map<btSoftBody::Node *, size_t>* indices)
: btCGProjection(softBodies, dt, indices)
{
}

11
src/BulletSoftBody/btDeformableGravityForce.h
View File

@@ -16,7 +16,7 @@ public:
using TVStack = btDeformableLagrangianForce::TVStack;
btVector3 m_gravity;
btDeformableGravityForce(const btAlignedObjectArray<btSoftBody *>& softBodies, const btVector3& g) : btDeformableLagrangianForce(softBodies), m_gravity(g)
btDeformableGravityForce(const btVector3& g) : m_gravity(g)
{
}
@@ -39,14 +39,14 @@ public:
virtual void addScaledGravityForce(btScalar scale, TVStack& force)
{
int numNodes = getNumNodes();
btAssert(numNodes == force.size())
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];
size_t id = m_indices->at(&n);
btScalar mass = (n.m_im == 0) ? 0 : 1. / n.m_im;
btVector3 scaled_force = scale * m_gravity * mass;
force[id] += scaled_force;
@@ -54,7 +54,10 @@ public:
}
}
virtual btDeformableLagrangianForceType getForceType()
{
return BT_GRAVITY_FORCE;
}
};

39
src/BulletSoftBody/btDeformableLagrangianForce.h
View File

@@ -10,16 +10,20 @@
#include "btSoftBody.h"
#include <unordered_map>
enum btDeformableLagrangianForceType
{
BT_GRAVITY_FORCE = 1,
BT_MASSSPRING_FORCE = 2
};
class btDeformableLagrangianForce
{
public:
using TVStack = btAlignedObjectArray<btVector3>;
const btAlignedObjectArray<btSoftBody *>& m_softBodies;
std::unordered_map<btSoftBody::Node *, size_t> m_indices;
btAlignedObjectArray<btSoftBody *> m_softBodies;
const std::unordered_map<btSoftBody::Node *, size_t>* m_indices;
btDeformableLagrangianForce(const btAlignedObjectArray<btSoftBody *>& softBodies)
: m_softBodies(softBodies)
btDeformableLagrangianForce()
{
}
@@ -31,23 +35,10 @@ public:
virtual void addScaledExplicitForce(btScalar scale, TVStack& force) = 0;
virtual btDeformableLagrangianForceType getForceType() = 0;
virtual void reinitialize(bool nodeUpdated)
{
if (nodeUpdated)
updateId();
}
virtual void updateId()
{
size_t index = 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)
{
m_indices[&(psb->m_nodes[j])] = index++;
}
}
}
virtual int getNumNodes()
@@ -59,5 +50,15 @@ public:
}
return numNodes;
}
virtual void addSoftBody(btSoftBody* psb)
{
m_softBodies.push_back(psb);
}
virtual void setIndices(const std::unordered_map<btSoftBody::Node *, size_t>* indices)
{
m_indices = indices;
}
};
#endif /* BT_DEFORMABLE_LAGRANGIAN_FORCE */

24
src/BulletSoftBody/btDeformableMassSpringForce.h
View File

@@ -14,15 +14,13 @@ class btDeformableMassSpringForce : public btDeformableLagrangianForce
{
public:
using TVStack = btDeformableLagrangianForce::TVStack;
btDeformableMassSpringForce(const btAlignedObjectArray<btSoftBody *>& softBodies) : btDeformableLagrangianForce(softBodies)
btDeformableMassSpringForce()
{
}
virtual void addScaledImplicitForce(btScalar scale, TVStack& force)
{
addScaledDampingForce(scale, force);
// addScaledElasticForce(scale, force);
}
virtual void addScaledExplicitForce(btScalar scale, TVStack& force)
@@ -33,7 +31,7 @@ public:
virtual void addScaledDampingForce(btScalar scale, TVStack& force)
{
int numNodes = getNumNodes();
btAssert(numNodes == force.size())
btAssert(numNodes <= force.size())
for (int i = 0; i < m_softBodies.size(); ++i)
{
const btSoftBody* psb = m_softBodies[i];
@@ -42,8 +40,8 @@ public:
const auto& link = psb->m_links[j];
const auto node1 = link.m_n[0];
const auto node2 = link.m_n[1];
size_t id1 = m_indices[node1];
size_t id2 = m_indices[node2];
size_t id1 = m_indices->at(node1);
size_t id2 = m_indices->at(node2);
// damping force
btVector3 v_diff = (node2->m_v - node1->m_v);
@@ -58,7 +56,7 @@ public:
virtual void addScaledElasticForce(btScalar scale, TVStack& force)
{
int numNodes = getNumNodes();
btAssert(numNodes == force.size())
btAssert(numNodes <= force.size())
for (int i = 0; i < m_softBodies.size(); ++i)
{
const btSoftBody* psb = m_softBodies[i];
@@ -69,8 +67,8 @@ public:
const auto node2 = link.m_n[1];
btScalar kLST = link.Feature::m_material->m_kLST;
btScalar r = link.m_rl;
size_t id1 = m_indices[node1];
size_t id2 = m_indices[node2];
size_t id1 = m_indices->at(node1);
size_t id2 = m_indices->at(node2);
// elastic force
// explicit elastic force
@@ -95,8 +93,8 @@ public:
const auto node1 = link.m_n[0];
const auto node2 = link.m_n[1];
btScalar k_damp = psb->m_dampingCoefficient;
size_t id1 = m_indices[node1];
size_t id2 = m_indices[node2];
size_t id1 = m_indices->at(node1);
size_t id2 = m_indices->at(node2);
btVector3 local_scaled_df = scale * k_damp * (dv[id2] - dv[id1]);
df[id1] += local_scaled_df;
df[id2] -= local_scaled_df;
@@ -104,6 +102,10 @@ public:
}
}
virtual btDeformableLagrangianForceType getForceType()
{
return BT_MASSSPRING_FORCE;
}
};

33
src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp
View File

@@ -60,13 +60,12 @@ void btDeformableRigidDynamicsWorld::positionCorrection(btScalar dt)
if (c == nullptr || c->m_node->m_im == 0)
continue;
const btSoftBody::sCti& cti = c->m_cti;
btRigidBody* rigidCol = 0;
btVector3 va(0, 0, 0);
// grab the velocity of the rigid body
if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
{
rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
btRigidBody* rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
va = rigidCol ? (rigidCol->getVelocityInLocalPoint(c->m_c1)): btVector3(0, 0, 0);
}
else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
@@ -78,25 +77,25 @@ void btDeformableRigidDynamicsWorld::positionCorrection(btScalar dt)
const btScalar* J_n = &c->jacobianData_normal.m_jacobians[0];
const btScalar* J_t1 = &c->jacobianData_t1.m_jacobians[0];
const btScalar* J_t2 = &c->jacobianData_t2.m_jacobians[0];
const btScalar* local_v = multibodyLinkCol->m_multiBody->getVelocityVector();
// 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] * J_n[k];
vel += local_v[k] * J_n[k];
}
va = cti.m_normal * vel;
vel = 0.0;
for (int k = 0; k < ndof; ++k)
{
vel += multibodyLinkCol->m_multiBody->getVelocityVector()[k] * J_t1[k];
vel += local_v[k] * J_t1[k];
}
va += c->t1 * vel;
vel = 0.0;
for (int k = 0; k < ndof; ++k)
{
vel += multibodyLinkCol->m_multiBody->getVelocityVector()[k] * J_t2[k];
vel += local_v[k] * J_t2[k];
}
va += c->t2 * vel;
}
@@ -110,7 +109,6 @@ void btDeformableRigidDynamicsWorld::positionCorrection(btScalar dt)
if (cti.m_colObj->hasContactResponse())
{
btScalar dp = cti.m_offset;
rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
if (friction.m_static[j] == true)
{
c->m_node->m_v = va;
@@ -214,3 +212,24 @@ void btDeformableRigidDynamicsWorld::afterSolverCallbacks(btScalar timeStep)
for (int i = 0; i < m_beforeSolverCallbacks.size(); ++i)
m_beforeSolverCallbacks[i](m_internalTime, this);
}
void btDeformableRigidDynamicsWorld::addForce(btSoftBody* psb, btDeformableLagrangianForce* force)
{
btAlignedObjectArray<btDeformableLagrangianForce*>& forces = m_deformableBodySolver->m_objective->m_lf;
bool added = false;
for (int i = 0; i < forces.size(); ++i)
{
if (forces[i]->getForceType() == force->getForceType())
{
forces[i]->addSoftBody(psb);
added = true;
break;
}
}
if (!added)
{
force->addSoftBody(psb);
force->setIndices(m_deformableBodySolver->m_objective->getIndices());
forces.push_back(force);
}
}

2
src/BulletSoftBody/btDeformableRigidDynamicsWorld.h
View File

@@ -126,6 +126,8 @@ public:
void afterSolverCallbacks(btScalar timeStep);
void addForce(btSoftBody* psb, btDeformableLagrangianForce* force);
int getDrawFlags() const { return (m_drawFlags); }
void setDrawFlags(int f) { m_drawFlags = f; }
};

10
src/BulletSoftBody/btSoftBody.cpp
View File

@@ -2266,14 +2266,15 @@ btVector3 btSoftBody::evaluateCom() const
bool btSoftBody::checkContact(const btCollisionObjectWrapper* colObjWrap,
const btVector3& x,
btScalar margin,
btSoftBody::sCti& cti) const
btSoftBody::sCti& cti, bool predict) const
{
btVector3 nrm;
const btCollisionShape* shp = colObjWrap->getCollisionShape();
const btRigidBody *tmpRigid = btRigidBody::upcast(colObjWrap->getCollisionObject());
const btRigidBody *tmpRigid = btRigidBody::upcast(colObjWrap->getCollisionObject());
// get the position x_{n+1}^* = x_n + dt * v_{n+1}^* where v_{n+1}^* = v_n + dtg
const btTransform &wtr = tmpRigid ? tmpRigid->getInterpolationWorldTransform() : colObjWrap->getWorldTransform();
const btTransform &wtr = (tmpRigid&&predict) ? tmpRigid->getInterpolationWorldTransform() : colObjWrap->getWorldTransform();
// const btTransform &wtr = predict ? colObjWrap->getInterpolationWorldTransform() : colObjWrap->getWorldTransform();
// TODO: get the correct transform for multibody
btScalar dst =
@@ -2282,11 +2283,10 @@ bool btSoftBody::checkContact(const btCollisionObjectWrapper* colObjWrap,
shp,
nrm,
margin);
if (dst < 0)
if (dst < 0 || !predict)
{
cti.m_colObj = colObjWrap->getCollisionObject();
cti.m_normal = wtr.getBasis() * nrm;
// cti.m_offset = -btDot(cti.m_normal, x - cti.m_normal * dst);
cti.m_offset = dst;
return (true);
}

2
src/BulletSoftBody/btSoftBody.h
View File

@@ -1005,7 +1005,7 @@ public:
btScalar& mint, eFeature::_& feature, int& index, bool bcountonly) const;
void initializeFaceTree();
btVector3 evaluateCom() const;
bool checkContact(const btCollisionObjectWrapper* colObjWrap, const btVector3& x, btScalar margin, btSoftBody::sCti& cti) const;
bool checkContact(const btCollisionObjectWrapper* colObjWrap, const btVector3& x, btScalar margin, btSoftBody::sCti& cti, bool predict = false) const;
void updateNormals();
void updateBounds();
void updatePose();

25
src/BulletSoftBody/btSoftBodyInternals.h
View File

@@ -945,14 +945,17 @@ struct btSoftColliders
if (!n.m_battach)
{
if (psb->checkContact(m_colObj1Wrap, n.m_x, m, c.m_cti))
// check for collision at x_{n+1}^*
if (psb->checkContact(m_colObj1Wrap, n.m_x, m, c.m_cti, /*predicted = */ true))
// if (psb->checkContact(m_colObj1Wrap, n.m_q, m, c.m_cti, /*predicted = */ false));
{
const btScalar ima = n.m_im;
const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f;
const btScalar ms = ima + imb;
if (ms > 0)
{
psb->checkContact(m_colObj1Wrap, n.m_q, m, c.m_cti);
// resolve contact at x_n
psb->checkContact(m_colObj1Wrap, n.m_q, m, c.m_cti, /*predicted = */ false);
auto& cti = c.m_cti;
c.m_node = &n;
const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction();
@@ -982,8 +985,13 @@ struct btSoftColliders
btVector3 t2 = btCross(normal, t1);
btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2;
findJacobian(multibodyLinkCol, jacobianData_normal, c.m_node->m_q, normal);
findJacobian(multibodyLinkCol, jacobianData_t1, c.m_node->m_q, t1);
findJacobian(multibodyLinkCol, jacobianData_t2, c.m_node->m_q, t2);
// findJacobian is hella expensive, avoid calling if possible
if (fc != 0)
{
findJacobian(multibodyLinkCol, jacobianData_t1, c.m_node->m_q, t1);
findJacobian(multibodyLinkCol, jacobianData_t2, c.m_node->m_q, t2);
}
btScalar* J_n = &jacobianData_normal.m_jacobians[0];
btScalar* J_t1 = &jacobianData_t1.m_jacobians[0];
@@ -995,16 +1003,7 @@ struct btSoftColliders
btMatrix3x3 rot(normal, t1, t2); // world frame to local frame
const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
btVector3 u_dot_J(0,0,0);
for (int i = 0; i < ndof; ++i)
{
u_dot_J += btVector3(J_n[i] * u_n[i], J_t1[i] * u_t1[i], J_t2[i] * u_t2[i]);
}
btVector3 impulse_matrix_diag;
btScalar dt = psb->m_sst.sdt;
impulse_matrix_diag.setX(1/((u_dot_J.getX() + n.m_im) * dt));
impulse_matrix_diag.setY(1/((u_dot_J.getY() + n.m_im) * dt));
impulse_matrix_diag.setZ(1/((u_dot_J.getZ() + n.m_im) * dt));
btMatrix3x3 local_impulse_matrix = Diagonal(1/dt) * (Diagonal(n.m_im) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse();
c.m_c0 = rot.transpose() * local_impulse_matrix * rot;
c.jacobianData_normal = jacobianData_normal;