mirror of
https://github.com/bulletphysics/bullet3
synced 2024-12-14 13:50:04 +00:00
documentation and optimization
This commit is contained in:
Xuchen Han
parent
20abbc9ee7
commit
657a7468b3
@ -153,7 +153,7 @@ void DeformableSelfCollision::initPhysics()
|
||||
btVector3(-s, h, +4*s),
|
||||
btVector3(+s, h, +4*s),
|
||||
10,40,
|
||||
0, true, 0.1);
|
||||
0, true, 0.01);
|
||||
|
||||
|
||||
psb->getCollisionShape()->setMargin(0.2);
|
||||
@ -161,7 +161,7 @@ void DeformableSelfCollision::initPhysics()
|
||||
psb->setTotalMass(1);
|
||||
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.02;
|
||||
psb->m_cfg.kDF = 0.2;
|
||||
psb->rotate(btQuaternion(0,SIMD_PI / 2, 0));
|
||||
psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
|
||||
psb->m_cfg.collisions |= btSoftBody::fCollision::VF_DD;
|
||||
|
||||
@ -91,7 +91,7 @@ public:
|
||||
|
||||
void Ctor_RbUpStack(int count)
|
||||
{
|
||||
float mass = 0.02;
|
||||
float mass = 1;
|
||||
|
||||
btCompoundShape* cylinderCompound = new btCompoundShape;
|
||||
btCollisionShape* cylinderShape = new btCylinderShapeX(btVector3(2, .5, .5));
|
||||
@ -166,7 +166,7 @@ void VolumetricDeformable::initPhysics()
|
||||
m_solver = sol;
|
||||
|
||||
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);
|
||||
getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity;
|
||||
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
|
||||
@ -196,7 +196,6 @@ void VolumetricDeformable::initPhysics()
|
||||
//add the ground to the dynamics world
|
||||
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));
|
||||
@ -220,19 +219,17 @@ void VolumetricDeformable::initPhysics()
|
||||
psb->m_cfg.kDF = 0.5;
|
||||
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);
|
||||
getDeformableDynamicsWorld()->addForce(psb, 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);
|
||||
m_forces.push_back(neohookean);
|
||||
|
||||
}
|
||||
getDeformableDynamicsWorld()->setImplicit(true);
|
||||
getDeformableDynamicsWorld()->setLineSearch(false);
|
||||
// add a few rigid bodies
|
||||
Ctor_RbUpStack(4);
|
||||
|
||||
|
||||
@ -69,7 +69,7 @@ public:
|
||||
if (dot(p,temp) < SIMD_EPSILON)
|
||||
{
|
||||
if (verbose)
|
||||
std::cout << "Encountered negative direction in CG!"<<std::endl;
|
||||
std::cout << "Encountered negative direction in CG!" << std::endl;
|
||||
if (k == 1)
|
||||
{
|
||||
x = b;
|
||||
|
||||
@ -22,7 +22,7 @@
|
||||
btDeformableBodySolver::btDeformableBodySolver()
|
||||
: m_numNodes(0)
|
||||
, m_cg(20)
|
||||
, m_maxNewtonIterations(10)
|
||||
, m_maxNewtonIterations(5)
|
||||
, m_newtonTolerance(1e-4)
|
||||
, m_lineSearch(true)
|
||||
{
|
||||
@ -412,6 +412,7 @@ void btDeformableBodySolver::predictDeformableMotion(btSoftBody* psb, btScalar d
|
||||
psb->m_fdbvt.optimizeIncremental(1);
|
||||
}
|
||||
|
||||
|
||||
void btDeformableBodySolver::updateSoftBodies()
|
||||
{
|
||||
for (int i = 0; i < m_softBodies.size(); i++)
|
||||
@ -419,7 +420,7 @@ void btDeformableBodySolver::updateSoftBodies()
|
||||
btSoftBody *psb = (btSoftBody *)m_softBodies[i];
|
||||
if (psb->isActive())
|
||||
{
|
||||
psb->updateNormals(); // normal is updated here
|
||||
psb->updateNormals();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -29,26 +29,24 @@ class btDeformableMultiBodyDynamicsWorld;
|
||||
|
||||
class btDeformableBodySolver : public btSoftBodySolver
|
||||
{
|
||||
// using TVStack = btAlignedObjectArray<btVector3>;
|
||||
typedef btAlignedObjectArray<btVector3> TVStack;
|
||||
protected:
|
||||
int m_numNodes;
|
||||
TVStack m_dv;
|
||||
TVStack m_backup_dv;
|
||||
TVStack m_ddv;
|
||||
TVStack m_residual;
|
||||
btAlignedObjectArray<btSoftBody *> m_softBodies;
|
||||
|
||||
btAlignedObjectArray<btVector3> m_backupVelocity;
|
||||
btScalar m_dt;
|
||||
btScalar m_contact_iterations;
|
||||
btConjugateGradient<btDeformableBackwardEulerObjective> m_cg;
|
||||
bool m_implicit;
|
||||
int m_maxNewtonIterations;
|
||||
btScalar m_newtonTolerance;
|
||||
bool m_lineSearch;
|
||||
int m_numNodes; // total number of deformable body nodes
|
||||
TVStack m_dv; // v_{n+1} - v_n
|
||||
TVStack m_backup_dv; // backed up dv
|
||||
TVStack m_ddv; // incremental dv
|
||||
TVStack m_residual; // rhs of the linear solve
|
||||
btAlignedObjectArray<btSoftBody *> m_softBodies; // all deformable bodies
|
||||
TVStack m_backupVelocity; // backed up v, equals v_n for implicit, equals v_{n+1}^* for explicit
|
||||
btScalar m_dt; // dt
|
||||
btConjugateGradient<btDeformableBackwardEulerObjective> m_cg; // CG solver
|
||||
bool m_implicit; // use implicit scheme if true, explicit scheme if false
|
||||
int m_maxNewtonIterations; // max number of newton iterations
|
||||
btScalar m_newtonTolerance; // stop newton iterations if f(x) < m_newtonTolerance
|
||||
bool m_lineSearch; // If true, use newton's method with line search under implicit scheme
|
||||
|
||||
public:
|
||||
// handles data related to objective function
|
||||
btDeformableBackwardEulerObjective* m_objective;
|
||||
|
||||
btDeformableBodySolver();
|
||||
@ -60,49 +58,62 @@ public:
|
||||
return DEFORMABLE_SOLVER;
|
||||
}
|
||||
|
||||
// update soft body normals
|
||||
virtual void updateSoftBodies();
|
||||
|
||||
virtual void copyBackToSoftBodies(bool bMove = true) {}
|
||||
|
||||
// solve the momentum equation
|
||||
virtual void solveDeformableConstraints(btScalar solverdt);
|
||||
|
||||
// solve the contact between deformable and rigid as well as among deformables
|
||||
btScalar solveContactConstraints();
|
||||
|
||||
virtual void solveConstraints(btScalar dt){}
|
||||
|
||||
|
||||
// resize/clear data structures
|
||||
void reinitialize(const btAlignedObjectArray<btSoftBody *>& softBodies, btScalar dt);
|
||||
|
||||
// set up contact constraints
|
||||
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);
|
||||
|
||||
// save the current velocity to m_backupVelocity
|
||||
void backupVelocity();
|
||||
|
||||
// set m_dv and m_backupVelocity to desired value to prepare for momentum solve
|
||||
void setupDeformableSolve(bool implicit);
|
||||
|
||||
// set the current velocity to that backed up in m_backupVelocity
|
||||
void revertVelocity();
|
||||
|
||||
// set velocity to m_dv + m_backupVelocity
|
||||
void updateVelocity();
|
||||
|
||||
// update the node count
|
||||
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);
|
||||
|
||||
virtual void predictMotion(btScalar solverdt);
|
||||
|
||||
virtual void copySoftBodyToVertexBuffer(const btSoftBody *const softBody, btVertexBufferDescriptor *vertexBuffer) {}
|
||||
|
||||
// process collision between deformable and rigid
|
||||
virtual void processCollision(btSoftBody * softBody, const btCollisionObjectWrapper * collisionObjectWrap)
|
||||
{
|
||||
softBody->defaultCollisionHandler(collisionObjectWrap);
|
||||
}
|
||||
|
||||
|
||||
// process collision between deformable and deformable
|
||||
virtual void processCollision(btSoftBody * softBody, btSoftBody * 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.
|
||||
// Otherwise, explicit time stepping scheme is used
|
||||
void setImplicit(bool implicit);
|
||||
@ -136,6 +147,12 @@ public:
|
||||
// 1/2 * dv^T * M * dv
|
||||
// used in line search
|
||||
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 */
|
||||
|
||||
@ -26,8 +26,6 @@ class btDeformableContactProjection
|
||||
{
|
||||
public:
|
||||
typedef btAlignedObjectArray<btVector3> TVStack;
|
||||
typedef btAlignedObjectArray<btAlignedObjectArray<btVector3> > TVArrayStack;
|
||||
typedef btAlignedObjectArray<btAlignedObjectArray<btScalar> > TArrayStack;
|
||||
btAlignedObjectArray<btSoftBody *>& m_softBodies;
|
||||
|
||||
// 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);
|
||||
|
||||
// add to friction
|
||||
// add friction force to the rhs of the linear solve
|
||||
virtual void applyDynamicFriction(TVStack& f);
|
||||
|
||||
// update the constraints
|
||||
|
||||
@ -67,6 +67,7 @@ public:
|
||||
{
|
||||
}
|
||||
|
||||
// get number of nodes that have the force
|
||||
virtual int getNumNodes()
|
||||
{
|
||||
int numNodes = 0;
|
||||
@ -94,10 +95,7 @@ public:
|
||||
btVector3 c1 = dx[id1] - dx[id0];
|
||||
btVector3 c2 = dx[id2] - dx[id0];
|
||||
btVector3 c3 = dx[id3] - dx[id0];
|
||||
btMatrix3x3 dF(c1.getX(), c2.getX(), c3.getX(),
|
||||
c1.getY(), c2.getY(), c3.getY(),
|
||||
c1.getZ(), c2.getZ(), c3.getZ());
|
||||
return dF;
|
||||
return btMatrix3x3(c1,c2,c3).transpose();
|
||||
}
|
||||
|
||||
// Calculate the incremental deformable generated from the current velocity
|
||||
@ -106,12 +104,10 @@ public:
|
||||
btVector3 c1 = n1->m_v - n0->m_v;
|
||||
btVector3 c2 = n2->m_v - n0->m_v;
|
||||
btVector3 c3 = n3->m_v - n0->m_v;
|
||||
btMatrix3x3 dF(c1.getX(), c2.getX(), c3.getX(),
|
||||
c1.getY(), c2.getY(), c3.getY(),
|
||||
c1.getZ(), c2.getZ(), c3.getZ());
|
||||
return dF;
|
||||
return btMatrix3x3(c1,c2,c3).transpose();
|
||||
}
|
||||
|
||||
// test for addScaledElasticForce function
|
||||
virtual void testDerivative()
|
||||
{
|
||||
for (int i = 0; i<m_softBodies.size();++i)
|
||||
@ -222,6 +218,7 @@ public:
|
||||
}
|
||||
}
|
||||
|
||||
// test for addScaledElasticForce function
|
||||
virtual void testHessian()
|
||||
{
|
||||
for (int i = 0; i<m_softBodies.size();++i)
|
||||
|
||||
@ -20,6 +20,8 @@
|
||||
|
||||
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;
|
||||
btScalar m_elasticStiffness, m_dampingStiffness;
|
||||
public:
|
||||
|
||||
@ -59,3 +59,67 @@ btScalar btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlyIteration
|
||||
}
|
||||
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;
|
||||
|
||||
// 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)
|
||||
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);
|
||||
|
||||
// write the velocity of the the solver body to the underlying rigid body
|
||||
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)
|
||||
{
|
||||
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;
|
||||
}
|
||||
}
|
||||
}
|
||||
void solverBodyWriteBack(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);
|
||||
|
||||
public:
|
||||
BT_DECLARE_ALIGNED_ALLOCATOR();
|
||||
@ -69,23 +52,7 @@ public:
|
||||
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)
|
||||
{
|
||||
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;
|
||||
}
|
||||
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);
|
||||
};
|
||||
|
||||
#endif /* BT_DEFORMABLE_MULTIBODY_CONSTRAINT_SOLVER_H */
|
||||
|
||||
@ -17,7 +17,7 @@ subject to the following restrictions:
|
||||
#define BT_NEOHOOKEAN_H
|
||||
|
||||
#include "btDeformableLagrangianForce.h"
|
||||
|
||||
#include "LinearMath/btQuickprof.h"
|
||||
// This energy is as described in https://graphics.pixar.com/library/StableElasticity/paper.pdf
|
||||
class btDeformableNeoHookeanForce : public btDeformableLagrangianForce
|
||||
{
|
||||
@ -155,8 +155,9 @@ public:
|
||||
btSoftBody::Tetra& tetra = psb->m_tetras[j];
|
||||
btMatrix3x3 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();
|
||||
btVector3 force_on_node0 = force_on_node123 * grad_N_hat_1st_col;
|
||||
|
||||
btSoftBody::Node* node0 = tetra.m_n[0];
|
||||
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 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();
|
||||
btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col;
|
||||
|
||||
// damping force differential
|
||||
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 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();
|
||||
btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col;
|
||||
|
||||
// elastic force differential
|
||||
btScalar scale1 = scale * tetra.m_element_measure;
|
||||
@ -252,27 +255,33 @@ public:
|
||||
|
||||
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.
|
||||
// This function calculates the dP = dP/dF * dF
|
||||
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);
|
||||
dP += s.m_cofF * m_lambda * DotProduct(s.m_cofF, dF);
|
||||
dP += s.m_cofF * c3;
|
||||
}
|
||||
|
||||
// Let Q be the damping stress.
|
||||
// This function calculates the dP = dQ/dF * dF
|
||||
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);
|
||||
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)
|
||||
|
||||
Loading…
Reference in New Issue
Block a user