diff options
Diffstat (limited to 'src/BulletSoftBody/btContactProjection.h')
| -rw-r--r-- | src/BulletSoftBody/btContactProjection.h | 43 |
1 files changed, 34 insertions, 9 deletions
diff --git a/src/BulletSoftBody/btContactProjection.h b/src/BulletSoftBody/btContactProjection.h index 1e5c52a60..b52bb2d72 100644 --- a/src/BulletSoftBody/btContactProjection.h +++ b/src/BulletSoftBody/btContactProjection.h @@ -34,16 +34,31 @@ public: { const btAlignedObjectArray<Constraint>& constraints = it.second; size_t i = m_indices[it.first]; - const Friction& friction = m_frictions[it.first]; + btAlignedObjectArray<Friction>& frictions = m_frictions[it.first]; btAssert(constraints.size() <= dim); btAssert(constraints.size() > 0); if (constraints.size() == 1) { x[i] -= x[i].dot(constraints[0].m_direction[0]) * constraints[0].m_direction[0]; - if (friction.m_direction.norm() > SIMD_EPSILON) + Friction& friction= frictions[0]; + + bool has_static_constraint = false; + for (int j = 0; j < friction.m_static.size(); ++j) + has_static_constraint = has_static_constraint || friction.m_static[j]; + + for (int j = 0; j < friction.m_direction.size(); ++j) { - btVector3 dir = friction.m_direction.normalized(); - x[i] -= x[i].dot(dir) * dir; + // clear the old friction force + if (friction.m_static_prev[j] == false) + { + x[i] -= friction.m_direction_prev[j] * friction.m_value_prev[j]; + } + + // only add to the rhs if there is no static friction constraint on the node + if (friction.m_static[j] == false && !has_static_constraint) + { + x[i] += friction.m_direction[j] * friction.m_value[j]; + } } } else if (constraints.size() == 2) @@ -67,7 +82,7 @@ public: { const btAlignedObjectArray<Constraint>& constraints = it.second; size_t i = m_indices[it.first]; - const Friction& friction = m_frictions[it.first]; + const btAlignedObjectArray<Friction>& frictions = m_frictions[it.first]; btAssert(constraints.size() <= dim); btAssert(constraints.size() > 0); if (constraints.size() == 1) @@ -75,15 +90,25 @@ public: x[i] -= x[i].dot(constraints[0].m_direction[0]) * constraints[0].m_direction[0]; for (int j = 0; j < constraints[0].m_direction.size(); ++j) x[i] += constraints[0].m_value[j] * constraints[0].m_direction[j]; - if (friction.m_direction.norm() > SIMD_EPSILON) + + const Friction& friction= frictions[0]; + for (int j = 0; j < friction.m_direction.size(); ++j) { - btVector3 dir = friction.m_direction.normalized(); - x[i] -= x[i].dot(dir) * dir; - x[i] += friction.m_dv; + // clear the old constraint + if (friction.m_static_prev[j] == true) + { + x[i] -= friction.m_direction_prev[j] * friction.m_value_prev[j]; + } + // add the new constraint + if (friction.m_static[j] == true) + { + x[i] += friction.m_direction[j] * friction.m_value[j]; + } } } else if (constraints.size() == 2) { + // TODO: friction btVector3 free_dir = btCross(constraints[0].m_direction[0], constraints[1].m_direction[0]); btAssert(free_dir.norm() > SIMD_EPSILON) free_dir.normalize(); |