diff options
author | Xuchen Han <xuchenhan@xuchenhan-macbookpro.roam.corp.google.com> | 2019-07-23 12:15:23 -0700 |
---|---|---|
committer | Xuchen Han <xuchenhan@xuchenhan-macbookpro.roam.corp.google.com> | 2019-08-02 13:14:15 -0700 |
commit | 243b9fc8c7e4727eb42947389a99c0254d16032d (patch) | |
tree | 22cd5df5177903c2ceda06d417085ca56085576b | |
parent | a90cad2a9650ad57d597eaf72cc8b0e091e51ad5 (diff) | |
download | bullet3-243b9fc8c7e4727eb42947389a99c0254d16032d.tar.gz |
combat friction drift in positionCorrect by changing velocity and change it back (effectively only changing position)
-rw-r--r-- | examples/DeformableContact/DeformableContact.cpp | 23 | ||||
-rw-r--r-- | examples/DeformableDemo/DeformableDemo.cpp | 21 | ||||
-rw-r--r-- | examples/Pinch/Pinch.cpp | 136 | ||||
-rw-r--r-- | examples/VolumetricDeformable/VolumetricDeformable.cpp | 4 | ||||
-rw-r--r-- | src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp | 7 | ||||
-rw-r--r-- | src/BulletSoftBody/btContactProjection.cpp | 54 | ||||
-rw-r--r-- | src/BulletSoftBody/btDeformableBodySolver.cpp | 15 | ||||
-rw-r--r-- | src/BulletSoftBody/btDeformableBodySolver.h | 6 | ||||
-rw-r--r-- | src/BulletSoftBody/btDeformableContactProjection.cpp | 91 | ||||
-rw-r--r-- | src/BulletSoftBody/btDeformableGravityForce.h | 1 | ||||
-rw-r--r-- | src/BulletSoftBody/btDeformableMassSpringForce.h | 3 | ||||
-rw-r--r-- | src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp | 67 | ||||
-rw-r--r-- | src/BulletSoftBody/btDeformableRigidDynamicsWorld.h | 4 | ||||
-rw-r--r-- | src/BulletSoftBody/btSoftBody.cpp | 3 | ||||
-rw-r--r-- | src/BulletSoftBody/btSoftBodyInternals.h | 8 |
15 files changed, 291 insertions, 152 deletions
diff --git a/examples/DeformableContact/DeformableContact.cpp b/examples/DeformableContact/DeformableContact.cpp index b70751e2f..a43fc04ca 100644 --- a/examples/DeformableContact/DeformableContact.cpp +++ b/examples/DeformableContact/DeformableContact.cpp @@ -54,13 +54,6 @@ class DeformableContact : public CommonMultiBodyBase btMultiBody* m_multiBody; btAlignedObjectArray<btMultiBodyJointFeedback*> m_jointFeedbacks; public: - - struct TetraBunny - { - #include "../SoftDemo/bunny.inl" - }; - - DeformableContact(struct GUIHelperInterface* helper) : CommonMultiBodyBase(helper) { @@ -147,7 +140,7 @@ void DeformableContact::initPhysics() btTransform groundTransform; groundTransform.setIdentity(); groundTransform.setOrigin(btVector3(0, -40, 0)); - groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.0)); + groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.1)); //We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here: btScalar mass(0.); @@ -170,16 +163,16 @@ void DeformableContact::initPhysics() { - bool damping = true; - bool gyro = true; - int numLinks = 3; + bool damping = false; + bool gyro = false; + int numLinks = 0; bool spherical = true; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals bool canSleep = false; bool selfCollide = true; btVector3 linkHalfExtents(1, 1, 1); btVector3 baseHalfExtents(1, 1, 1); - btMultiBody* mbC = createFeatherstoneMultiBody_testMultiDof(m_dynamicsWorld, numLinks, btVector3(0.f, 10.f,0.f), linkHalfExtents, baseHalfExtents, spherical, g_floatingBase); + btMultiBody* mbC = createFeatherstoneMultiBody_testMultiDof(m_dynamicsWorld, numLinks, btVector3(0.f, 2.f,0.f), linkHalfExtents, baseHalfExtents, spherical, g_floatingBase); mbC->setCanSleep(canSleep); mbC->setHasSelfCollision(selfCollide); @@ -216,12 +209,13 @@ void DeformableContact::initPhysics() // create a patch of cloth { - const btScalar s = 6; + const btScalar s = 4; btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, 0, -s), btVector3(+s, 0, -s), btVector3(-s, 0, +s), btVector3(+s, 0, +s), - 20,20, +// 20,20, + 3,3, 1 + 2 + 4 + 8, true); psb->getCollisionShape()->setMargin(0.25); @@ -232,7 +226,6 @@ void DeformableContact::initPhysics() psb->m_cfg.kCHR = 1; // collision hardness with rigid body psb->m_cfg.kDF = 0; getDeformableDynamicsWorld()->addSoftBody(psb); - } m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); diff --git a/examples/DeformableDemo/DeformableDemo.cpp b/examples/DeformableDemo/DeformableDemo.cpp index 7ce94a314..aa58fbd6f 100644 --- a/examples/DeformableDemo/DeformableDemo.cpp +++ b/examples/DeformableDemo/DeformableDemo.cpp @@ -105,15 +105,15 @@ public: // } btTransform startTransform; startTransform.setIdentity(); - startTransform.setOrigin(btVector3(1, 2, 1)); + startTransform.setOrigin(btVector3(1, 1.5, 1)); createRigidBody(mass, startTransform, shape[0]); - startTransform.setOrigin(btVector3(1, 2, -1)); + startTransform.setOrigin(btVector3(1, 1.5, -1)); createRigidBody(mass, startTransform, shape[0]); - startTransform.setOrigin(btVector3(-1, 2, 1)); + startTransform.setOrigin(btVector3(-1, 1.5, 1)); createRigidBody(mass, startTransform, shape[0]); - startTransform.setOrigin(btVector3(-1, 2, -1)); + startTransform.setOrigin(btVector3(-1, 1.5, -1)); createRigidBody(mass, startTransform, shape[0]); - startTransform.setOrigin(btVector3(0, 4, 0)); + startTransform.setOrigin(btVector3(0, 3.5, 0)); createRigidBody(mass, startTransform, shape[0]); } @@ -182,7 +182,7 @@ void DeformableDemo::initPhysics() btTransform groundTransform; groundTransform.setIdentity(); - groundTransform.setOrigin(btVector3(0, -30, 0)); + groundTransform.setOrigin(btVector3(0, -32, 0)); groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.)); //We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here: btScalar mass(0.); @@ -206,7 +206,7 @@ void DeformableDemo::initPhysics() // create a piece of cloth { - bool onGround = true; + bool onGround = false; const btScalar s = 4; btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, 0, -s), btVector3(+s, 0, -s), @@ -222,13 +222,14 @@ void DeformableDemo::initPhysics() btVector3(+s, 0, -s), btVector3(-s, 0, +s), btVector3(+s, 0, +s), - 20,20, +// 20,20, + 2,2, 0, true); psb->getCollisionShape()->setMargin(0.1); psb->generateBendingConstraints(2); - psb->setTotalMass(10); - psb->setSpringStiffness(10); + psb->setTotalMass(1); + psb->setSpringStiffness(1); psb->setDampingCoefficient(0.01); psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects psb->m_cfg.kCHR = 1; // collision hardness with rigid body diff --git a/examples/Pinch/Pinch.cpp b/examples/Pinch/Pinch.cpp index cedc51750..cc3008f9c 100644 --- a/examples/Pinch/Pinch.cpp +++ b/examples/Pinch/Pinch.cpp @@ -73,9 +73,9 @@ public: void resetCamera() { float dist = 25; - float pitch = -45; + float pitch = -30; float yaw = 100; - float targetPos[3] = {0, -3, 0}; + float targetPos[3] = {0, -0, 0}; m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]); } @@ -137,17 +137,56 @@ void dynamics(btScalar time, btDeformableRigidDynamicsWorld* world) return; btRigidBody* rb0 = rbs[0]; btScalar pressTime = 0.9; + btScalar liftTime = 2.5; + btScalar shiftTime = 3.5; + btScalar holdTime = 4.5*1000; + btScalar dropTime = 5.3*1000; btTransform rbTransform; rbTransform.setIdentity(); - btVector3 translation = btVector3(0.5,3,4); - btVector3 velocity = btVector3(0,5,0); + btVector3 translation; + btVector3 velocity; + + btVector3 initialTranslationLeft = btVector3(0.5,3,4); + btVector3 initialTranslationRight = btVector3(0.5,3,-4); + btVector3 pinchVelocityLeft = btVector3(0,0,-2); + btVector3 pinchVelocityRight = btVector3(0,0,2); + btVector3 liftVelocity = btVector3(0,5,0); + btVector3 shiftVelocity = btVector3(0,0,5); + btVector3 holdVelocity = btVector3(0,0,0); + btVector3 openVelocityLeft = btVector3(0,0,4); + btVector3 openVelocityRight = btVector3(0,0,-4); + if (time < pressTime) { - velocity = btVector3(0,0,-2); - translation += velocity * time; + velocity = pinchVelocityLeft; + translation = initialTranslationLeft + pinchVelocityLeft * time; + } + else if (time < liftTime) + { + velocity = liftVelocity; + translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (time - pressTime); + + } + else if (time < shiftTime) + { + velocity = shiftVelocity; + translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (time - liftTime); + } + else if (time < holdTime) + { + velocity = btVector3(0,0,0); + translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (time - shiftTime); + } + else if (time < dropTime) + { + velocity = openVelocityLeft; + translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (holdTime - shiftTime)+ openVelocityLeft * (time - holdTime); } else - translation += btVector3(0,0,-2) * pressTime + (time-pressTime)*velocity; + { + velocity = holdVelocity; + translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (holdTime - shiftTime)+ openVelocityLeft * (dropTime - holdTime); + } rbTransform.setOrigin(translation); rbTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0)); rb0->setCenterOfMassTransform(rbTransform); @@ -155,23 +194,45 @@ void dynamics(btScalar time, btDeformableRigidDynamicsWorld* world) rb0->setLinearVelocity(velocity); btRigidBody* rb1 = rbs[1]; - translation = btVector3(0.5,3,-4); - velocity = btVector3(0,5,0); if (time < pressTime) { - velocity = btVector3(0,0,2); - translation += velocity * time; + velocity = pinchVelocityRight; + translation = initialTranslationRight + pinchVelocityRight * time; + } + else if (time < liftTime) + { + velocity = liftVelocity; + translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (time - pressTime); + + } + else if (time < shiftTime) + { + velocity = shiftVelocity; + translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (time - liftTime); + } + else if (time < holdTime) + { + velocity = btVector3(0,0,0); + translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (time - shiftTime); + } + else if (time < dropTime) + { + velocity = openVelocityRight; + translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (holdTime - shiftTime)+ openVelocityRight * (time - holdTime); } else - translation += btVector3(0,0,2) * pressTime + (time-pressTime)*velocity; + { + velocity = holdVelocity; + translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (holdTime - shiftTime)+ openVelocityRight * (dropTime - holdTime); + } rbTransform.setOrigin(translation); rbTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0)); rb1->setCenterOfMassTransform(rbTransform); rb1->setAngularVelocity(btVector3(0,0,0)); rb1->setLinearVelocity(velocity); - rb0->setFriction(2); - rb1->setFriction(2); + rb0->setFriction(20); + rb1->setFriction(20); } void Pinch::initPhysics() @@ -194,7 +255,7 @@ void Pinch::initPhysics() m_dynamicsWorld->setGravity(btVector3(0, -10, 0)); getDeformableDynamicsWorld()->getWorldInfo().m_gravity.setValue(0, -10, 0); - getDeformableDynamicsWorld()->before_solver_callbacks.push_back(dynamics); + getDeformableDynamicsWorld()->m_beforeSolverCallbacks.push_back(dynamics); m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); //create a ground @@ -229,21 +290,58 @@ void Pinch::initPhysics() // create a soft block { + btScalar verts[24] = {0.f, 0.f, 0.f, + 1.f, 0.f, 0.f, + 0.f, 1.f, 0.f, + 0.f, 0.f, 1.f, + 1.f, 1.f, 0.f, + 0.f, 1.f, 1.f, + 1.f, 0.f, 1.f, + 1.f, 1.f, 1.f + }; + int triangles[60] = {0, 6, 3, + 0,1,6, + 7,5,3, + 7,3,6, + 4,7,6, + 4,6,1, + 7,2,5, + 7,4,2, + 0,3,2, + 2,3,5, + 0,2,4, + 0,4,1, + 0,6,5, + 0,6,4, + 3,4,2, + 3,4,7, + 2,7,3, + 2,7,1, + 4,5,0, + 4,5,6, + }; +// btSoftBody* psb = btSoftBodyHelpers::CreateFromTriMesh(getDeformableDynamicsWorld()->getWorldInfo(), &verts[0], &triangles[0], 20); +//// btSoftBody* psb = btSoftBodyHelpers::CreateFromTetGenData(getDeformableDynamicsWorld()->getWorldInfo(), TetraCube::getElements(), 0, TetraCube::getNodes(), false, true, true); - getDeformableDynamicsWorld()->addSoftBody(psb); + psb->scale(btVector3(2, 2, 2)); psb->translate(btVector3(0, 4, 0)); psb->getCollisionShape()->setMargin(0.1); psb->setTotalMass(1); - psb->setSpringStiffness(10); - psb->setDampingCoefficient(0.01); +// psb->scale(btVector3(5, 5, 5)); +// psb->translate(btVector3(-2.5, 4, -2.5)); +// psb->getCollisionShape()->setMargin(0.1); +// psb->setTotalMass(1); + psb->setSpringStiffness(4); + psb->setDampingCoefficient(0.02); psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects psb->m_cfg.kCHR = 1; // collision hardness with rigid body - psb->m_cfg.kDF = 0.5; + psb->m_cfg.kDF = 2; + getDeformableDynamicsWorld()->addSoftBody(psb); // add a grippers createGrip(); } diff --git a/examples/VolumetricDeformable/VolumetricDeformable.cpp b/examples/VolumetricDeformable/VolumetricDeformable.cpp index b30e9bc24..9bcf63ab2 100644 --- a/examples/VolumetricDeformable/VolumetricDeformable.cpp +++ b/examples/VolumetricDeformable/VolumetricDeformable.cpp @@ -77,8 +77,8 @@ public: void stepSimulation(float deltaTime) { //use a smaller internal timestep, there are stability issues - float internalTimeStep = 1. / 240.f; - m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep); + float internalTimeStep = 1. / 480.f; + m_dynamicsWorld->stepSimulation(deltaTime, 8, internalTimeStep); } void createStaticBox(const btVector3& halfEdge, const btVector3& translation) diff --git a/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp b/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp index e5097ccbb..11fa97cdd 100644 --- a/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp +++ b/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp @@ -233,7 +233,7 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher, btCollisi // printf("error in island management\n"); } - btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); +// btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); if (colObj0->getIslandTag() == islandId) { if (colObj0->getActivationState() == ACTIVE_TAG || @@ -257,7 +257,7 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher, btCollisi // printf("error in island management\n"); } - btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); +// btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); if (colObj0->getIslandTag() == islandId) { @@ -278,7 +278,8 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher, btCollisi // printf("error in island management\n"); } - btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); +// btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); + if (colObj0->getIslandTag() == islandId) { diff --git a/src/BulletSoftBody/btContactProjection.cpp b/src/BulletSoftBody/btContactProjection.cpp index 99ab6f473..e81cb2c50 100644 --- a/src/BulletSoftBody/btContactProjection.cpp +++ b/src/BulletSoftBody/btContactProjection.cpp @@ -50,12 +50,12 @@ void btDeformableContactProjection::update() // loop through constraints to set constrained values for (auto& it : m_constraints) { - btAlignedObjectArray<Friction>& frictions = m_frictions[it.first]; - btAlignedObjectArray<Constraint>& constraints = it.second; + btAlignedObjectArray<DeformableFrictionConstraint>& frictions = m_frictions[it.first]; + btAlignedObjectArray<DeformableContactConstraint>& constraints = it.second; for (int i = 0; i < constraints.size(); ++i) { - Constraint& constraint = constraints[i]; - Friction& friction = frictions[i]; + DeformableContactConstraint& constraint = constraints[i]; + DeformableFrictionConstraint& friction = frictions[i]; for (int j = 0; j < constraint.m_contact.size(); ++j) { if (constraint.m_contact[j] == nullptr) @@ -229,16 +229,16 @@ void btDeformableContactProjection::setConstraints() { if (psb->m_nodes[j].m_im == 0) { - btAlignedObjectArray<Constraint> c; - c.push_back(Constraint(btVector3(1,0,0))); - c.push_back(Constraint(btVector3(0,1,0))); - c.push_back(Constraint(btVector3(0,0,1))); + btAlignedObjectArray<DeformableContactConstraint> c; + c.push_back(DeformableContactConstraint(btVector3(1,0,0))); + c.push_back(DeformableContactConstraint(btVector3(0,1,0))); + c.push_back(DeformableContactConstraint(btVector3(0,0,1))); m_constraints[&(psb->m_nodes[j])] = c; - btAlignedObjectArray<Friction> f; - f.push_back(Friction()); - f.push_back(Friction()); - f.push_back(Friction()); + btAlignedObjectArray<DeformableFrictionConstraint> f; + f.push_back(DeformableFrictionConstraint()); + f.push_back(DeformableFrictionConstraint()); + f.push_back(DeformableFrictionConstraint()); m_frictions[&(psb->m_nodes[j])] = f; } } @@ -310,11 +310,11 @@ void btDeformableContactProjection::setConstraints() if (m_constraints.find(c.m_node) == m_constraints.end()) { - btAlignedObjectArray<Constraint> constraints; - constraints.push_back(Constraint(c, jacobianData_normal)); + btAlignedObjectArray<DeformableContactConstraint> constraints; + constraints.push_back(DeformableContactConstraint(c, jacobianData_normal)); m_constraints[c.m_node] = constraints; - btAlignedObjectArray<Friction> frictions; - frictions.push_back(Friction(complementaryDirection, jacobianData_complementary)); + btAlignedObjectArray<DeformableFrictionConstraint> frictions; + frictions.push_back(DeformableFrictionConstraint(complementaryDirection, jacobianData_complementary)); m_frictions[c.m_node] = frictions; } else @@ -322,8 +322,8 @@ void btDeformableContactProjection::setConstraints() // group colinear constraints into one const btScalar angle_epsilon = 0.015192247; // less than 10 degree bool merged = false; - btAlignedObjectArray<Constraint>& constraints = m_constraints[c.m_node]; - btAlignedObjectArray<Friction>& frictions = m_frictions[c.m_node]; + btAlignedObjectArray<DeformableContactConstraint>& constraints = m_constraints[c.m_node]; + btAlignedObjectArray<DeformableFrictionConstraint>& frictions = m_frictions[c.m_node]; for (int j = 0; j < constraints.size(); ++j) { const btAlignedObjectArray<btVector3>& dirs = constraints[j].m_direction; @@ -343,8 +343,8 @@ void btDeformableContactProjection::setConstraints() // hard coded no more than 3 constraint directions if (!merged && constraints.size() < dim) { - constraints.push_back(Constraint(c, jacobianData_normal)); - frictions.push_back(Friction(complementaryDirection, jacobianData_complementary)); + constraints.push_back(DeformableContactConstraint(c, jacobianData_normal)); + frictions.push_back(DeformableFrictionConstraint(complementaryDirection, jacobianData_complementary)); } } } @@ -358,9 +358,9 @@ void btDeformableContactProjection::enforceConstraint(TVStack& x) const int dim = 3; for (auto& it : m_constraints) { - const btAlignedObjectArray<Constraint>& constraints = it.second; + const btAlignedObjectArray<DeformableContactConstraint>& constraints = it.second; size_t i = m_indices[it.first]; - const btAlignedObjectArray<Friction>& frictions = m_frictions[it.first]; + const btAlignedObjectArray<DeformableFrictionConstraint>& frictions = m_frictions[it.first]; btAssert(constraints.size() <= dim); btAssert(constraints.size() > 0); if (constraints.size() == 1) @@ -401,7 +401,7 @@ void btDeformableContactProjection::enforceConstraint(TVStack& x) { for (int f = 0; f < frictions.size(); ++f) { - const Friction& friction= frictions[f]; + const DeformableFrictionConstraint& friction= frictions[f]; for (int j = 0; j < friction.m_direction.size(); ++j) { // clear the old constraint @@ -425,9 +425,9 @@ void btDeformableContactProjection::project(TVStack& x) const int dim = 3; for (auto& it : m_constraints) { - const btAlignedObjectArray<Constraint>& constraints = it.second; + const btAlignedObjectArray<DeformableContactConstraint>& constraints = it.second; size_t i = m_indices[it.first]; - btAlignedObjectArray<Friction>& frictions = m_frictions[it.first]; + btAlignedObjectArray<DeformableFrictionConstraint>& frictions = m_frictions[it.first]; btAssert(constraints.size() <= dim); btAssert(constraints.size() > 0); if (constraints.size() == 1) @@ -450,14 +450,14 @@ void btDeformableContactProjection::project(TVStack& x) bool has_static_constraint = false; for (int f = 0; f < frictions.size(); ++f) { - Friction& friction= frictions[f]; + DeformableFrictionConstraint& 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 < frictions.size(); ++f) { - Friction& friction= frictions[f]; + DeformableFrictionConstraint& friction= frictions[f]; for (int j = 0; j < friction.m_direction.size(); ++j) { // clear the old friction force diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index 96d2e0029..e3009dcbb 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -29,6 +29,7 @@ void btDeformableBodySolver::solveConstraints(float solverdt) // save v_{n+1}^* velocity after explicit forces backupVelocity(); + m_objective->computeResidual(solverdt, m_residual); // m_objective->initialGuess(m_dv, m_residual); computeStep(m_dv, m_residual); @@ -98,6 +99,20 @@ void btDeformableBodySolver::backupVelocity() } } +void btDeformableBodySolver::revertVelocity() +{ + // serial implementation + int 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) + { + psb->m_nodes[j].m_v = m_backupVelocity[counter++]; + } + } +} + bool btDeformableBodySolver::updateNodes() { int numNodes = 0; diff --git a/src/BulletSoftBody/btDeformableBodySolver.h b/src/BulletSoftBody/btDeformableBodySolver.h index 570cce90f..0b4a4819f 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.h +++ b/src/BulletSoftBody/btDeformableBodySolver.h @@ -27,12 +27,14 @@ protected: TVStack m_dv; TVStack m_residual; btAlignedObjectArray<btSoftBody *> m_softBodySet; - btDeformableBackwardEulerObjective* m_objective; + btAlignedObjectArray<btVector3> m_backupVelocity; btScalar m_dt; btConjugateGradient<btDeformableBackwardEulerObjective> cg; public: + btDeformableBackwardEulerObjective* m_objective; + btDeformableBodySolver(); virtual ~btDeformableBodySolver(); @@ -57,7 +59,7 @@ public: void predictDeformableMotion(btSoftBody* psb, btScalar dt); void backupVelocity(); - + void revertVelocity(); void updateVelocity(); bool updateNodes(); diff --git a/src/BulletSoftBody/btDeformableContactProjection.cpp b/src/BulletSoftBody/btDeformableContactProjection.cpp index e81cb2c50..25f667711 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.cpp +++ b/src/BulletSoftBody/btDeformableContactProjection.cpp @@ -143,6 +143,8 @@ void btDeformableContactProjection::update() friction.m_direction[j] = -local_tangent_dir; // do not allow switching from static friction to dynamic friction // it causes cg to explode + btScalar comp1 = -accumulated_normal*c->m_c3; + btScalar comp2 = tangent_norm; if (-accumulated_normal*c->m_c3 < tangent_norm && friction.m_static_prev[j] == false && friction.m_released[j] == false) { friction.m_static[j] = false; @@ -167,49 +169,44 @@ void btDeformableContactProjection::update() // the incremental impulse applied to rb in the tangential direction btVector3 incremental_tangent = (friction.m_impulse_prev[j] * friction.m_direction_prev[j])-(friction.m_impulse[j] * friction.m_direction[j]); - // TODO cleanup - if (1) // in the same CG solve, the set of constraits doesn't change - { - // c0 is the impulse matrix, c3 is 1 - the friction coefficient or 0, c4 is the contact hardness coefficient - - // dv = new_impulse + accumulated velocity change in previous CG iterations - // so we have the invariant node->m_v = backupVelocity + dv; - btScalar dvn = -accumulated_normal * c->m_c2/m_dt; - - // the following is equivalent - /* - btVector3 dv = -impulse_normal * c->m_c2/m_dt + c->m_node->m_v - backupVelocity[m_indices[c->m_node]]; - btScalar dvn = dv.dot(cti.m_normal); - */ - - constraint.m_value[j] = dvn; + // dv = new_impulse + accumulated velocity change in previous CG iterations + // so we have the invariant node->m_v = backupVelocity + dv; + + btScalar dvn = -accumulated_normal * c->m_c2/m_dt; + + // the following is equivalent + /* + btVector3 dv = -impulse_normal * c->m_c2/m_dt + c->m_node->m_v - backupVelocity[m_indices[c->m_node]]; + btScalar dvn = dv.dot(cti.m_normal); + */ + + constraint.m_value[j] = dvn; + + // 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 + incremental_tangent; + if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) + { + if (rigidCol) + rigidCol->applyImpulse(impulse, c->m_c1); + } + else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + { - // 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 + incremental_tangent; - if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) - { - if (rigidCol) - rigidCol->applyImpulse(impulse, c->m_c1); - } - else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + if (multibodyLinkCol) { + double multiplier = 1; + multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_normal, -impulse_normal.length() * multiplier); - if (multibodyLinkCol) + if (incremental_tangent.norm() > SIMD_EPSILON) { - double multiplier = 1; - multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_normal, -impulse_normal.length() * multiplier); - - if (incremental_tangent.norm() > SIMD_EPSILON) - { - btMultiBodyJacobianData jacobian_tangent; - btVector3 tangent = incremental_tangent.normalized(); - findJacobian(multibodyLinkCol, jacobian_tangent, c->m_node->m_x, tangent); - const btScalar* deltaV_tangent = &jacobian_tangent.m_deltaVelocitiesUnitImpulse[0]; - multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_tangent, incremental_tangent.length() * multiplier); - } + btMultiBodyJacobianData jacobian_tangent; + btVector3 tangent = incremental_tangent.normalized(); + findJacobian(multibodyLinkCol, jacobian_tangent, c->m_node->m_x, tangent); + const btScalar* deltaV_tangent = &jacobian_tangent.m_deltaVelocitiesUnitImpulse[0]; + multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_tangent, incremental_tangent.length() * multiplier); } } } @@ -404,14 +401,10 @@ void btDeformableContactProjection::enforceConstraint(TVStack& x) const DeformableFrictionConstraint& 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 + // add the friction constraint if (friction.m_static[j] == true) { + x[i] -= x[i].dot(friction.m_direction[j]) * friction.m_direction[j]; x[i] += friction.m_direction[j] * friction.m_dv[j]; } } @@ -467,9 +460,15 @@ void btDeformableContactProjection::project(TVStack& x) } // only add to the rhs if there is no static friction constraint on the node - if (friction.m_static[j] == false && !has_static_constraint) + if (friction.m_static[j] == false) + { + if (!has_static_constraint) + x[i] += friction.m_direction[j] * friction.m_impulse[j]; + } + else { - x[i] += friction.m_direction[j] * friction.m_impulse[j]; + // 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/btDeformableGravityForce.h b/src/BulletSoftBody/btDeformableGravityForce.h index 545615a47..d8571fa73 100644 --- a/src/BulletSoftBody/btDeformableGravityForce.h +++ b/src/BulletSoftBody/btDeformableGravityForce.h @@ -23,6 +23,7 @@ public: virtual void addScaledImplicitForce(btScalar scale, TVStack& force) { +// addScaledGravityForce(scale, force); } virtual void addScaledExplicitForce(btScalar scale, TVStack& force) diff --git a/src/BulletSoftBody/btDeformableMassSpringForce.h b/src/BulletSoftBody/btDeformableMassSpringForce.h index 31389c20f..70ad6e69c 100644 --- a/src/BulletSoftBody/btDeformableMassSpringForce.h +++ b/src/BulletSoftBody/btDeformableMassSpringForce.h @@ -22,6 +22,7 @@ public: virtual void addScaledImplicitForce(btScalar scale, TVStack& force) { addScaledDampingForce(scale, force); +// addScaledElasticForce(scale, force); } virtual void addScaledExplicitForce(btScalar scale, TVStack& force) @@ -102,6 +103,8 @@ public: } } } + + }; #endif /* btMassSpring_h */ diff --git a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp index 3af3c8499..216c1a24d 100644 --- a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp @@ -13,7 +13,7 @@ void btDeformableRigidDynamicsWorld::internalSingleStepSimulation(btScalar timeStep) { reinitialize(timeStep); - +// beforeSolverCallbacks(timeStep); // add gravity to velocity of rigid and multi bodys applyRigidBodyGravity(timeStep); @@ -30,7 +30,7 @@ void btDeformableRigidDynamicsWorld::internalSingleStepSimulation(btScalar timeS ///solve deformable bodies constraints solveDeformableBodiesConstraints(timeStep); - positionCorrection(); + afterSolverCallbacks(timeStep); integrateTransforms(timeStep); @@ -42,36 +42,57 @@ void btDeformableRigidDynamicsWorld::internalSingleStepSimulation(btScalar timeS // /////////////////////////////// } -void btDeformableRigidDynamicsWorld::positionCorrection() +void btDeformableRigidDynamicsWorld::positionCorrection(btScalar dt) { - // perform position correction for all geometric collisions - for (int i = 0; i < m_softBodies.size(); ++i) + // perform position correction for all constraints + for (auto& it : m_deformableBodySolver->m_objective->projection.m_constraints) { - btSoftBody* psb = m_softBodies[i]; - const btScalar mrg = psb->getCollisionShape()->getMargin(); - for (int j = 0; j < psb->m_rcontacts.size(); ++j) + btAlignedObjectArray<DeformableFrictionConstraint>& frictions = m_deformableBodySolver->m_objective->projection.m_frictions[it.first]; + btAlignedObjectArray<DeformableContactConstraint>& constraints = it.second; + for (int i = 0; i < constraints.size(); ++i) { - const btSoftBody::RContact& c = psb->m_rcontacts[j]; - // skip anchor points - if (c.m_node->m_im == 0) - continue; - - const btSoftBody::sCti& cti = c.m_cti; - if (cti.m_colObj->hasContactResponse()) + DeformableContactConstraint& constraint = constraints[i]; + DeformableFrictionConstraint& friction = frictions[i]; + for (int j = 0; j < constraint.m_contact.size(); ++j) { - btScalar dp = btMin((btDot(c.m_node->m_x, cti.m_normal) + cti.m_offset), mrg); - if (dp < 0) + const btSoftBody::RContact* c = constraint.m_contact[j]; + // skip anchor points + if (c == nullptr || c->m_node->m_im == 0) + continue; + const btSoftBody::sCti& cti = c->m_cti; + btRigidBody* rigidCol = 0; + btVector3 va(0, 0, 0); + + // grab the velocity of the rigid body + if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) { - // m_c4 is the collision hardness - c.m_node->m_q -= dp * cti.m_normal * c.m_c4; + rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); + va = rigidCol ? (rigidCol->getVelocityInLocalPoint(c->m_c1)): btVector3(0, 0, 0); + } + + if (cti.m_colObj->hasContactResponse()) + { + btScalar dp = cti.m_offset; + rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); + if (friction.m_static[j] == true) + { + c->m_node->m_v = va; + } + if (dp < 0) + { + c->m_node->m_v -= dp * cti.m_normal / dt; + } } } } } } + void btDeformableRigidDynamicsWorld::integrateTransforms(btScalar dt) { + m_deformableBodySolver->backupVelocity(); + positionCorrection(dt); btMultiBodyDynamicsWorld::integrateTransforms(dt); for (int i = 0; i < m_softBodies.size(); ++i) { @@ -82,6 +103,7 @@ void btDeformableRigidDynamicsWorld::integrateTransforms(btScalar dt) node.m_x = node.m_q + dt * node.m_v; } } + m_deformableBodySolver->revertVelocity(); } void btDeformableRigidDynamicsWorld::solveDeformableBodiesConstraints(btScalar timeStep) @@ -146,7 +168,12 @@ void btDeformableRigidDynamicsWorld::beforeSolverCallbacks(btScalar timeStep) { (*m_internalTickCallback)(this, timeStep); } - + for (int i = 0; i < m_beforeSolverCallbacks.size(); ++i) + m_beforeSolverCallbacks[i](m_internalTime, this); +} + +void btDeformableRigidDynamicsWorld::afterSolverCallbacks(btScalar timeStep) +{ for (int i = 0; i < m_beforeSolverCallbacks.size(); ++i) m_beforeSolverCallbacks[i](m_internalTime, this); } diff --git a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h index c5cf280ef..f1be4fd85 100644 --- a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h +++ b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h @@ -47,7 +47,7 @@ protected: virtual void integrateTransforms(btScalar timeStep); - void positionCorrection(); + void positionCorrection(btScalar dt); void solveDeformableBodiesConstraints(btScalar timeStep); @@ -124,6 +124,8 @@ public: void beforeSolverCallbacks(btScalar timeStep); + void afterSolverCallbacks(btScalar timeStep); + int getDrawFlags() const { return (m_drawFlags); } void setDrawFlags(int f) { m_drawFlags = f; } }; diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index 876b6b6e4..bd7587c47 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -2286,7 +2286,8 @@ bool btSoftBody::checkContact(const btCollisionObjectWrapper* colObjWrap, { cti.m_colObj = colObjWrap->getCollisionObject(); cti.m_normal = wtr.getBasis() * nrm; - cti.m_offset = -btDot(cti.m_normal, x - cti.m_normal * dst); +// cti.m_offset = -btDot(cti.m_normal, x - cti.m_normal * dst); + cti.m_offset = dst; return (true); } return (false); diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h index b892498ea..9b3e45ea4 100644 --- a/src/BulletSoftBody/btSoftBodyInternals.h +++ b/src/BulletSoftBody/btSoftBodyInternals.h @@ -878,15 +878,11 @@ struct btSoftColliders const btScalar ms = ima + imb; if (ms > 0) { + psb->checkContact(m_colObj1Wrap, n.m_q, m, c.m_cti); const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform(); static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0); const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic; - const btVector3 ra = n.m_x - wtr.getOrigin(); - const btVector3 va = m_rigidBody ? m_rigidBody->getVelocityInLocalPoint(ra) * psb->m_sst.sdt : btVector3(0, 0, 0); - const btVector3 vb = n.m_x - n.m_q; - const btVector3 vr = vb - va; - const btScalar dn = btDot(vr, c.m_cti.m_normal); - const btVector3 fv = vr - c.m_cti.m_normal * dn; + const btVector3 ra = n.m_q - wtr.getOrigin(); const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction(); c.m_node = &n; c.m_c0 = ImpulseMatrix(psb->m_sst.sdt, ima, imb, iwi, ra); |