From b8997c36b2b1a25e2d3085ded367b65352a45e31 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Sat, 6 Jul 2019 20:55:41 -0700 Subject: [PATCH] update contact projection --- src/BulletSoftBody/btBackwardEulerObjective.h | 1 + src/BulletSoftBody/btContactProjection.cpp | 5 ++-- src/BulletSoftBody/btDeformableBodySolver.h | 27 +++++++++++++++++-- .../btDeformableRigidDynamicsWorld.cpp | 9 +++++-- .../btDeformableRigidDynamicsWorld.h | 2 +- 5 files changed, 37 insertions(+), 7 deletions(-) diff --git a/src/BulletSoftBody/btBackwardEulerObjective.h b/src/BulletSoftBody/btBackwardEulerObjective.h index f8f0596b5..cef07aea1 100644 --- a/src/BulletSoftBody/btBackwardEulerObjective.h +++ b/src/BulletSoftBody/btBackwardEulerObjective.h @@ -101,6 +101,7 @@ public: { // damping force is implicit and elastic force is explicit m_lf[i]->addScaledDampingForceDifferential(-m_dt, x, b); +// m_lf[i]->addScaledElasticForceDifferential(-m_dt*m_dt, x, b); } } diff --git a/src/BulletSoftBody/btContactProjection.cpp b/src/BulletSoftBody/btContactProjection.cpp index b50918af4..2666ff789 100644 --- a/src/BulletSoftBody/btContactProjection.cpp +++ b/src/BulletSoftBody/btContactProjection.cpp @@ -76,7 +76,7 @@ void btContactProjection::update(btScalar dt, const TVStack& dv) if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) { rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); - va = rigidCol ? (rigidCol->getVelocityInLocalPoint(c.m_c1)) * dt : btVector3(0, 0, 0); + va = rigidCol ? (rigidCol->getVelocityInLocalPoint(c.m_c1) + btVector3(0,-10,0)*dt) * dt : btVector3(0, 0, 0); } else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) { @@ -117,7 +117,8 @@ void btContactProjection::update(btScalar dt, const TVStack& dv) //c.m_node->m_v -= impulse * c.m_c2 / dt; // TODO: only contact is considered here, add friction later btVector3 normal = cti.m_normal.normalized(); - btVector3 dv = -impulse * c.m_c2; + btVector3 diff = c.m_node->m_v - m_backupVelocity[m_indices[c.m_node]]; + btVector3 dv = -impulse * c.m_c2/dt + diff; btScalar dvn = dv.dot(normal); m_constrainedDirections[m_indices[c.m_node]].push_back(normal); m_constrainedValues[m_indices[c.m_node]].push_back(dvn); diff --git a/src/BulletSoftBody/btDeformableBodySolver.h b/src/BulletSoftBody/btDeformableBodySolver.h index 85441a803..5a0a3a05a 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.h +++ b/src/BulletSoftBody/btDeformableBodySolver.h @@ -8,6 +8,7 @@ #ifndef BT_DEFORMABLE_BODY_SOLVERS_H #define BT_DEFORMABLE_BODY_SOLVERS_H +#include #include "btSoftBodySolvers.h" #include "btBackwardEulerObjective.h" #include "btDeformableRigidDynamicsWorld.h" @@ -87,6 +88,7 @@ public: // only need to advect x here if elastic force is implicit // prepareSolve(solverdt); m_objective.computeResidual(solverdt, m_residual); + moveTempVelocity(solverdt, m_residual); m_objective.computeStep(m_dv, m_residual, solverdt); updateVelocity(); @@ -94,6 +96,20 @@ public: advect(solverdt); } + 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) { if (nodeUpdated) @@ -119,7 +135,7 @@ public: for (int j = 0; j < psb->m_nodes.size(); ++j) { auto& node = psb->m_nodes[j]; - node.m_x = node.m_q + dt * node.m_v * psb->m_dampingCoefficient; + node.m_x = node.m_q + dt * node.m_v; } } } @@ -132,7 +148,13 @@ public: for (int j = 0; j < psb->m_nodes.size(); ++j) { auto& node = psb->m_nodes[j]; - node.m_x += dt * m_dv[counter++]; +// node.m_x += dt * m_dv[counter++]; + node.m_x += dt * node.m_v; + if (j == 4) + { + std::cout << "x " << psb->m_nodes[j].m_x.getY() << std::endl; + std::cout << "v " << psb->m_nodes[j].m_v.getY() << std::endl; + } } } } @@ -147,6 +169,7 @@ public: for (int j = 0; j < psb->m_nodes.size(); ++j) { psb->m_nodes[j].m_v = m_backupVelocity[counter] + m_dv[counter]; + ++counter; } } diff --git a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp index 1b21ed5b3..c0afc98d4 100644 --- a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp @@ -26,7 +26,7 @@ void btDeformableRigidDynamicsWorld::internalSingleStepSimulation(btScalar timeS } ///apply gravity, predict motion - btDiscreteDynamicsWorld::predictUnconstraintMotion(timeStep); + predictUnconstraintMotion(timeStep); btDispatcherInfo& dispatchInfo = btSoftRigidDynamicsWorld::btDiscreteDynamicsWorld::getDispatchInfo(); @@ -56,7 +56,7 @@ void btDeformableRigidDynamicsWorld::internalSingleStepSimulation(btScalar timeS ///solve deformable bodies constraints solveDeformableBodiesConstraints(timeStep); - predictUnconstraintMotion(timeStep); +// predictUnconstraintMotion(timeStep); //integrate transforms btSoftRigidDynamicsWorld::btDiscreteDynamicsWorld::integrateTransforms(timeStep); @@ -68,6 +68,11 @@ void btDeformableRigidDynamicsWorld::internalSingleStepSimulation(btScalar timeS ///update soft bodies m_deformableBodySolver->updateSoftBodies(); + for (int i = 0; i < m_nonStaticRigidBodies.size(); i++) + { + btRigidBody* body = m_nonStaticRigidBodies[i]; + std::cout << "rb v = " << body->getLinearVelocity().getY() << std::endl; + } // End solver-wise simulation step // /////////////////////////////// } diff --git a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h index 7f70fef56..98dc4602c 100644 --- a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h +++ b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h @@ -64,7 +64,7 @@ public: virtual void predictUnconstraintMotion(btScalar timeStep) { -// btDiscreteDynamicsWorld::predictUnconstraintMotion(timeStep); + btDiscreteDynamicsWorld::predictUnconstraintMotion(timeStep); m_deformableBodySolver->predictMotion(float(timeStep)); } // virtual void internalStepSingleStepSimulation(btScalar timeStep);