From fb6612c0beefdbd9a7eb3c4f0463466729edd552 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Fri, 9 Aug 2019 16:02:44 -0700 Subject: [PATCH] friction fixes --- .../btDeformableBackwardEulerObjective.cpp | 5 ++ .../btDeformableBackwardEulerObjective.h | 2 + src/BulletSoftBody/btDeformableBodySolver.cpp | 3 +- .../btDeformableContactProjection.cpp | 48 +++++++++++++------ .../btDeformableContactProjection.h | 2 + 5 files changed, 45 insertions(+), 15 deletions(-) diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp index 746048562..09bb9ee2d 100644 --- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp +++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp @@ -145,3 +145,8 @@ void btDeformableBackwardEulerObjective::setConstraints() m_world->btMultiBodyDynamicsWorld::buildIslands(); projection.setConstraints(); } + +void btDeformableBackwardEulerObjective::projectFriction(TVStack& r) +{ + projection.projectFriction(r); +} diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h index f95cb05aa..5de89849a 100644 --- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h +++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h @@ -75,6 +75,8 @@ public: projection.enforceConstraint(x); } + void projectFriction(TVStack& r); + // add dv to velocity void updateVelocity(const TVStack& dv); diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index 2b2827e88..daa85849b 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -38,6 +38,7 @@ void btDeformableBodySolver::solveConstraints(float solverdt) setConstraints(); m_objective->computeResidual(solverdt, m_residual); + m_objective->projectFriction(m_residual); computeStep(m_dv, m_residual); updateVelocity(); } @@ -75,7 +76,7 @@ void btDeformableBodySolver::setConstraints() { BT_PROFILE("setConstraint"); m_objective->setConstraints(); - for (int i = 0; i < 10; ++i) + for (int i = 0; i < 1; ++i) { m_objective->projection.update(); m_objective->enforceConstraint(m_dv); diff --git a/src/BulletSoftBody/btDeformableContactProjection.cpp b/src/BulletSoftBody/btDeformableContactProjection.cpp index 84d35c062..ea88bee49 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.cpp +++ b/src/BulletSoftBody/btDeformableContactProjection.cpp @@ -133,10 +133,11 @@ void btDeformableContactProjection::update() friction.m_impulse_prev[j] = friction.m_impulse[j]; friction.m_dv_prev[j] = friction.m_dv[j]; friction.m_static_prev[j] = friction.m_static[j]; + friction.m_direction_prev[j] = friction.m_direction[j]; // get the current tangent direction btScalar local_tangent_norm = impulse_tangent.norm(); - btVector3 local_tangent_dir = btVector3(0,0,0); + btVector3 local_tangent_dir = -friction.m_direction[j]; if (local_tangent_norm > SIMD_EPSILON) local_tangent_dir = impulse_tangent.normalized(); @@ -395,7 +396,9 @@ void btDeformableContactProjection::enforceConstraint(TVStack& x) // add the friction constraint if (friction.m_static[j] == true) { - x[i] -= x[i].dot(friction.m_direction[j]) * friction.m_direction[j]; +// if (friction.m_static_prev[j] == true) +// x[i] -= friction.m_direction_prev[j] * friction.m_dv_prev[j]; +// x[i] -= x[i].dot(friction.m_direction[j]) * friction.m_direction[j]; x[i] += friction.m_direction[j] * friction.m_dv[j]; } } @@ -410,8 +413,8 @@ void btDeformableContactProjection::project(TVStack& x) for (int index = 0; index < m_constraints.size(); ++index) { const btAlignedObjectArray& constraints = *m_constraints.getAtIndex(index); - size_t i = m_constraints.getKeyAtIndex(index).getUid1(); btAlignedObjectArray& frictions = *m_frictions[m_constraints.getKeyAtIndex(index)]; + size_t i = m_constraints.getKeyAtIndex(index).getUid1(); btAssert(constraints.size() <= dim); btAssert(constraints.size() > 0); if (constraints.size() == 1) @@ -427,6 +430,34 @@ void btDeformableContactProjection::project(TVStack& x) } else x[i].setZero(); + // apply the friction constraint + if (constraints.size() < 3) + { + for (int f = 0; f < frictions.size(); ++f) + { + DeformableFrictionConstraint& friction= frictions[f]; + for (int j = 0; j < friction.m_direction.size(); ++j) + { + if (friction.m_static[j] == true) + { + x[i] -= x[i].dot(friction.m_direction[j]) * friction.m_direction[j]; + } + } + } + } + } +} + +void btDeformableContactProjection::projectFriction(TVStack& x) +{ + const int dim = 3; + for (int index = 0; index < m_constraints.size(); ++index) + { + const btAlignedObjectArray& constraints = *m_constraints.getAtIndex(index); + size_t i = m_constraints.getKeyAtIndex(index).getUid1(); + btAlignedObjectArray& frictions = *m_frictions[m_constraints.getKeyAtIndex(index)]; + btAssert(constraints.size() <= dim); + btAssert(constraints.size() > 0); // apply friction if the node is not constrained in all directions if (constraints.size() < 3) @@ -444,23 +475,12 @@ void btDeformableContactProjection::project(TVStack& x) DeformableFrictionConstraint& friction= frictions[f]; for (int j = 0; j < friction.m_direction.size(); ++j) { - // clear the old friction force - if (friction.m_static_prev[j] == false) - { - x[i] -= friction.m_direction_prev[j] * friction.m_impulse_prev[j]; - } - // only add to the rhs if there is no static friction constraint on the node if (friction.m_static[j] == false) { if (!has_static_constraint) x[i] += friction.m_direction[j] * friction.m_impulse[j]; } - else - { - // otherwise clear the constraint in the friction direction - x[i] -= x[i].dot(friction.m_direction[j]) * friction.m_direction[j]; - } } } } diff --git a/src/BulletSoftBody/btDeformableContactProjection.h b/src/BulletSoftBody/btDeformableContactProjection.h index 64d448b5e..0827554bb 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.h +++ b/src/BulletSoftBody/btDeformableContactProjection.h @@ -36,6 +36,8 @@ public: // apply the constraints to the rhs virtual void project(TVStack& x); + // add to friction + virtual void projectFriction(TVStack& x); // apply constraints to x in Ax=b virtual void enforceConstraint(TVStack& x);