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();
}
TVStack dx;
dx.resize(getNumNodes());
TVStack dphi_dx;
@@ -140,7 +139,6 @@ public:
}
btAlignedObjectArray<double> errors;
double h = 1;
for (int it = 0; it < 10; ++it)
{
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;
// elastic force
// explicit elastic force
btVector3 dir = (node2->m_q - node1->m_q);
btVector3 dir_normalized = (dir.norm() > SIMD_EPSILON) ? dir.normalized() : btVector3(0,0,0);
btVector3 scaled_force = scale * m_elasticStiffness * (dir - dir_normalized * r);
@@ -106,7 +105,7 @@ public:
// implicit damping force differential
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;
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)
{
@@ -137,7 +156,7 @@ public:
for (int i = 0; i < m_softBodies.size(); ++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)
{
const btSoftBody::Link& link = psb->m_links[j];
@@ -147,14 +166,20 @@ public:
size_t id2 = node2->index;
btScalar r = link.m_rl;
// todo @xuchenhan: find df
btVector3 dir = (node2->m_q - node1->m_q);
btVector3 dir_normalized = (dir.norm() > SIMD_EPSILON) ? dir.normalized() : btVector3(0,0,0);
btVector3 scaled_force = scale * m_elasticStiffness * (dir - dir_normalized * r);
btVector3 local_scaled_df = btVector3(0,0,0);
btVector3 dir = (node1->m_q - node2->m_q);
btScalar dir_norm = dir.norm();
btVector3 dir_normalized = (dir_norm > SIMD_EPSILON) ? dir.normalized() : btVector3(0,0,0);
btVector3 dx_diff = dx[id1] - dx[id2];
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[id2] -= local_scaled_df;
df[id1] += scaled_df;
df[id2] -= scaled_df;
}
}
}