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

add elastic force differential for mass spring

Browse Source
This commit is contained in:
Xuchen Han
2019-08-27 21:43:33 -07:00
parent d4a15e016e
commit 5826492020
2 changed files with 35 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
src/BulletSoftBody/btDeformableLagrangianForce.h
View File

@@ -105,7 +105,6 @@ public:
psb->updateDeformation(); psb->updateDeformation();
} }
TVStack dx; TVStack dx;
dx.resize(getNumNodes()); dx.resize(getNumNodes());
TVStack dphi_dx; TVStack dphi_dx;
@@ -140,7 +139,6 @@ public:
} }
btAlignedObjectArray<double> errors; btAlignedObjectArray<double> errors;
double h = 1;
for (int it = 0; it < 10; ++it) for (int it = 0; it < 10; ++it)
{ {
for (int i = 0; i < dx.size(); ++i) for (int i = 0; i < dx.size(); ++i)

45
src/BulletSoftBody/btDeformableMassSpringForce.h
View File

@@ -91,7 +91,6 @@ public:
size_t id2 = node2->index; size_t id2 = node2->index;
// elastic force // elastic force
// explicit elastic force
btVector3 dir = (node2->m_q - node1->m_q); btVector3 dir = (node2->m_q - node1->m_q);
btVector3 dir_normalized = (dir.norm() > SIMD_EPSILON) ? dir.normalized() : btVector3(0,0,0); btVector3 dir_normalized = (dir.norm() > SIMD_EPSILON) ? dir.normalized() : btVector3(0,0,0);
btVector3 scaled_force = scale * m_elasticStiffness * (dir - dir_normalized * r); btVector3 scaled_force = scale * m_elasticStiffness * (dir - dir_normalized * r);
@@ -106,7 +105,7 @@ public:
// implicit damping force differential // implicit damping force differential
for (int i = 0; i < m_softBodies.size(); ++i) for (int i = 0; i < m_softBodies.size(); ++i)
{ {
const btSoftBody* psb = m_softBodies[i]; btSoftBody* psb = m_softBodies[i];
btScalar scaled_k_damp = m_dampingStiffness * scale; btScalar scaled_k_damp = m_dampingStiffness * scale;
for (int j = 0; j < psb->m_links.size(); ++j) for (int j = 0; j < psb->m_links.size(); ++j)
{ {
@@ -130,6 +129,26 @@ public:
} }
} }
} }
virtual double totalElasticEnergy()
{
double energy = 0;
for (int i = 0; i < m_softBodies.size(); ++i)
{
const btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_links.size(); ++j)
{
const btSoftBody::Link& link = psb->m_links[j];
btSoftBody::Node* node1 = link.m_n[0];
btSoftBody::Node* node2 = link.m_n[1];
btScalar r = link.m_rl;
// elastic force
btVector3 dir = (node2->m_q - node1->m_q);
energy += 0.5 * m_elasticStiffness * (dir.norm() - r) * (dir.norm() -r );
}
}
return energy;
}
virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df)
{ {
@@ -137,7 +156,7 @@ public:
for (int i = 0; i < m_softBodies.size(); ++i) for (int i = 0; i < m_softBodies.size(); ++i)
{ {
const btSoftBody* psb = m_softBodies[i]; const btSoftBody* psb = m_softBodies[i];
btScalar scaled_k_damp = m_dampingStiffness * scale; btScalar scaled_k = m_elasticStiffness * scale;
for (int j = 0; j < psb->m_links.size(); ++j) for (int j = 0; j < psb->m_links.size(); ++j)
{ {
const btSoftBody::Link& link = psb->m_links[j]; const btSoftBody::Link& link = psb->m_links[j];
@@ -147,14 +166,20 @@ public:
size_t id2 = node2->index; size_t id2 = node2->index;
btScalar r = link.m_rl; btScalar r = link.m_rl;
// todo @xuchenhan: find df btVector3 dir = (node1->m_q - node2->m_q);
btVector3 dir = (node2->m_q - node1->m_q); btScalar dir_norm = dir.norm();
btVector3 dir_normalized = (dir.norm() > SIMD_EPSILON) ? dir.normalized() : btVector3(0,0,0); btVector3 dir_normalized = (dir_norm > SIMD_EPSILON) ? dir.normalized() : btVector3(0,0,0);
btVector3 scaled_force = scale * m_elasticStiffness * (dir - dir_normalized * r); btVector3 dx_diff = dx[id1] - dx[id2];
btVector3 local_scaled_df = btVector3(0,0,0); btVector3 scaled_df = btVector3(0,0,0);
if (dir_norm > SIMD_EPSILON)
{
scaled_df -= scaled_k * dir_normalized.dot(dx_diff) * dir_normalized;
scaled_df += scaled_k * dir_normalized.dot(dx_diff) * ((dir_norm-r)/dir_norm) * dir_normalized;
scaled_df -= scaled_k * ((dir_norm-r)/dir_norm) * dx_diff;
}
df[id1] += local_scaled_df; df[id1] += scaled_df;
df[id2] -= local_scaled_df; df[id2] -= scaled_df;
} }
} }
} }