summaryrefslogtreecommitdiff
path: root/thirdparty/bullet/BulletSoftBody/btDeformableLagrangianForce.h
diff options
context:
space:
mode:
Diffstat (limited to 'thirdparty/bullet/BulletSoftBody/btDeformableLagrangianForce.h')
-rw-r--r--thirdparty/bullet/BulletSoftBody/btDeformableLagrangianForce.h675
1 files changed, 337 insertions, 338 deletions
diff --git a/thirdparty/bullet/BulletSoftBody/btDeformableLagrangianForce.h b/thirdparty/bullet/BulletSoftBody/btDeformableLagrangianForce.h
index 0b6447442d..d58d825d1c 100644
--- a/thirdparty/bullet/BulletSoftBody/btDeformableLagrangianForce.h
+++ b/thirdparty/bullet/BulletSoftBody/btDeformableLagrangianForce.h
@@ -22,352 +22,351 @@
enum btDeformableLagrangianForceType
{
- BT_GRAVITY_FORCE = 1,
- BT_MASSSPRING_FORCE = 2,
- BT_COROTATED_FORCE = 3,
- BT_NEOHOOKEAN_FORCE = 4,
- BT_LINEAR_ELASTICITY_FORCE = 5,
- BT_MOUSE_PICKING_FORCE = 6
+ BT_GRAVITY_FORCE = 1,
+ BT_MASSSPRING_FORCE = 2,
+ BT_COROTATED_FORCE = 3,
+ BT_NEOHOOKEAN_FORCE = 4,
+ BT_LINEAR_ELASTICITY_FORCE = 5,
+ BT_MOUSE_PICKING_FORCE = 6
};
static inline double randomDouble(double low, double high)
{
- return low + static_cast<double>(rand()) / RAND_MAX * (high - low);
+ return low + static_cast<double>(rand()) / RAND_MAX * (high - low);
}
class btDeformableLagrangianForce
{
public:
- typedef btAlignedObjectArray<btVector3> TVStack;
- btAlignedObjectArray<btSoftBody *> m_softBodies;
- const btAlignedObjectArray<btSoftBody::Node*>* m_nodes;
-
- btDeformableLagrangianForce()
- {
- }
-
- virtual ~btDeformableLagrangianForce(){}
-
- // add all forces
- virtual void addScaledForces(btScalar scale, TVStack& force) = 0;
-
- // add damping df
- virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) = 0;
-
- // build diagonal of A matrix
- virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA) = 0;
-
- // add elastic df
- virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) = 0;
-
- // add all forces that are explicit in explicit solve
- virtual void addScaledExplicitForce(btScalar scale, TVStack& force) = 0;
-
- // add all damping forces
- virtual void addScaledDampingForce(btScalar scale, TVStack& force) = 0;
-
- virtual btDeformableLagrangianForceType getForceType() = 0;
-
- virtual void reinitialize(bool nodeUpdated)
- {
- }
-
- // get number of nodes that have the force
- virtual int getNumNodes()
- {
- int numNodes = 0;
- for (int i = 0; i < m_softBodies.size(); ++i)
- {
- numNodes += m_softBodies[i]->m_nodes.size();
- }
- return numNodes;
- }
-
- // add a soft body to be affected by the particular lagrangian force
- virtual void addSoftBody(btSoftBody* psb)
- {
- m_softBodies.push_back(psb);
- }
-
- virtual void removeSoftBody(btSoftBody* psb)
- {
- m_softBodies.remove(psb);
- }
-
- virtual void setIndices(const btAlignedObjectArray<btSoftBody::Node*>* nodes)
- {
- m_nodes = nodes;
- }
-
- // Calculate the incremental deformable generated from the input dx
- virtual btMatrix3x3 Ds(int id0, int id1, int id2, int id3, const TVStack& dx)
- {
- btVector3 c1 = dx[id1] - dx[id0];
- btVector3 c2 = dx[id2] - dx[id0];
- btVector3 c3 = dx[id3] - dx[id0];
- return btMatrix3x3(c1,c2,c3).transpose();
- }
-
- // Calculate the incremental deformable generated from the current velocity
- virtual btMatrix3x3 DsFromVelocity(const btSoftBody::Node* n0, const btSoftBody::Node* n1, const btSoftBody::Node* n2, const btSoftBody::Node* n3)
- {
- btVector3 c1 = n1->m_v - n0->m_v;
- btVector3 c2 = n2->m_v - n0->m_v;
- btVector3 c3 = n3->m_v - n0->m_v;
- return btMatrix3x3(c1,c2,c3).transpose();
- }
-
- // test for addScaledElasticForce function
- virtual void testDerivative()
- {
- for (int i = 0; i<m_softBodies.size();++i)
- {
- btSoftBody* psb = m_softBodies[i];
- for (int j = 0; j < psb->m_nodes.size(); ++j)
- {
- psb->m_nodes[j].m_q += btVector3(randomDouble(-.1, .1), randomDouble(-.1, .1), randomDouble(-.1, .1));
- }
- psb->updateDeformation();
- }
-
- TVStack dx;
- dx.resize(getNumNodes());
- TVStack dphi_dx;
- dphi_dx.resize(dx.size());
- for (int i =0; i < dphi_dx.size();++i)
- {
- dphi_dx[i].setZero();
- }
- addScaledForces(-1, dphi_dx);
-
- // write down the current position
- TVStack x;
- x.resize(dx.size());
- int counter = 0;
- for (int i = 0; i<m_softBodies.size();++i)
- {
- btSoftBody* psb = m_softBodies[i];
- for (int j = 0; j < psb->m_nodes.size(); ++j)
- {
- x[counter] = psb->m_nodes[j].m_q;
- counter++;
- }
- }
- counter = 0;
-
- // populate dx with random vectors
- for (int i = 0; i < dx.size(); ++i)
- {
- dx[i].setX(randomDouble(-1, 1));
- dx[i].setY(randomDouble(-1, 1));
- dx[i].setZ(randomDouble(-1, 1));
- }
-
- btAlignedObjectArray<double> errors;
- for (int it = 0; it < 10; ++it)
- {
- for (int i = 0; i < dx.size(); ++i)
- {
- dx[i] *= 0.5;
- }
-
- // get dphi/dx * dx
- double dphi = 0;
- for (int i = 0; i < dx.size(); ++i)
- {
- dphi += dphi_dx[i].dot(dx[i]);
- }
-
-
- for (int i = 0; i<m_softBodies.size();++i)
- {
- btSoftBody* psb = m_softBodies[i];
- for (int j = 0; j < psb->m_nodes.size(); ++j)
- {
- psb->m_nodes[j].m_q = x[counter] + dx[counter];
- counter++;
- }
- psb->updateDeformation();
- }
- counter = 0;
- double f1 = totalElasticEnergy(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)
- {
- psb->m_nodes[j].m_q = x[counter] - dx[counter];
- counter++;
- }
- psb->updateDeformation();
- }
- counter = 0;
-
- double f2 = totalElasticEnergy(0);
-
- //restore m_q
- for (int i = 0; i<m_softBodies.size();++i)
- {
- btSoftBody* psb = m_softBodies[i];
- for (int j = 0; j < psb->m_nodes.size(); ++j)
- {
- psb->m_nodes[j].m_q = x[counter];
- counter++;
- }
- psb->updateDeformation();
- }
- counter = 0;
- double error = f1-f2-2*dphi;
- errors.push_back(error);
- std::cout << "Iteration = " << it <<", f1 = " << f1 << ", f2 = " << f2 << ", error = " << error << std::endl;
- }
- for (int i = 1; i < errors.size(); ++i)
- {
- std::cout << "Iteration = " << i << ", ratio = " << errors[i-1]/errors[i] << std::endl;
- }
- }
-
- // test for addScaledElasticForce function
- virtual void testHessian()
- {
- for (int i = 0; i<m_softBodies.size();++i)
- {
- btSoftBody* psb = m_softBodies[i];
- for (int j = 0; j < psb->m_nodes.size(); ++j)
- {
- psb->m_nodes[j].m_q += btVector3(randomDouble(-.1, .1), randomDouble(-.1, .1), randomDouble(-.1, .1));
- }
- psb->updateDeformation();
- }
-
-
- TVStack dx;
- dx.resize(getNumNodes());
- TVStack df;
- df.resize(dx.size());
- TVStack f1;
- f1.resize(dx.size());
- TVStack f2;
- f2.resize(dx.size());
-
-
- // write down the current position
- TVStack x;
- x.resize(dx.size());
- int counter = 0;
- for (int i = 0; i<m_softBodies.size();++i)
- {
- btSoftBody* psb = m_softBodies[i];
- for (int j = 0; j < psb->m_nodes.size(); ++j)
- {
- x[counter] = psb->m_nodes[j].m_q;
- counter++;
- }
- }
- counter = 0;
-
- // populate dx with random vectors
- for (int i = 0; i < dx.size(); ++i)
- {
- dx[i].setX(randomDouble(-1, 1));
- dx[i].setY(randomDouble(-1, 1));
- dx[i].setZ(randomDouble(-1, 1));
- }
-
- btAlignedObjectArray<double> errors;
- for (int it = 0; it < 10; ++it)
- {
- for (int i = 0; i < dx.size(); ++i)
- {
- dx[i] *= 0.5;
- }
-
- // get df
- for (int i =0; i < df.size();++i)
- {
- df[i].setZero();
- f1[i].setZero();
- f2[i].setZero();
- }
-
- //set df
- addScaledElasticForceDifferential(-1, dx, df);
-
- for (int i = 0; i<m_softBodies.size();++i)
- {
- btSoftBody* psb = m_softBodies[i];
- for (int j = 0; j < psb->m_nodes.size(); ++j)
- {
- psb->m_nodes[j].m_q = x[counter] + dx[counter];
- counter++;
- }
- psb->updateDeformation();
- }
- counter = 0;
-
- //set f1
- addScaledForces(-1, f1);
-
- for (int i = 0; i<m_softBodies.size();++i)
- {
- btSoftBody* psb = m_softBodies[i];
- for (int j = 0; j < psb->m_nodes.size(); ++j)
- {
- psb->m_nodes[j].m_q = x[counter] - dx[counter];
- counter++;
- }
- psb->updateDeformation();
- }
- counter = 0;
-
- //set f2
- addScaledForces(-1, f2);
-
- //restore m_q
- for (int i = 0; i<m_softBodies.size();++i)
- {
- btSoftBody* psb = m_softBodies[i];
- for (int j = 0; j < psb->m_nodes.size(); ++j)
- {
- psb->m_nodes[j].m_q = x[counter];
- counter++;
- }
- psb->updateDeformation();
- }
- counter = 0;
- double error = 0;
- for (int i = 0; i < df.size();++i)
- {
- btVector3 error_vector = f1[i]-f2[i]-2*df[i];
- error += error_vector.length2();
- }
- error = btSqrt(error);
- errors.push_back(error);
- std::cout << "Iteration = " << it << ", error = " << error << std::endl;
- }
- for (int i = 1; i < errors.size(); ++i)
- {
- std::cout << "Iteration = " << i << ", ratio = " << errors[i-1]/errors[i] << std::endl;
- }
- }
-
- //
- virtual double totalElasticEnergy(btScalar dt)
- {
- return 0;
- }
-
- //
- virtual double totalDampingEnergy(btScalar dt)
- {
- return 0;
- }
-
- // total Energy takes dt as input because certain energies depend on dt
- virtual double totalEnergy(btScalar dt)
- {
- return totalElasticEnergy(dt) + totalDampingEnergy(dt);
- }
+ typedef btAlignedObjectArray<btVector3> TVStack;
+ btAlignedObjectArray<btSoftBody*> m_softBodies;
+ const btAlignedObjectArray<btSoftBody::Node*>* m_nodes;
+
+ btDeformableLagrangianForce()
+ {
+ }
+
+ virtual ~btDeformableLagrangianForce() {}
+
+ // add all forces
+ virtual void addScaledForces(btScalar scale, TVStack& force) = 0;
+
+ // add damping df
+ virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) = 0;
+
+ // build diagonal of A matrix
+ virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA) = 0;
+
+ // add elastic df
+ virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) = 0;
+
+ // add all forces that are explicit in explicit solve
+ virtual void addScaledExplicitForce(btScalar scale, TVStack& force) = 0;
+
+ // add all damping forces
+ virtual void addScaledDampingForce(btScalar scale, TVStack& force) = 0;
+
+ virtual void addScaledHessian(btScalar scale) {}
+
+ virtual btDeformableLagrangianForceType getForceType() = 0;
+
+ virtual void reinitialize(bool nodeUpdated)
+ {
+ }
+
+ // get number of nodes that have the force
+ virtual int getNumNodes()
+ {
+ int numNodes = 0;
+ for (int i = 0; i < m_softBodies.size(); ++i)
+ {
+ numNodes += m_softBodies[i]->m_nodes.size();
+ }
+ return numNodes;
+ }
+
+ // add a soft body to be affected by the particular lagrangian force
+ virtual void addSoftBody(btSoftBody* psb)
+ {
+ m_softBodies.push_back(psb);
+ }
+
+ virtual void removeSoftBody(btSoftBody* psb)
+ {
+ m_softBodies.remove(psb);
+ }
+
+ virtual void setIndices(const btAlignedObjectArray<btSoftBody::Node*>* nodes)
+ {
+ m_nodes = nodes;
+ }
+
+ // Calculate the incremental deformable generated from the input dx
+ virtual btMatrix3x3 Ds(int id0, int id1, int id2, int id3, const TVStack& dx)
+ {
+ btVector3 c1 = dx[id1] - dx[id0];
+ btVector3 c2 = dx[id2] - dx[id0];
+ btVector3 c3 = dx[id3] - dx[id0];
+ return btMatrix3x3(c1, c2, c3).transpose();
+ }
+
+ // Calculate the incremental deformable generated from the current velocity
+ virtual btMatrix3x3 DsFromVelocity(const btSoftBody::Node* n0, const btSoftBody::Node* n1, const btSoftBody::Node* n2, const btSoftBody::Node* n3)
+ {
+ btVector3 c1 = n1->m_v - n0->m_v;
+ btVector3 c2 = n2->m_v - n0->m_v;
+ btVector3 c3 = n3->m_v - n0->m_v;
+ return btMatrix3x3(c1, c2, c3).transpose();
+ }
+
+ // test for addScaledElasticForce function
+ virtual void testDerivative()
+ {
+ for (int i = 0; i < m_softBodies.size(); ++i)
+ {
+ btSoftBody* psb = m_softBodies[i];
+ for (int j = 0; j < psb->m_nodes.size(); ++j)
+ {
+ psb->m_nodes[j].m_q += btVector3(randomDouble(-.1, .1), randomDouble(-.1, .1), randomDouble(-.1, .1));
+ }
+ psb->updateDeformation();
+ }
+
+ TVStack dx;
+ dx.resize(getNumNodes());
+ TVStack dphi_dx;
+ dphi_dx.resize(dx.size());
+ for (int i = 0; i < dphi_dx.size(); ++i)
+ {
+ dphi_dx[i].setZero();
+ }
+ addScaledForces(-1, dphi_dx);
+
+ // write down the current position
+ TVStack x;
+ x.resize(dx.size());
+ int counter = 0;
+ for (int i = 0; i < m_softBodies.size(); ++i)
+ {
+ btSoftBody* psb = m_softBodies[i];
+ for (int j = 0; j < psb->m_nodes.size(); ++j)
+ {
+ x[counter] = psb->m_nodes[j].m_q;
+ counter++;
+ }
+ }
+ counter = 0;
+
+ // populate dx with random vectors
+ for (int i = 0; i < dx.size(); ++i)
+ {
+ dx[i].setX(randomDouble(-1, 1));
+ dx[i].setY(randomDouble(-1, 1));
+ dx[i].setZ(randomDouble(-1, 1));
+ }
+
+ btAlignedObjectArray<double> errors;
+ for (int it = 0; it < 10; ++it)
+ {
+ for (int i = 0; i < dx.size(); ++i)
+ {
+ dx[i] *= 0.5;
+ }
+
+ // get dphi/dx * dx
+ double dphi = 0;
+ for (int i = 0; i < dx.size(); ++i)
+ {
+ dphi += dphi_dx[i].dot(dx[i]);
+ }
+
+ for (int i = 0; i < m_softBodies.size(); ++i)
+ {
+ btSoftBody* psb = m_softBodies[i];
+ for (int j = 0; j < psb->m_nodes.size(); ++j)
+ {
+ psb->m_nodes[j].m_q = x[counter] + dx[counter];
+ counter++;
+ }
+ psb->updateDeformation();
+ }
+ counter = 0;
+ double f1 = totalElasticEnergy(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)
+ {
+ psb->m_nodes[j].m_q = x[counter] - dx[counter];
+ counter++;
+ }
+ psb->updateDeformation();
+ }
+ counter = 0;
+
+ double f2 = totalElasticEnergy(0);
+
+ //restore m_q
+ for (int i = 0; i < m_softBodies.size(); ++i)
+ {
+ btSoftBody* psb = m_softBodies[i];
+ for (int j = 0; j < psb->m_nodes.size(); ++j)
+ {
+ psb->m_nodes[j].m_q = x[counter];
+ counter++;
+ }
+ psb->updateDeformation();
+ }
+ counter = 0;
+ double error = f1 - f2 - 2 * dphi;
+ errors.push_back(error);
+ std::cout << "Iteration = " << it << ", f1 = " << f1 << ", f2 = " << f2 << ", error = " << error << std::endl;
+ }
+ for (int i = 1; i < errors.size(); ++i)
+ {
+ std::cout << "Iteration = " << i << ", ratio = " << errors[i - 1] / errors[i] << std::endl;
+ }
+ }
+
+ // test for addScaledElasticForce function
+ virtual void testHessian()
+ {
+ for (int i = 0; i < m_softBodies.size(); ++i)
+ {
+ btSoftBody* psb = m_softBodies[i];
+ for (int j = 0; j < psb->m_nodes.size(); ++j)
+ {
+ psb->m_nodes[j].m_q += btVector3(randomDouble(-.1, .1), randomDouble(-.1, .1), randomDouble(-.1, .1));
+ }
+ psb->updateDeformation();
+ }
+
+ TVStack dx;
+ dx.resize(getNumNodes());
+ TVStack df;
+ df.resize(dx.size());
+ TVStack f1;
+ f1.resize(dx.size());
+ TVStack f2;
+ f2.resize(dx.size());
+
+ // write down the current position
+ TVStack x;
+ x.resize(dx.size());
+ int counter = 0;
+ for (int i = 0; i < m_softBodies.size(); ++i)
+ {
+ btSoftBody* psb = m_softBodies[i];
+ for (int j = 0; j < psb->m_nodes.size(); ++j)
+ {
+ x[counter] = psb->m_nodes[j].m_q;
+ counter++;
+ }
+ }
+ counter = 0;
+
+ // populate dx with random vectors
+ for (int i = 0; i < dx.size(); ++i)
+ {
+ dx[i].setX(randomDouble(-1, 1));
+ dx[i].setY(randomDouble(-1, 1));
+ dx[i].setZ(randomDouble(-1, 1));
+ }
+
+ btAlignedObjectArray<double> errors;
+ for (int it = 0; it < 10; ++it)
+ {
+ for (int i = 0; i < dx.size(); ++i)
+ {
+ dx[i] *= 0.5;
+ }
+
+ // get df
+ for (int i = 0; i < df.size(); ++i)
+ {
+ df[i].setZero();
+ f1[i].setZero();
+ f2[i].setZero();
+ }
+
+ //set df
+ addScaledElasticForceDifferential(-1, dx, df);
+
+ for (int i = 0; i < m_softBodies.size(); ++i)
+ {
+ btSoftBody* psb = m_softBodies[i];
+ for (int j = 0; j < psb->m_nodes.size(); ++j)
+ {
+ psb->m_nodes[j].m_q = x[counter] + dx[counter];
+ counter++;
+ }
+ psb->updateDeformation();
+ }
+ counter = 0;
+
+ //set f1
+ addScaledForces(-1, f1);
+
+ for (int i = 0; i < m_softBodies.size(); ++i)
+ {
+ btSoftBody* psb = m_softBodies[i];
+ for (int j = 0; j < psb->m_nodes.size(); ++j)
+ {
+ psb->m_nodes[j].m_q = x[counter] - dx[counter];
+ counter++;
+ }
+ psb->updateDeformation();
+ }
+ counter = 0;
+
+ //set f2
+ addScaledForces(-1, f2);
+
+ //restore m_q
+ for (int i = 0; i < m_softBodies.size(); ++i)
+ {
+ btSoftBody* psb = m_softBodies[i];
+ for (int j = 0; j < psb->m_nodes.size(); ++j)
+ {
+ psb->m_nodes[j].m_q = x[counter];
+ counter++;
+ }
+ psb->updateDeformation();
+ }
+ counter = 0;
+ double error = 0;
+ for (int i = 0; i < df.size(); ++i)
+ {
+ btVector3 error_vector = f1[i] - f2[i] - 2 * df[i];
+ error += error_vector.length2();
+ }
+ error = btSqrt(error);
+ errors.push_back(error);
+ std::cout << "Iteration = " << it << ", error = " << error << std::endl;
+ }
+ for (int i = 1; i < errors.size(); ++i)
+ {
+ std::cout << "Iteration = " << i << ", ratio = " << errors[i - 1] / errors[i] << std::endl;
+ }
+ }
+
+ //
+ virtual double totalElasticEnergy(btScalar dt)
+ {
+ return 0;
+ }
+
+ //
+ virtual double totalDampingEnergy(btScalar dt)
+ {
+ return 0;
+ }
+
+ // total Energy takes dt as input because certain energies depend on dt
+ virtual double totalEnergy(btScalar dt)
+ {
+ return totalElasticEnergy(dt) + totalDampingEnergy(dt);
+ }
};
#endif /* BT_DEFORMABLE_LAGRANGIAN_FORCE */
generated by cgit v1.2.3 (git 2.25.1) at 2024-12-20 22:48:44 +0000