diff --git a/src/BulletSoftBody/btBackwardEulerObjective.cpp b/src/BulletSoftBody/btBackwardEulerObjective.cpp index e8c4c3edc..3f27df5a9 100644 --- a/src/BulletSoftBody/btBackwardEulerObjective.cpp +++ b/src/BulletSoftBody/btBackwardEulerObjective.cpp @@ -57,9 +57,6 @@ void btBackwardEulerObjective::multiply(const TVStack& x, TVStack& b) const { // add damping matrix m_lf[i]->addScaledDampingForceDifferential(-m_dt, x, b); - - // add stiffness matrix when fully implicity - m_lf[i]->addScaledElasticForceDifferential(-m_dt*m_dt, x, b); } } @@ -80,3 +77,16 @@ void btBackwardEulerObjective::updateVelocity(const TVStack& dv) } } +void btBackwardEulerObjective::initialGuess(TVStack& dv, const TVStack& residual) +{ + size_t 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) + { + dv[counter] = psb->m_nodes[j].m_im * residual[counter] * 1; + ++counter; + } + } +} diff --git a/src/BulletSoftBody/btBackwardEulerObjective.h b/src/BulletSoftBody/btBackwardEulerObjective.h index 9b94341b7..e9642f6ae 100644 --- a/src/BulletSoftBody/btBackwardEulerObjective.h +++ b/src/BulletSoftBody/btBackwardEulerObjective.h @@ -76,15 +76,6 @@ class btBackwardEulerObjective { public: using TVStack = btAlignedObjectArray; -// struct DefaultPreconditioner -// { -// void operator()(const TVStack& x, TVStack& b) -// { -// btAssert(b.size() == x.size()); -// for (int i = 0; i < b.size(); ++i) -// b[i] = x[i]; -// } -// }; btScalar m_dt; btConjugateGradient cg; btDeformableRigidDynamicsWorld* m_world; @@ -102,11 +93,29 @@ public: void computeResidual(btScalar dt, TVStack& residual) const { - // gravity is treated explicitly in predictUnconstraintMotion - // add force + // add implicit force for (int i = 0; i < m_lf.size(); ++i) { - m_lf[i]->addScaledForce(dt, residual); + m_lf[i]->addScaledImplicitForce(dt, residual); + } + } + + void applyExplicitForce(TVStack& force) + { + for (int i = 0; i < m_lf.size(); ++i) + m_lf[i]->addScaledExplicitForce(m_dt, force); + + size_t 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) + { + btScalar one_over_mass = (psb->m_nodes[j].m_im == 0) ? 0 : psb->m_nodes[j].m_im; + psb->m_nodes[j].m_v += one_over_mass * force[counter]; + force[counter].setZero(); + counter++; + } } } @@ -117,7 +126,7 @@ public: { norm_squared += residual[i].length2(); } - return std::sqrt(norm_squared); + return std::sqrt(norm_squared+SIMD_EPSILON); } void computeStep(TVStack& dv, const TVStack& residual, const btScalar& dt); @@ -128,6 +137,7 @@ public: { projection.update(dv, m_backupVelocity); } + void initialGuess(TVStack& dv, const TVStack& residual); void reinitialize(bool nodeUpdated); diff --git a/src/BulletSoftBody/btCGProjection.h b/src/BulletSoftBody/btCGProjection.h index b0f798dad..a00fe384b 100644 --- a/src/BulletSoftBody/btCGProjection.h +++ b/src/BulletSoftBody/btCGProjection.h @@ -40,13 +40,17 @@ struct Constraint struct Friction { btAlignedObjectArray m_static; - btAlignedObjectArray m_value; + btAlignedObjectArray m_impulse; + btAlignedObjectArray m_dv; btAlignedObjectArray m_direction; btAlignedObjectArray m_static_prev; - btAlignedObjectArray m_value_prev; + btAlignedObjectArray m_impulse_prev; + btAlignedObjectArray m_dv_prev; btAlignedObjectArray m_direction_prev; + btAlignedObjectArray m_released; + btAlignedObjectArray m_accumulated_impulse; Friction() { @@ -56,10 +60,14 @@ struct Friction m_direction.push_back(btVector3(0,0,0)); m_direction_prev.push_back(btVector3(0,0,0)); - m_value.push_back(0); - m_value_prev.push_back(0); + m_impulse.push_back(0); + m_impulse_prev.push_back(0); + + m_dv.push_back(0); + m_dv_prev.push_back(0); m_accumulated_impulse.push_back(btVector3(0,0,0)); + m_released.push_back(false); } }; diff --git a/src/BulletSoftBody/btContactProjection.cpp b/src/BulletSoftBody/btContactProjection.cpp index e356da264..3250e010b 100644 --- a/src/BulletSoftBody/btContactProjection.cpp +++ b/src/BulletSoftBody/btContactProjection.cpp @@ -85,35 +85,41 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit // start friction handling // copy old data friction.m_direction_prev[j] = friction.m_direction[j]; - friction.m_value_prev[j] = friction.m_value[j]; + 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]; if (accumulated_normal < 0 && accumulated_tangent.norm() > SIMD_EPSILON) { + + btScalar compare1 = -accumulated_normal*c->m_c3; + btScalar compare2 = accumulated_tangent.norm(); // do not allow switching from static friction to dynamic friction // it causes cg to explode - if (-accumulated_normal*c->m_c3 < accumulated_tangent.norm() && friction.m_static_prev[j] == false) + if (-accumulated_normal*c->m_c3 < accumulated_tangent.norm() && friction.m_static_prev[j] == false && friction.m_released[j] == false) { friction.m_static[j] = false; - friction.m_value[j] = -accumulated_normal*c->m_c3; + friction.m_impulse[j] = -accumulated_normal*c->m_c3; + friction.m_dv[j] = -accumulated_normal*c->m_c3 *c->m_c2/m_dt ; } else { friction.m_static[j] = true; - friction.m_value[j] = accumulated_tangent.norm(); + friction.m_impulse[j] = accumulated_tangent.norm(); + friction.m_dv[j] = accumulated_tangent.norm() * c->m_c2/m_dt; } const btVector3 tangent_dir = accumulated_tangent.normalized(); - impulse_tangent = friction.m_value[j] * tangent_dir; friction.m_direction[j] = -tangent_dir; } else { + friction.m_released[j] = true; friction.m_static[j] = false; - friction.m_value[j] = 0; - impulse_tangent.setZero(); + friction.m_impulse[j] = 0; + friction.m_dv[j] = 0; } - + friction.m_accumulated_impulse[j] = accumulated_normal * cti.m_normal - friction.m_impulse[j] * friction.m_direction[j]; if (1) // in the same CG solve, the set of constraits doesn't change // if (dn <= SIMD_EPSILON) { @@ -128,8 +134,8 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit // the incremental impulse: // in the normal direction it's the normal component of "impulse" // in the tangent direction it's the difference between the frictional impulse in the iteration and the previous iteration - impulse = impulse_normal + (friction.m_value_prev[j] * friction.m_direction_prev[j]) - (friction.m_value[j] * friction.m_direction[j]); - + impulse = impulse_normal + (friction.m_impulse_prev[j] * friction.m_direction_prev[j]) - (friction.m_impulse[j] * friction.m_direction[j]); +// impulse = impulse_normal; if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) { if (rigidCol) @@ -172,7 +178,6 @@ void btContactProjection::setConstraintDirections() f.push_back(Friction()); f.push_back(Friction()); m_frictions[&(psb->m_nodes[j])] = f; - // no friction constraints for dirichlet } } } @@ -262,11 +267,14 @@ void btContactProjection::setConstraintDirections() // push in an empty friction frictions[j].m_direction.push_back(btVector3(0,0,0)); frictions[j].m_direction_prev.push_back(btVector3(0,0,0)); - frictions[j].m_value.push_back(0); - frictions[j].m_value_prev.push_back(0); + frictions[j].m_impulse.push_back(0); + frictions[j].m_impulse_prev.push_back(0); + frictions[j].m_dv.push_back(0); + frictions[j].m_dv_prev.push_back(0); frictions[j].m_static.push_back(false); frictions[j].m_static_prev.push_back(false); frictions[j].m_accumulated_impulse.push_back(btVector3(0,0,0)); + frictions[j].m_released.push_back(false); merged = true; break; } diff --git a/src/BulletSoftBody/btContactProjection.h b/src/BulletSoftBody/btContactProjection.h index b52bb2d72..969745d93 100644 --- a/src/BulletSoftBody/btContactProjection.h +++ b/src/BulletSoftBody/btContactProjection.h @@ -51,13 +51,13 @@ public: // clear the old friction force if (friction.m_static_prev[j] == false) { - x[i] -= friction.m_direction_prev[j] * friction.m_value_prev[j]; + 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 && !has_static_constraint) { - x[i] += friction.m_direction[j] * friction.m_value[j]; + x[i] += friction.m_direction[j] * friction.m_impulse[j]; } } } @@ -68,6 +68,33 @@ public: btAssert(free_dir.norm() > SIMD_EPSILON) free_dir.normalize(); x[i] = x[i].dot(free_dir) * free_dir; + + bool has_static_constraint = false; + for (int f = 0; f < 2; ++f) + { + Friction& friction= frictions[f]; + for (int j = 0; j < friction.m_static.size(); ++j) + has_static_constraint = has_static_constraint || friction.m_static[j]; + } + + for (int f = 0; f < 2; ++f) + { + Friction& 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 && !has_static_constraint) + { + x[i] += friction.m_direction[j] * friction.m_impulse[j]; + } + } + } } else x[i].setZero(); @@ -97,12 +124,12 @@ public: // clear the old constraint if (friction.m_static_prev[j] == true) { - x[i] -= friction.m_direction_prev[j] * friction.m_value_prev[j]; + x[i] -= friction.m_direction_prev[j] * friction.m_dv_prev[j]; } // add the new constraint if (friction.m_static[j] == true) { - x[i] += friction.m_direction[j] * friction.m_value[j]; + x[i] += friction.m_direction[j] * friction.m_dv[j]; } } } @@ -120,6 +147,24 @@ public: x[i] += constraints[j].m_value[k] * constraints[j].m_direction[k]; } } + + for (int f = 0; f < 2; ++f) + { + const Friction& friction= frictions[f]; + for (int j = 0; j < friction.m_direction.size(); ++j) + { + // clear the old constraint + if (friction.m_static_prev[j] == true) + { + x[i] -= friction.m_direction_prev[j] * friction.m_dv_prev[j]; + } + // add the new constraint + if (friction.m_static[j] == true) + { + x[i] += friction.m_direction[j] * friction.m_dv[j]; + } + } + } } else { diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index e9bcf226a..a8e1e5cd5 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -118,7 +118,15 @@ void btDeformableBodySolver::solveConstraints(float solverdt) m_dt = solverdt; bool nodeUpdated = updateNodes(); reinitialize(nodeUpdated); + + // apply explicit force + m_objective->applyExplicitForce(m_residual); + + // remove contact constraints with separating velocity + setConstraintDirections(); + backupVelocity(); + for (int i = 0; i < m_solveIterations; ++i) { m_objective->computeResidual(solverdt, m_residual); @@ -143,9 +151,6 @@ void btDeformableBodySolver::reinitialize(bool nodeUpdated) m_residual[i].setZero(); } m_objective->reinitialize(nodeUpdated); - - // remove contact constraints with separating velocity - setConstraintDirections(); } void btDeformableBodySolver::setConstraintDirections() diff --git a/src/BulletSoftBody/btDeformableBodySolver.h b/src/BulletSoftBody/btDeformableBodySolver.h index 3fac0743d..17f088c72 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.h +++ b/src/BulletSoftBody/btDeformableBodySolver.h @@ -74,20 +74,6 @@ public: void postStabilize(); - void moveTempVelocity(btScalar dt, const TVStack& f) - { - size_t counter = 0; - for (int i = 0; i < m_softBodySet.size(); ++i) - { - btSoftBody* psb = m_softBodySet[i]; - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - auto& node = psb->m_nodes[j]; - node.m_v += node.m_im * dt * f[counter++]; - } - } - } - void reinitialize(bool nodeUpdated); void setConstraintDirections(); diff --git a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp index 5fdf01a18..173f71206 100644 --- a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp @@ -12,6 +12,7 @@ void btDeformableRigidDynamicsWorld::internalSingleStepSimulation(btScalar timeStep) { + m_internalTime += timeStep; // Let the solver grab the soft bodies and if necessary optimize for it m_deformableBodySolver->optimize(m_softBodies); @@ -54,7 +55,6 @@ void btDeformableRigidDynamicsWorld::internalSingleStepSimulation(btScalar timeS // gravity is applied in stepSimulation and then cleared here and then applied here and then cleared here again // so that 1) gravity is applied to velocity before constraint solve and 2) gravity is applied in each substep // when there are multiple substeps - clearForces(); clearMultiBodyForces(); btMultiBodyDynamicsWorld::applyGravity(); @@ -69,9 +69,14 @@ void btDeformableRigidDynamicsWorld::internalSingleStepSimulation(btScalar timeS clearForces(); clearMultiBodyForces(); + + for (int i = 0; i < before_solver_callbacks.size(); ++i) + before_solver_callbacks[i](m_internalTime, this); ///solve deformable bodies constraints solveDeformableBodiesConstraints(timeStep); + + //integrate transforms btMultiBodyDynamicsWorld::integrateTransforms(timeStep); diff --git a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h index ca1fff885..5bbebeda3 100644 --- a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h +++ b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h @@ -21,7 +21,7 @@ #include "btMassSpring.h" #include "btDeformableBodySolver.h" #include "btSoftBodyHelpers.h" - +#include typedef btAlignedObjectArray btSoftBodyArray; class btDeformableBodySolver; @@ -40,6 +40,7 @@ class btDeformableRigidDynamicsWorld : public btMultiBodyDynamicsWorld bool m_drawClusterTree; btSoftBodyWorldInfo m_sbi; bool m_ownsSolver; + btScalar m_internalTime; protected: virtual void internalSingleStepSimulation(btScalar timeStep); @@ -67,8 +68,9 @@ public: m_sbi.m_gravity.setValue(0, -10, 0); m_sbi.m_sparsesdf.Initialize(); + m_internalTime = 0.0; } - + btAlignedObjectArray > before_solver_callbacks; virtual ~btDeformableRigidDynamicsWorld() { } @@ -89,9 +91,6 @@ public: } virtual void predictUnconstraintMotion(btScalar timeStep); - // virtual void internalStepSingleStepSimulation(btScalar timeStep); - - // virtual void solveDeformableBodiesConstraints(btScalar timeStep); virtual void addSoftBody(btSoftBody* body, int collisionFilterGroup = btBroadphaseProxy::DefaultFilter, int collisionFilterMask = btBroadphaseProxy::AllFilter); diff --git a/src/BulletSoftBody/btLagrangianForce.h b/src/BulletSoftBody/btLagrangianForce.h index 40e63207e..b95a30ce9 100644 --- a/src/BulletSoftBody/btLagrangianForce.h +++ b/src/BulletSoftBody/btLagrangianForce.h @@ -25,12 +25,14 @@ public: virtual ~btLagrangianForce(){} - virtual void addScaledForce(btScalar scale, TVStack& force) = 0; + virtual void addScaledImplicitForce(btScalar scale, TVStack& force) = 0; virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) = 0; virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) = 0; + virtual void addScaledExplicitForce(btScalar scale, TVStack& force) = 0; + virtual void reinitialize(bool nodeUpdated) { if (nodeUpdated) diff --git a/src/BulletSoftBody/btMassSpring.h b/src/BulletSoftBody/btMassSpring.h index 9fd3c405a..78cdcf7e6 100644 --- a/src/BulletSoftBody/btMassSpring.h +++ b/src/BulletSoftBody/btMassSpring.h @@ -19,7 +19,17 @@ public: } - virtual void addScaledForce(btScalar scale, TVStack& force) + virtual void addScaledImplicitForce(btScalar scale, TVStack& force) + { + addScaledDampingForce(scale, force); + } + + virtual void addScaledExplicitForce(btScalar scale, TVStack& force) + { + addScaledElasticForce(scale, force); + } + + virtual void addScaledDampingForce(btScalar scale, TVStack& force) { int numNodes = getNumNodes(); btAssert(numNodes == force.size()) @@ -31,7 +41,32 @@ public: const auto& link = psb->m_links[j]; const auto node1 = link.m_n[0]; const auto node2 = link.m_n[1]; - btScalar kLST = link.Feature::m_material->m_kLST; // this is probly wrong, TODO: figure out how to get stiffness + size_t id1 = m_indices[node1]; + size_t id2 = m_indices[node2]; + + // damping force + btVector3 v_diff = (node2->m_v - node1->m_v); + btScalar k_damp = psb->m_dampingCoefficient; + btVector3 scaled_force = scale * v_diff * k_damp; + force[id1] += scaled_force; + force[id2] -= scaled_force; + } + } + } + + virtual void addScaledElasticForce(btScalar scale, TVStack& force) + { + int numNodes = getNumNodes(); + btAssert(numNodes == force.size()) + 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 auto& link = psb->m_links[j]; + const auto node1 = link.m_n[0]; + const auto node2 = link.m_n[1]; + btScalar kLST = link.Feature::m_material->m_kLST; btScalar r = link.m_rl; size_t id1 = m_indices[node1]; size_t id2 = m_indices[node2]; @@ -39,21 +74,14 @@ public: // elastic force // fully implicit - btVector3 dir = (node2->m_x - node1->m_x); +// btVector3 dir = (node2->m_x - node1->m_x); // explicit elastic force -// btVector3 dir = (node2->m_q - node1->m_q); + btVector3 dir = (node2->m_q - node1->m_q); btVector3 dir_normalized = dir.normalized(); btVector3 scaled_force = scale * kLST * (dir - dir_normalized * r); force[id1] += scaled_force; force[id2] -= scaled_force; - - // damping force - btVector3 v_diff = (node2->m_v - node1->m_v); - btScalar k_damp = psb->m_dampingCoefficient; // TODO: FIX THIS HACK and set k_damp properly - scaled_force = scale * v_diff * k_damp; - force[id1] += scaled_force; - force[id2] -= scaled_force; } } } @@ -64,7 +92,7 @@ public: btAssert(numNodes == dx.size()); btAssert(numNodes == df.size()); - // implicit elastic force + // implicit elastic force differential for (int i = 0; i < m_softBodies.size(); ++i) { const btSoftBody* psb = m_softBodies[i]; @@ -83,9 +111,10 @@ public: } } + virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) { - // implicity damping force + // implicit damping force differential for (int i = 0; i < m_softBodies.size(); ++i) { const btSoftBody* psb = m_softBodies[i]; diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index 066072426..974246448 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -1782,7 +1782,7 @@ void btSoftBody::predictMotion(btScalar dt) m_sst.radmrg = getCollisionShape()->getMargin(); m_sst.updmrg = m_sst.radmrg * (btScalar)0.25; /* Forces */ - addVelocity(m_worldInfo->m_gravity * m_sst.sdt); + addVelocity(m_worldInfo->m_gravity * m_sst.sdt); applyForces(); /* Integrate */ for (i = 0, ni = m_nodes.size(); i < ni; ++i) @@ -1805,7 +1805,7 @@ void btSoftBody::predictMotion(btScalar dt) } } } - n.m_v += deltaV; + n.m_v += deltaV; n.m_x += n.m_v * m_sst.sdt; n.m_f = btVector3(0, 0, 0); } @@ -2775,6 +2775,14 @@ void btSoftBody::dampClusters() } } +void btSoftBody::setSpringStiffness(btScalar k) +{ + for (int i = 0; i < m_links.size(); ++i) + { + m_links[i].Feature::m_material->m_kLST = k; + } +} + // void btSoftBody::Joint::Prepare(btScalar dt, int) { diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h index 01cde89e3..e8fd3da39 100644 --- a/src/BulletSoftBody/btSoftBody.h +++ b/src/BulletSoftBody/btSoftBody.h @@ -1011,6 +1011,7 @@ public: void solveClusters(btScalar sor); void applyClusters(bool drift); void dampClusters(); + void setSpringStiffness(btScalar k); void applyForces(); static void PSolve_Anchors(btSoftBody* psb, btScalar kst, btScalar ti); static void PSolve_RContacts(btSoftBody* psb, btScalar kst, btScalar ti); @@ -1021,7 +1022,7 @@ public: static vsolver_t getSolver(eVSolver::_ solver); virtual int calculateSerializeBufferSize() const; - + ///fills the dataBuffer and returns the struct name (and 0 on failure) virtual const char* serialize(void* dataBuffer, class btSerializer* serializer) const;