mirror of
https://github.com/bulletphysics/bullet3
synced 2024-12-15 06:00:12 +00:00
documentation and optimization
This commit is contained in:
parent
20abbc9ee7
commit
657a7468b3
@ -153,7 +153,7 @@ void DeformableSelfCollision::initPhysics()
|
|||||||
btVector3(-s, h, +4*s),
|
btVector3(-s, h, +4*s),
|
||||||
btVector3(+s, h, +4*s),
|
btVector3(+s, h, +4*s),
|
||||||
10,40,
|
10,40,
|
||||||
0, true, 0.1);
|
0, true, 0.01);
|
||||||
|
|
||||||
|
|
||||||
psb->getCollisionShape()->setMargin(0.2);
|
psb->getCollisionShape()->setMargin(0.2);
|
||||||
@ -161,7 +161,7 @@ void DeformableSelfCollision::initPhysics()
|
|||||||
psb->setTotalMass(1);
|
psb->setTotalMass(1);
|
||||||
psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
|
psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
|
||||||
psb->m_cfg.kCHR = 1; // collision hardness with rigid body
|
psb->m_cfg.kCHR = 1; // collision hardness with rigid body
|
||||||
psb->m_cfg.kDF = 0.02;
|
psb->m_cfg.kDF = 0.2;
|
||||||
psb->rotate(btQuaternion(0,SIMD_PI / 2, 0));
|
psb->rotate(btQuaternion(0,SIMD_PI / 2, 0));
|
||||||
psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
|
psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
|
||||||
psb->m_cfg.collisions |= btSoftBody::fCollision::VF_DD;
|
psb->m_cfg.collisions |= btSoftBody::fCollision::VF_DD;
|
||||||
|
@ -91,7 +91,7 @@ public:
|
|||||||
|
|
||||||
void Ctor_RbUpStack(int count)
|
void Ctor_RbUpStack(int count)
|
||||||
{
|
{
|
||||||
float mass = 0.02;
|
float mass = 1;
|
||||||
|
|
||||||
btCompoundShape* cylinderCompound = new btCompoundShape;
|
btCompoundShape* cylinderCompound = new btCompoundShape;
|
||||||
btCollisionShape* cylinderShape = new btCylinderShapeX(btVector3(2, .5, .5));
|
btCollisionShape* cylinderShape = new btCylinderShapeX(btVector3(2, .5, .5));
|
||||||
@ -166,7 +166,7 @@ void VolumetricDeformable::initPhysics()
|
|||||||
m_solver = sol;
|
m_solver = sol;
|
||||||
|
|
||||||
m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver);
|
m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver);
|
||||||
btVector3 gravity = btVector3(0, -10, 0);
|
btVector3 gravity = btVector3(0, -100, 0);
|
||||||
m_dynamicsWorld->setGravity(gravity);
|
m_dynamicsWorld->setGravity(gravity);
|
||||||
getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity;
|
getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity;
|
||||||
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
|
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
|
||||||
@ -196,7 +196,6 @@ void VolumetricDeformable::initPhysics()
|
|||||||
//add the ground to the dynamics world
|
//add the ground to the dynamics world
|
||||||
m_dynamicsWorld->addRigidBody(body);
|
m_dynamicsWorld->addRigidBody(body);
|
||||||
}
|
}
|
||||||
getDeformableDynamicsWorld()->setLineSearch(false);
|
|
||||||
|
|
||||||
createStaticBox(btVector3(1, 5, 5), btVector3(-5,0,0));
|
createStaticBox(btVector3(1, 5, 5), btVector3(-5,0,0));
|
||||||
createStaticBox(btVector3(1, 5, 5), btVector3(5,0,0));
|
createStaticBox(btVector3(1, 5, 5), btVector3(5,0,0));
|
||||||
@ -220,19 +219,17 @@ void VolumetricDeformable::initPhysics()
|
|||||||
psb->m_cfg.kDF = 0.5;
|
psb->m_cfg.kDF = 0.5;
|
||||||
psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
|
psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
|
||||||
|
|
||||||
btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(0,0.03);
|
|
||||||
getDeformableDynamicsWorld()->addForce(psb, mass_spring);
|
|
||||||
m_forces.push_back(mass_spring);
|
|
||||||
|
|
||||||
btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity);
|
btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity);
|
||||||
getDeformableDynamicsWorld()->addForce(psb, gravity_force);
|
getDeformableDynamicsWorld()->addForce(psb, gravity_force);
|
||||||
m_forces.push_back(gravity_force);
|
m_forces.push_back(gravity_force);
|
||||||
|
|
||||||
btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(.5,2.5);
|
btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(30,100,0.02);
|
||||||
getDeformableDynamicsWorld()->addForce(psb, neohookean);
|
getDeformableDynamicsWorld()->addForce(psb, neohookean);
|
||||||
m_forces.push_back(neohookean);
|
m_forces.push_back(neohookean);
|
||||||
|
|
||||||
}
|
}
|
||||||
|
getDeformableDynamicsWorld()->setImplicit(true);
|
||||||
|
getDeformableDynamicsWorld()->setLineSearch(false);
|
||||||
// add a few rigid bodies
|
// add a few rigid bodies
|
||||||
Ctor_RbUpStack(4);
|
Ctor_RbUpStack(4);
|
||||||
|
|
||||||
|
@ -69,7 +69,7 @@ public:
|
|||||||
if (dot(p,temp) < SIMD_EPSILON)
|
if (dot(p,temp) < SIMD_EPSILON)
|
||||||
{
|
{
|
||||||
if (verbose)
|
if (verbose)
|
||||||
std::cout << "Encountered negative direction in CG!"<<std::endl;
|
std::cout << "Encountered negative direction in CG!" << std::endl;
|
||||||
if (k == 1)
|
if (k == 1)
|
||||||
{
|
{
|
||||||
x = b;
|
x = b;
|
||||||
|
@ -22,7 +22,7 @@
|
|||||||
btDeformableBodySolver::btDeformableBodySolver()
|
btDeformableBodySolver::btDeformableBodySolver()
|
||||||
: m_numNodes(0)
|
: m_numNodes(0)
|
||||||
, m_cg(20)
|
, m_cg(20)
|
||||||
, m_maxNewtonIterations(10)
|
, m_maxNewtonIterations(5)
|
||||||
, m_newtonTolerance(1e-4)
|
, m_newtonTolerance(1e-4)
|
||||||
, m_lineSearch(true)
|
, m_lineSearch(true)
|
||||||
{
|
{
|
||||||
@ -412,6 +412,7 @@ void btDeformableBodySolver::predictDeformableMotion(btSoftBody* psb, btScalar d
|
|||||||
psb->m_fdbvt.optimizeIncremental(1);
|
psb->m_fdbvt.optimizeIncremental(1);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
void btDeformableBodySolver::updateSoftBodies()
|
void btDeformableBodySolver::updateSoftBodies()
|
||||||
{
|
{
|
||||||
for (int i = 0; i < m_softBodies.size(); i++)
|
for (int i = 0; i < m_softBodies.size(); i++)
|
||||||
@ -419,7 +420,7 @@ void btDeformableBodySolver::updateSoftBodies()
|
|||||||
btSoftBody *psb = (btSoftBody *)m_softBodies[i];
|
btSoftBody *psb = (btSoftBody *)m_softBodies[i];
|
||||||
if (psb->isActive())
|
if (psb->isActive())
|
||||||
{
|
{
|
||||||
psb->updateNormals(); // normal is updated here
|
psb->updateNormals();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -29,26 +29,24 @@ class btDeformableMultiBodyDynamicsWorld;
|
|||||||
|
|
||||||
class btDeformableBodySolver : public btSoftBodySolver
|
class btDeformableBodySolver : public btSoftBodySolver
|
||||||
{
|
{
|
||||||
// using TVStack = btAlignedObjectArray<btVector3>;
|
|
||||||
typedef btAlignedObjectArray<btVector3> TVStack;
|
typedef btAlignedObjectArray<btVector3> TVStack;
|
||||||
protected:
|
protected:
|
||||||
int m_numNodes;
|
int m_numNodes; // total number of deformable body nodes
|
||||||
TVStack m_dv;
|
TVStack m_dv; // v_{n+1} - v_n
|
||||||
TVStack m_backup_dv;
|
TVStack m_backup_dv; // backed up dv
|
||||||
TVStack m_ddv;
|
TVStack m_ddv; // incremental dv
|
||||||
TVStack m_residual;
|
TVStack m_residual; // rhs of the linear solve
|
||||||
btAlignedObjectArray<btSoftBody *> m_softBodies;
|
btAlignedObjectArray<btSoftBody *> m_softBodies; // all deformable bodies
|
||||||
|
TVStack m_backupVelocity; // backed up v, equals v_n for implicit, equals v_{n+1}^* for explicit
|
||||||
btAlignedObjectArray<btVector3> m_backupVelocity;
|
btScalar m_dt; // dt
|
||||||
btScalar m_dt;
|
btConjugateGradient<btDeformableBackwardEulerObjective> m_cg; // CG solver
|
||||||
btScalar m_contact_iterations;
|
bool m_implicit; // use implicit scheme if true, explicit scheme if false
|
||||||
btConjugateGradient<btDeformableBackwardEulerObjective> m_cg;
|
int m_maxNewtonIterations; // max number of newton iterations
|
||||||
bool m_implicit;
|
btScalar m_newtonTolerance; // stop newton iterations if f(x) < m_newtonTolerance
|
||||||
int m_maxNewtonIterations;
|
bool m_lineSearch; // If true, use newton's method with line search under implicit scheme
|
||||||
btScalar m_newtonTolerance;
|
|
||||||
bool m_lineSearch;
|
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
// handles data related to objective function
|
||||||
btDeformableBackwardEulerObjective* m_objective;
|
btDeformableBackwardEulerObjective* m_objective;
|
||||||
|
|
||||||
btDeformableBodySolver();
|
btDeformableBodySolver();
|
||||||
@ -60,48 +58,61 @@ public:
|
|||||||
return DEFORMABLE_SOLVER;
|
return DEFORMABLE_SOLVER;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// update soft body normals
|
||||||
virtual void updateSoftBodies();
|
virtual void updateSoftBodies();
|
||||||
|
|
||||||
virtual void copyBackToSoftBodies(bool bMove = true) {}
|
// solve the momentum equation
|
||||||
|
|
||||||
virtual void solveDeformableConstraints(btScalar solverdt);
|
virtual void solveDeformableConstraints(btScalar solverdt);
|
||||||
|
|
||||||
|
// solve the contact between deformable and rigid as well as among deformables
|
||||||
btScalar solveContactConstraints();
|
btScalar solveContactConstraints();
|
||||||
|
|
||||||
virtual void solveConstraints(btScalar dt){}
|
// resize/clear data structures
|
||||||
|
|
||||||
void reinitialize(const btAlignedObjectArray<btSoftBody *>& softBodies, btScalar dt);
|
void reinitialize(const btAlignedObjectArray<btSoftBody *>& softBodies, btScalar dt);
|
||||||
|
|
||||||
|
// set up contact constraints
|
||||||
void setConstraints();
|
void setConstraints();
|
||||||
|
|
||||||
|
// add in elastic forces and gravity to obtain v_{n+1}^* and calls predictDeformableMotion
|
||||||
|
virtual void predictMotion(btScalar solverdt);
|
||||||
|
|
||||||
|
// move to temporary position x_{n+1}^* = x_n + dt * v_{n+1}^*
|
||||||
|
// x_{n+1}^* is stored in m_q
|
||||||
void predictDeformableMotion(btSoftBody* psb, btScalar dt);
|
void predictDeformableMotion(btSoftBody* psb, btScalar dt);
|
||||||
|
|
||||||
|
// save the current velocity to m_backupVelocity
|
||||||
void backupVelocity();
|
void backupVelocity();
|
||||||
|
|
||||||
|
// set m_dv and m_backupVelocity to desired value to prepare for momentum solve
|
||||||
void setupDeformableSolve(bool implicit);
|
void setupDeformableSolve(bool implicit);
|
||||||
|
|
||||||
|
// set the current velocity to that backed up in m_backupVelocity
|
||||||
void revertVelocity();
|
void revertVelocity();
|
||||||
|
|
||||||
|
// set velocity to m_dv + m_backupVelocity
|
||||||
void updateVelocity();
|
void updateVelocity();
|
||||||
|
|
||||||
|
// update the node count
|
||||||
bool updateNodes();
|
bool updateNodes();
|
||||||
|
|
||||||
void computeStep(TVStack& dv, const TVStack& residual);
|
// calculate the change in dv resulting from the momentum solve
|
||||||
|
void computeStep(TVStack& ddv, const TVStack& residual);
|
||||||
|
|
||||||
|
// calculate the change in dv resulting from the momentum solve when line search is turned on
|
||||||
btScalar computeDescentStep(TVStack& ddv, const TVStack& residual);
|
btScalar computeDescentStep(TVStack& ddv, const TVStack& residual);
|
||||||
|
|
||||||
virtual void predictMotion(btScalar solverdt);
|
|
||||||
|
|
||||||
virtual void copySoftBodyToVertexBuffer(const btSoftBody *const softBody, btVertexBufferDescriptor *vertexBuffer) {}
|
virtual void copySoftBodyToVertexBuffer(const btSoftBody *const softBody, btVertexBufferDescriptor *vertexBuffer) {}
|
||||||
|
|
||||||
|
// process collision between deformable and rigid
|
||||||
virtual void processCollision(btSoftBody * softBody, const btCollisionObjectWrapper * collisionObjectWrap)
|
virtual void processCollision(btSoftBody * softBody, const btCollisionObjectWrapper * collisionObjectWrap)
|
||||||
{
|
{
|
||||||
softBody->defaultCollisionHandler(collisionObjectWrap);
|
softBody->defaultCollisionHandler(collisionObjectWrap);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// process collision between deformable and deformable
|
||||||
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;}
|
|
||||||
|
|
||||||
// If true, implicit time stepping scheme is used.
|
// If true, implicit time stepping scheme is used.
|
||||||
// Otherwise, explicit time stepping scheme is used
|
// Otherwise, explicit time stepping scheme is used
|
||||||
@ -136,6 +147,12 @@ public:
|
|||||||
// 1/2 * dv^T * M * dv
|
// 1/2 * dv^T * M * dv
|
||||||
// used in line search
|
// used in line search
|
||||||
btScalar kineticEnergy();
|
btScalar kineticEnergy();
|
||||||
|
|
||||||
|
// unused functions
|
||||||
|
virtual void optimize(btAlignedObjectArray<btSoftBody *> &softBodies, bool forceUpdate = false){}
|
||||||
|
virtual void solveConstraints(btScalar dt){}
|
||||||
|
virtual bool checkInitialized(){return true;}
|
||||||
|
virtual void copyBackToSoftBodies(bool bMove = true) {}
|
||||||
};
|
};
|
||||||
|
|
||||||
#endif /* btDeformableBodySolver_h */
|
#endif /* btDeformableBodySolver_h */
|
||||||
|
@ -26,8 +26,6 @@ class btDeformableContactProjection
|
|||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
typedef btAlignedObjectArray<btVector3> TVStack;
|
typedef btAlignedObjectArray<btVector3> TVStack;
|
||||||
typedef btAlignedObjectArray<btAlignedObjectArray<btVector3> > TVArrayStack;
|
|
||||||
typedef btAlignedObjectArray<btAlignedObjectArray<btScalar> > TArrayStack;
|
|
||||||
btAlignedObjectArray<btSoftBody *>& m_softBodies;
|
btAlignedObjectArray<btSoftBody *>& m_softBodies;
|
||||||
|
|
||||||
// map from node index to static constraint
|
// map from node index to static constraint
|
||||||
@ -54,10 +52,10 @@ public:
|
|||||||
{
|
{
|
||||||
}
|
}
|
||||||
|
|
||||||
// apply the constraints to the rhs
|
// apply the constraints to the rhs of the linear solve
|
||||||
virtual void project(TVStack& x);
|
virtual void project(TVStack& x);
|
||||||
|
|
||||||
// add to friction
|
// add friction force to the rhs of the linear solve
|
||||||
virtual void applyDynamicFriction(TVStack& f);
|
virtual void applyDynamicFriction(TVStack& f);
|
||||||
|
|
||||||
// update the constraints
|
// update the constraints
|
||||||
|
@ -67,6 +67,7 @@ public:
|
|||||||
{
|
{
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// get number of nodes that have the force
|
||||||
virtual int getNumNodes()
|
virtual int getNumNodes()
|
||||||
{
|
{
|
||||||
int numNodes = 0;
|
int numNodes = 0;
|
||||||
@ -94,10 +95,7 @@ public:
|
|||||||
btVector3 c1 = dx[id1] - dx[id0];
|
btVector3 c1 = dx[id1] - dx[id0];
|
||||||
btVector3 c2 = dx[id2] - dx[id0];
|
btVector3 c2 = dx[id2] - dx[id0];
|
||||||
btVector3 c3 = dx[id3] - dx[id0];
|
btVector3 c3 = dx[id3] - dx[id0];
|
||||||
btMatrix3x3 dF(c1.getX(), c2.getX(), c3.getX(),
|
return btMatrix3x3(c1,c2,c3).transpose();
|
||||||
c1.getY(), c2.getY(), c3.getY(),
|
|
||||||
c1.getZ(), c2.getZ(), c3.getZ());
|
|
||||||
return dF;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// Calculate the incremental deformable generated from the current velocity
|
// Calculate the incremental deformable generated from the current velocity
|
||||||
@ -106,12 +104,10 @@ public:
|
|||||||
btVector3 c1 = n1->m_v - n0->m_v;
|
btVector3 c1 = n1->m_v - n0->m_v;
|
||||||
btVector3 c2 = n2->m_v - n0->m_v;
|
btVector3 c2 = n2->m_v - n0->m_v;
|
||||||
btVector3 c3 = n3->m_v - n0->m_v;
|
btVector3 c3 = n3->m_v - n0->m_v;
|
||||||
btMatrix3x3 dF(c1.getX(), c2.getX(), c3.getX(),
|
return btMatrix3x3(c1,c2,c3).transpose();
|
||||||
c1.getY(), c2.getY(), c3.getY(),
|
|
||||||
c1.getZ(), c2.getZ(), c3.getZ());
|
|
||||||
return dF;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// test for addScaledElasticForce function
|
||||||
virtual void testDerivative()
|
virtual void testDerivative()
|
||||||
{
|
{
|
||||||
for (int i = 0; i<m_softBodies.size();++i)
|
for (int i = 0; i<m_softBodies.size();++i)
|
||||||
@ -222,6 +218,7 @@ public:
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// test for addScaledElasticForce function
|
||||||
virtual void testHessian()
|
virtual void testHessian()
|
||||||
{
|
{
|
||||||
for (int i = 0; i<m_softBodies.size();++i)
|
for (int i = 0; i<m_softBodies.size();++i)
|
||||||
|
@ -20,6 +20,8 @@
|
|||||||
|
|
||||||
class btDeformableMassSpringForce : public btDeformableLagrangianForce
|
class btDeformableMassSpringForce : public btDeformableLagrangianForce
|
||||||
{
|
{
|
||||||
|
// If true, the damping force will be in the direction of the spring
|
||||||
|
// If false, the damping force will be in the direction of the velocity
|
||||||
bool m_momentum_conserving;
|
bool m_momentum_conserving;
|
||||||
btScalar m_elasticStiffness, m_dampingStiffness;
|
btScalar m_elasticStiffness, m_dampingStiffness;
|
||||||
public:
|
public:
|
||||||
|
@ -59,3 +59,67 @@ btScalar btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlyIteration
|
|||||||
}
|
}
|
||||||
return 0.f;
|
return 0.f;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void btDeformableMultiBodyConstraintSolver::solveMultiBodyGroup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher)
|
||||||
|
{
|
||||||
|
m_tmpMultiBodyConstraints = multiBodyConstraints;
|
||||||
|
m_tmpNumMultiBodyConstraints = numMultiBodyConstraints;
|
||||||
|
|
||||||
|
// inherited from MultiBodyConstraintSolver
|
||||||
|
solveGroupCacheFriendlySetup(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer);
|
||||||
|
|
||||||
|
// overriden
|
||||||
|
solveGroupCacheFriendlyIterations(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer);
|
||||||
|
|
||||||
|
// inherited from MultiBodyConstraintSolver
|
||||||
|
solveGroupCacheFriendlyFinish(bodies, numBodies, info);
|
||||||
|
|
||||||
|
m_tmpMultiBodyConstraints = 0;
|
||||||
|
m_tmpNumMultiBodyConstraints = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
void btDeformableMultiBodyConstraintSolver::writeToSolverBody(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal)
|
||||||
|
{
|
||||||
|
btSISolverSingleIterationData siData(m_tmpSolverBodyPool,
|
||||||
|
m_tmpSolverContactConstraintPool,
|
||||||
|
m_tmpSolverNonContactConstraintPool,
|
||||||
|
m_tmpSolverContactFrictionConstraintPool,
|
||||||
|
m_tmpSolverContactRollingFrictionConstraintPool,
|
||||||
|
m_orderTmpConstraintPool,
|
||||||
|
m_orderNonContactConstraintPool,
|
||||||
|
m_orderFrictionConstraintPool,
|
||||||
|
m_tmpConstraintSizesPool,
|
||||||
|
m_resolveSingleConstraintRowGeneric,
|
||||||
|
m_resolveSingleConstraintRowLowerLimit,
|
||||||
|
m_resolveSplitPenetrationImpulse,
|
||||||
|
m_kinematicBodyUniqueIdToSolverBodyTable,
|
||||||
|
m_btSeed2,
|
||||||
|
m_fixedBodyId,
|
||||||
|
m_maxOverrideNumSolverIterations);
|
||||||
|
|
||||||
|
for (int i = 0; i < numBodies; i++)
|
||||||
|
{
|
||||||
|
int bodyId = siData.getOrInitSolverBody(*bodies[i], infoGlobal.m_timeStep);
|
||||||
|
|
||||||
|
btRigidBody* body = btRigidBody::upcast(bodies[i]);
|
||||||
|
if (body && body->getInvMass())
|
||||||
|
{
|
||||||
|
btSolverBody& solverBody = siData.m_tmpSolverBodyPool[bodyId];
|
||||||
|
solverBody.m_linearVelocity = body->getLinearVelocity() - solverBody.m_deltaLinearVelocity;
|
||||||
|
solverBody.m_angularVelocity = body->getAngularVelocity() - solverBody.m_deltaAngularVelocity;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void btDeformableMultiBodyConstraintSolver::solverBodyWriteBack(const btContactSolverInfo& infoGlobal)
|
||||||
|
{
|
||||||
|
for (int i = 0; i < m_tmpSolverBodyPool.size(); i++)
|
||||||
|
{
|
||||||
|
btRigidBody* body = m_tmpSolverBodyPool[i].m_originalBody;
|
||||||
|
if (body)
|
||||||
|
{
|
||||||
|
m_tmpSolverBodyPool[i].m_originalBody->setLinearVelocity(m_tmpSolverBodyPool[i].m_linearVelocity + m_tmpSolverBodyPool[i].m_deltaLinearVelocity);
|
||||||
|
m_tmpSolverBodyPool[i].m_originalBody->setAngularVelocity(m_tmpSolverBodyPool[i].m_angularVelocity+m_tmpSolverBodyPool[i].m_deltaAngularVelocity);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
@ -22,6 +22,13 @@
|
|||||||
|
|
||||||
class btDeformableBodySolver;
|
class btDeformableBodySolver;
|
||||||
|
|
||||||
|
// btDeformableMultiBodyConstraintSolver extendsn btMultiBodyConstraintSolver to solve for the contact among rigid/multibody and deformable bodies. Notice that the following constraints
|
||||||
|
// 1. rigid/multibody against rigid/multibody
|
||||||
|
// 2. rigid/multibody against deforamble
|
||||||
|
// 3. deformable against deformable
|
||||||
|
// 4. deformable self collision
|
||||||
|
// 5. joint constraints
|
||||||
|
// are all coupled in this solve.
|
||||||
ATTRIBUTE_ALIGNED16(class)
|
ATTRIBUTE_ALIGNED16(class)
|
||||||
btDeformableMultiBodyConstraintSolver : public btMultiBodyConstraintSolver
|
btDeformableMultiBodyConstraintSolver : public btMultiBodyConstraintSolver
|
||||||
{
|
{
|
||||||
@ -32,34 +39,10 @@ protected:
|
|||||||
virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
|
virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
|
||||||
|
|
||||||
// write the velocity of the the solver body to the underlying rigid body
|
// write the velocity of the the solver body to the underlying rigid body
|
||||||
void solverBodyWriteBack(const btContactSolverInfo& infoGlobal)
|
void solverBodyWriteBack(const btContactSolverInfo& infoGlobal);
|
||||||
{
|
|
||||||
for (int i = 0; i < m_tmpSolverBodyPool.size(); i++)
|
|
||||||
{
|
|
||||||
btRigidBody* body = m_tmpSolverBodyPool[i].m_originalBody;
|
|
||||||
if (body)
|
|
||||||
{
|
|
||||||
m_tmpSolverBodyPool[i].m_originalBody->setLinearVelocity(m_tmpSolverBodyPool[i].m_linearVelocity + m_tmpSolverBodyPool[i].m_deltaLinearVelocity);
|
|
||||||
m_tmpSolverBodyPool[i].m_originalBody->setAngularVelocity(m_tmpSolverBodyPool[i].m_angularVelocity+m_tmpSolverBodyPool[i].m_deltaAngularVelocity);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void writeToSolverBody(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal)
|
// write the velocity of the underlying rigid body to the the the solver body
|
||||||
{
|
void writeToSolverBody(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal);
|
||||||
for (int i = 0; i < numBodies; i++)
|
|
||||||
{
|
|
||||||
int bodyId = getOrInitSolverBody(*bodies[i], infoGlobal.m_timeStep);
|
|
||||||
|
|
||||||
btRigidBody* body = btRigidBody::upcast(bodies[i]);
|
|
||||||
if (body && body->getInvMass())
|
|
||||||
{
|
|
||||||
btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId];
|
|
||||||
solverBody.m_linearVelocity = body->getLinearVelocity() - solverBody.m_deltaLinearVelocity;
|
|
||||||
solverBody.m_angularVelocity = body->getAngularVelocity() - solverBody.m_deltaAngularVelocity;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
public:
|
public:
|
||||||
BT_DECLARE_ALIGNED_ALLOCATOR();
|
BT_DECLARE_ALIGNED_ALLOCATOR();
|
||||||
@ -69,23 +52,7 @@ public:
|
|||||||
m_deformableSolver = deformableSolver;
|
m_deformableSolver = deformableSolver;
|
||||||
}
|
}
|
||||||
|
|
||||||
virtual void solveMultiBodyGroup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher)
|
virtual void solveMultiBodyGroup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher);
|
||||||
{
|
|
||||||
m_tmpMultiBodyConstraints = multiBodyConstraints;
|
|
||||||
m_tmpNumMultiBodyConstraints = numMultiBodyConstraints;
|
|
||||||
|
|
||||||
// inherited from MultiBodyConstraintSolver
|
|
||||||
solveGroupCacheFriendlySetup(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer);
|
|
||||||
|
|
||||||
// overriden
|
|
||||||
solveGroupCacheFriendlyIterations(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer);
|
|
||||||
|
|
||||||
// inherited from MultiBodyConstraintSolver
|
|
||||||
solveGroupCacheFriendlyFinish(bodies, numBodies, info);
|
|
||||||
|
|
||||||
m_tmpMultiBodyConstraints = 0;
|
|
||||||
m_tmpNumMultiBodyConstraints = 0;
|
|
||||||
}
|
|
||||||
};
|
};
|
||||||
|
|
||||||
#endif /* BT_DEFORMABLE_MULTIBODY_CONSTRAINT_SOLVER_H */
|
#endif /* BT_DEFORMABLE_MULTIBODY_CONSTRAINT_SOLVER_H */
|
||||||
|
@ -17,7 +17,7 @@ subject to the following restrictions:
|
|||||||
#define BT_NEOHOOKEAN_H
|
#define BT_NEOHOOKEAN_H
|
||||||
|
|
||||||
#include "btDeformableLagrangianForce.h"
|
#include "btDeformableLagrangianForce.h"
|
||||||
|
#include "LinearMath/btQuickprof.h"
|
||||||
// This energy is as described in https://graphics.pixar.com/library/StableElasticity/paper.pdf
|
// This energy is as described in https://graphics.pixar.com/library/StableElasticity/paper.pdf
|
||||||
class btDeformableNeoHookeanForce : public btDeformableLagrangianForce
|
class btDeformableNeoHookeanForce : public btDeformableLagrangianForce
|
||||||
{
|
{
|
||||||
@ -155,8 +155,9 @@ public:
|
|||||||
btSoftBody::Tetra& tetra = psb->m_tetras[j];
|
btSoftBody::Tetra& tetra = psb->m_tetras[j];
|
||||||
btMatrix3x3 P;
|
btMatrix3x3 P;
|
||||||
firstPiola(psb->m_tetraScratches[j],P);
|
firstPiola(psb->m_tetraScratches[j],P);
|
||||||
btVector3 force_on_node0 = P * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col);
|
// btVector3 force_on_node0 = P * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col);
|
||||||
btMatrix3x3 force_on_node123 = P * tetra.m_Dm_inverse.transpose();
|
btMatrix3x3 force_on_node123 = P * tetra.m_Dm_inverse.transpose();
|
||||||
|
btVector3 force_on_node0 = force_on_node123 * grad_N_hat_1st_col;
|
||||||
|
|
||||||
btSoftBody::Node* node0 = tetra.m_n[0];
|
btSoftBody::Node* node0 = tetra.m_n[0];
|
||||||
btSoftBody::Node* node1 = tetra.m_n[1];
|
btSoftBody::Node* node1 = tetra.m_n[1];
|
||||||
@ -202,8 +203,9 @@ public:
|
|||||||
btMatrix3x3 dF = Ds(id0, id1, id2, id3, dv) * tetra.m_Dm_inverse;
|
btMatrix3x3 dF = Ds(id0, id1, id2, id3, dv) * tetra.m_Dm_inverse;
|
||||||
btMatrix3x3 dP;
|
btMatrix3x3 dP;
|
||||||
firstPiolaDampingDifferential(psb->m_tetraScratchesTn[j], dF, dP);
|
firstPiolaDampingDifferential(psb->m_tetraScratchesTn[j], dF, dP);
|
||||||
btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col);
|
// btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col);
|
||||||
btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose();
|
btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose();
|
||||||
|
btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col;
|
||||||
|
|
||||||
// damping force differential
|
// damping force differential
|
||||||
btScalar scale1 = scale * tetra.m_element_measure;
|
btScalar scale1 = scale * tetra.m_element_measure;
|
||||||
@ -237,8 +239,9 @@ public:
|
|||||||
btMatrix3x3 dF = Ds(id0, id1, id2, id3, dx) * tetra.m_Dm_inverse;
|
btMatrix3x3 dF = Ds(id0, id1, id2, id3, dx) * tetra.m_Dm_inverse;
|
||||||
btMatrix3x3 dP;
|
btMatrix3x3 dP;
|
||||||
firstPiolaDifferential(psb->m_tetraScratches[j], dF, dP);
|
firstPiolaDifferential(psb->m_tetraScratches[j], dF, dP);
|
||||||
btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col);
|
// btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col);
|
||||||
btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose();
|
btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose();
|
||||||
|
btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col;
|
||||||
|
|
||||||
// elastic force differential
|
// elastic force differential
|
||||||
btScalar scale1 = scale * tetra.m_element_measure;
|
btScalar scale1 = scale * tetra.m_element_measure;
|
||||||
@ -252,27 +255,33 @@ public:
|
|||||||
|
|
||||||
void firstPiola(const btSoftBody::TetraScratch& s, btMatrix3x3& P)
|
void firstPiola(const btSoftBody::TetraScratch& s, btMatrix3x3& P)
|
||||||
{
|
{
|
||||||
P = s.m_F * m_mu * ( 1. - 1. / (s.m_trace + 1.)) + s.m_cofF * (m_lambda * (s.m_J - 1.) - 0.75 * m_mu);
|
btScalar c1 = (m_mu * ( 1. - 1. / (s.m_trace + 1.)));
|
||||||
|
btScalar c2 = (m_lambda * (s.m_J - 1.) - 0.75 * m_mu);
|
||||||
|
P = s.m_F * c1 + s.m_cofF * c2;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Let P be the first piola stress.
|
// Let P be the first piola stress.
|
||||||
// This function calculates the dP = dP/dF * dF
|
// This function calculates the dP = dP/dF * dF
|
||||||
void firstPiolaDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF, btMatrix3x3& dP)
|
void firstPiolaDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF, btMatrix3x3& dP)
|
||||||
{
|
{
|
||||||
dP = dF * m_mu * ( 1. - 1. / (s.m_trace + 1.)) + s.m_F * (2*m_mu) * DotProduct(s.m_F, dF) * (1./((1.+s.m_trace)*(1.+s.m_trace)));
|
btScalar c1 = m_mu * ( 1. - 1. / (s.m_trace + 1.));
|
||||||
|
btScalar c2 = (2.*m_mu) * DotProduct(s.m_F, dF) * (1./((1.+s.m_trace)*(1.+s.m_trace)));
|
||||||
|
btScalar c3 = (m_lambda * DotProduct(s.m_cofF, dF));
|
||||||
|
dP = dF * c1 + s.m_F * c2;
|
||||||
addScaledCofactorMatrixDifferential(s.m_F, dF, m_lambda*(s.m_J-1.) - 0.75*m_mu, dP);
|
addScaledCofactorMatrixDifferential(s.m_F, dF, m_lambda*(s.m_J-1.) - 0.75*m_mu, dP);
|
||||||
dP += s.m_cofF * m_lambda * DotProduct(s.m_cofF, dF);
|
dP += s.m_cofF * c3;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Let Q be the damping stress.
|
// Let Q be the damping stress.
|
||||||
// This function calculates the dP = dQ/dF * dF
|
// This function calculates the dP = dQ/dF * dF
|
||||||
void firstPiolaDampingDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF, btMatrix3x3& dP)
|
void firstPiolaDampingDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF, btMatrix3x3& dP)
|
||||||
{
|
{
|
||||||
dP = dF * m_mu_damp * ( 1. - 1. / (s.m_trace + 1.)) + s.m_F * (2*m_mu_damp) * DotProduct(s.m_F, dF) * (1./((1.+s.m_trace)*(1.+s.m_trace)));
|
btScalar c1 = (m_mu_damp * ( 1. - 1. / (s.m_trace + 1.)));
|
||||||
|
btScalar c2 = ((2.*m_mu_damp) * DotProduct(s.m_F, dF) *(1./((1.+s.m_trace)*(1.+s.m_trace))));
|
||||||
|
btScalar c3 = (m_lambda_damp * DotProduct(s.m_cofF, dF));
|
||||||
|
dP = dF * c1 + s.m_F * c2;
|
||||||
addScaledCofactorMatrixDifferential(s.m_F, dF, m_lambda_damp*(s.m_J-1.) - 0.75*m_mu_damp, dP);
|
addScaledCofactorMatrixDifferential(s.m_F, dF, m_lambda_damp*(s.m_J-1.) - 0.75*m_mu_damp, dP);
|
||||||
dP += s.m_cofF * m_lambda_damp * DotProduct(s.m_cofF, dF);
|
dP += s.m_cofF * c3;
|
||||||
}
|
}
|
||||||
|
|
||||||
btScalar DotProduct(const btMatrix3x3& A, const btMatrix3x3& B)
|
btScalar DotProduct(const btMatrix3x3& A, const btMatrix3x3& B)
|
||||||
|
Loading…
Reference in New Issue
Block a user