fixed gravity issue in rigid body and deformable body contact solve

7 files changed, 72 insertions, 121 deletions

diff --git a/examples/DeformableDemo/DeformableDemo.cpp b/examples/DeformableDemo/DeformableDemo.cpp
index 7edc3c4a3..990bdd7b9 100644
--- a/examples/DeformableDemo/DeformableDemo.cpp
+++ b/examples/DeformableDemo/DeformableDemo.cpp
@@ -59,16 +59,45 @@ public:
 
 	void resetCamera()
 	{
-//        float dist = 30;
-//        float pitch = -14;
-//        float yaw = 0;
-//        float targetPos[3] = {0, 0, 0};
-        float dist = 45;
+        float dist = 15;
         float pitch = -45;
         float yaw = 100;
-        float targetPos[3] = {0,0, 0};
+        float targetPos[3] = {0, -5, 0};
 		m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
 	}
+    
+    
+    void Ctor_RbUpStack(int count)
+    {
+        float mass = 1;
+        
+        btCompoundShape* cylinderCompound = new btCompoundShape;
+        btCollisionShape* cylinderShape = new btCylinderShapeX(btVector3(2, .5, .5));
+        btCollisionShape* boxShape = new btBoxShape(btVector3(2, .5, .5));
+        btTransform localTransform;
+        localTransform.setIdentity();
+        cylinderCompound->addChildShape(localTransform, boxShape);
+        btQuaternion orn(SIMD_HALF_PI, 0, 0);
+        localTransform.setRotation(orn);
+        //    localTransform.setOrigin(btVector3(1,1,1));
+        cylinderCompound->addChildShape(localTransform, cylinderShape);
+        
+        btCollisionShape* shape[] = {
+            new btBoxShape(btVector3(1, 1, 1)),
+            cylinderCompound,
+            new btSphereShape(0.75)
+            
+        };
+        static const int nshapes = sizeof(shape) / sizeof(shape[0]);
+        for (int i = 0; i < count; ++i)
+        {
+            btTransform startTransform;
+            startTransform.setIdentity();
+            startTransform.setOrigin(btVector3(0, 3 + 3 * i, 0));
+            createRigidBody(mass, startTransform, shape[i % nshapes]);
+        }
+    }
+    
     virtual const btDeformableRigidDynamicsWorld* getDeformableDynamicsWorld() const
     {
         ///just make it a btSoftRigidDynamicsWorld please
@@ -105,7 +134,6 @@ void DeformableDemo::initPhysics()
 	m_guiHelper->setUpAxis(1);
 
 	///collision configuration contains default setup for memory, collision setup
-//    m_collisionConfiguration = new btDefaultCollisionConfiguration();
     m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration();
 
 	///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
@@ -119,22 +147,22 @@ void DeformableDemo::initPhysics()
 	m_solver = sol;
 
 	m_dynamicsWorld = new btDeformableRigidDynamicsWorld(m_dispatcher, m_broadphase, m_solver, m_collisionConfiguration, deformableBodySolver);
+    deformableBodySolver->setWorld(getDeformableDynamicsWorld());
 	//	m_dynamicsWorld->getSolverInfo().m_singleAxisDeformableThreshold = 0.f;//faster but lower quality
 	m_dynamicsWorld->setGravity(btVector3(0, -10, 0));
     getDeformableDynamicsWorld()->getSoftDynamicsWorld()->getWorldInfo().m_gravity.setValue(0, -10, 0);
 	m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
 
     {
-        ///create a few basic rigid bodies
-//        btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(10.), btScalar(5.), btScalar(25.)));
-        btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(50.), btScalar(25.), btScalar(50.)));
+        ///create a ground
+        btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(150.), btScalar(25.), btScalar(150.)));
 
         m_collisionShapes.push_back(groundShape);
 
         btTransform groundTransform;
         groundTransform.setIdentity();
-        groundTransform.setOrigin(btVector3(0, -30, 0));
-//        groundTransform.setRotation(btQuaternion(btVector3(0, 1, 0), SIMD_PI * 0.03));
+        groundTransform.setOrigin(btVector3(0, -50, 0));
+        groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.0));
         //We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here:
         btScalar mass(0.);
 
@@ -151,76 +179,30 @@ void DeformableDemo::initPhysics()
         btRigidBody* body = new btRigidBody(rbInfo);
         body->setFriction(.5);
 
-        //add the body to the dynamics world
+        //add the ground to the dynamics world
         m_dynamicsWorld->addRigidBody(body);
     }
-//
-//    {
-//        ///create a few basic rigid bodies
-//        btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(100.), btScalar(100.), btScalar(50.)));
-//
-//        m_collisionShapes.push_back(groundShape);
-//
-//        btTransform groundTransform;
-//        groundTransform.setIdentity();
-//        groundTransform.setOrigin(btVector3(0, 0, -54));
-//        //We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here:
-//        btScalar mass(0.);
-//
-//        //rigidbody is dynamic if and only if mass is non zero, otherwise static
-//        bool isDynamic = (mass != 0.f);
-//
-//        btVector3 localInertia(0, 0, 0);
-//        if (isDynamic)
-//            groundShape->calculateLocalInertia(mass, localInertia);
-//
-//        //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
-//        btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform);
-//        btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, groundShape, localInertia);
-//        btRigidBody* body = new btRigidBody(rbInfo);
-//        body->setFriction(.1);
-//        //add the body to the dynamics world
-//        m_dynamicsWorld->addRigidBody(body);
-//    }
-//
-//    {
-//        // add a simple deformable body
-//        const btVector3 s(3,2,1); // side length
-//        const btVector3 p(0,30,0); // origin;
-//        const btVector3 h = s * 0.5;
-//        const btVector3 c[] = {p + h * btVector3(-1, -1, -1),
-//            p + h * btVector3(+1, -1, -1),
-//            p + h * btVector3(-1, +1, -1),
-//            p + h * btVector3(+1, +1, -1),
-//            p + h * btVector3(-1, -1, +1),
-//            p + h * btVector3(+1, -1, +1),
-//            p + h * btVector3(-1, +1, +1),
-//            p + h * btVector3(+1, +1, +1)};
-//        btSoftBody* psb = btSoftBodyHelpers::CreateFromConvexHull(getDeformableDynamicsWorld()->getSoftDynamicsWorld()->getWorldInfo(), c, 8);
-//        psb->generateBendingConstraints(2);
-//        psb->m_cfg.collisions |= btSoftBody::fCollision::VF_SS;
-//        psb->setTotalMass(150);
-//        getDeformableDynamicsWorld()->addSoftBody(psb);
-//    }
+    
+    // create a piece of cloth
     {
-        const btScalar s = 8;
+        const btScalar s = 4;
         btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getSoftDynamicsWorld()->getWorldInfo(), btVector3(-s, 0, -s),
                                                          btVector3(+s, 0, -s),
                                                          btVector3(-s, 0, +s),
                                                          btVector3(+s, 0, +s),
-                                                         10, 10,
-                                                         //        31,31,
+//                                                         3, 3,
+                                                                 20,20,
                                                           1 + 2 + 4 + 8, true);
 //                                                         0, true);
         
-        psb->getCollisionShape()->setMargin(0.5);
-//        btSoftBody::Material* pm = psb->appendMaterial();
-//        pm->m_kLST = 0.4 * 1000;
-//        pm->m_flags -= btSoftBody::fMaterial::DebugDraw;
+        psb->getCollisionShape()->setMargin(0.25);
         psb->generateBendingConstraints(2);
-        psb->setTotalMass(1);
-        psb->setDampingCoefficient(0.01);
+        psb->setTotalMass(2);
+        psb->setDampingCoefficient(0.02);
         getDeformableDynamicsWorld()->addSoftBody(psb);
+        
+        // add a few rigid bodies
+        Ctor_RbUpStack(10);
     }
 	m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
 }
@@ -263,7 +245,10 @@ void DeformableDemo::exitPhysics()
 }
 
 
+
 class CommonExampleInterface* DeformableCreateFunc(struct CommonExampleOptions& options)
 {
 	return new DeformableDemo(options.m_guiHelper);
 }
+
+
diff --git a/src/BulletSoftBody/btBackwardEulerObjective.h b/src/BulletSoftBody/btBackwardEulerObjective.h
index cef07aea1..5e81fc0f2 100644
--- a/src/BulletSoftBody/btBackwardEulerObjective.h
+++ b/src/BulletSoftBody/btBackwardEulerObjective.h
@@ -40,7 +40,7 @@ public:
     : cg(20)
     , m_softBodies(softBodies)
     , precondition(DefaultPreconditioner())
-    , projection(m_softBodies, backup_v)
+    , projection(m_softBodies)
     {
         // TODO: this should really be specified in initialization instead of here
         btMassSpring* mass_spring = new btMassSpring(m_softBodies);
diff --git a/src/BulletSoftBody/btCGProjection.h b/src/BulletSoftBody/btCGProjection.h
index 89743ef27..533e19c7a 100644
--- a/src/BulletSoftBody/btCGProjection.h
+++ b/src/BulletSoftBody/btCGProjection.h
@@ -23,11 +23,9 @@ public:
     std::unordered_map<btSoftBody::Node *, size_t> m_indices;
     TVArrayStack m_constrainedDirections;
     TArrayStack m_constrainedValues;
-    const TVStack& m_backupVelocity;
     
-    btCGProjection(btAlignedObjectArray<btSoftBody *>& softBodies, const TVStack& backup_v)
+    btCGProjection(btAlignedObjectArray<btSoftBody *>& softBodies)
     : m_softBodies(softBodies)
-    , m_backupVelocity(backup_v)
     {
         
     }
diff --git a/src/BulletSoftBody/btContactProjection.cpp b/src/BulletSoftBody/btContactProjection.cpp
index 2666ff789..8f9f9225a 100644
--- a/src/BulletSoftBody/btContactProjection.cpp
+++ b/src/BulletSoftBody/btContactProjection.cpp
@@ -9,17 +9,6 @@
 #include "btDeformableRigidDynamicsWorld.h"
 void btContactProjection::update(btScalar dt, const TVStack& dv)
 {
-    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)
-        {
-            psb->m_nodes[j].m_v = m_backupVelocity[counter] + dv[counter];
-            ++counter;
-        }
-    }
-        
     ///solve rigid body constraints
     m_world->btSoftRigidDynamicsWorld::btDiscreteDynamicsWorld::solveConstraints(m_world->getSolverInfo());
     
@@ -31,7 +20,7 @@ void btContactProjection::update(btScalar dt, const TVStack& dv)
     }
     
     // Set dirichlet constraints
-    counter = 0;
+    size_t counter = 0;
     for (int i = 0; i < m_softBodies.size(); ++i)
     {
         const btSoftBody* psb = m_softBodies[i];
@@ -76,7 +65,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) + btVector3(0,-10,0)*dt) * dt : btVector3(0, 0, 0);
+                    va = rigidCol ? (rigidCol->getVelocityInLocalPoint(c.m_c1)) * dt : btVector3(0, 0, 0);
                 }
                 else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
                 {
@@ -114,11 +103,9 @@ void btContactProjection::update(btScalar dt, const TVStack& dv)
 //                    const btVector3 impulse = c.m_c0 * ((vr - (fv * c.m_c3)));
                     const btVector3 impulse = c.m_c0 * ((vr - (fv * c.m_c3))+ (cti.m_normal * (dp * c.m_c4)));
                     
-                    //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 diff = c.m_node->m_v - m_backupVelocity[m_indices[c.m_node]];
-                    btVector3 dv = -impulse * c.m_c2/dt + diff;
+                    btVector3 dv = -impulse * c.m_c2/dt;
                     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/btContactProjection.h b/src/BulletSoftBody/btContactProjection.h
index dc2c8800e..e1c3bc50d 100644
--- a/src/BulletSoftBody/btContactProjection.h
+++ b/src/BulletSoftBody/btContactProjection.h
@@ -15,8 +15,8 @@
 class btContactProjection : public btCGProjection
 {
 public:
-    btContactProjection(btAlignedObjectArray<btSoftBody *>& softBodies, const btAlignedObjectArray<btVector3>& backup_v)
-    : btCGProjection(softBodies, backup_v)
+    btContactProjection(btAlignedObjectArray<btSoftBody *>& softBodies)
+    : btCGProjection(softBodies)
     {
         
     }
diff --git a/src/BulletSoftBody/btDeformableBodySolver.h b/src/BulletSoftBody/btDeformableBodySolver.h
index 5a0a3a05a..9afec42dc 100644
--- a/src/BulletSoftBody/btDeformableBodySolver.h
+++ b/src/BulletSoftBody/btDeformableBodySolver.h
@@ -82,13 +82,11 @@ public:
     {
         bool nodeUpdated = updateNodes();
         reinitialize(nodeUpdated);
-        backupVelocity();
         for (int i = 0; i < m_solveIterations; ++i)
         {
             // 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();
@@ -116,7 +114,6 @@ public:
         {
             m_dv.resize(m_numNodes);
             m_residual.resize(m_numNodes);
-            m_backupVelocity.resize(m_numNodes);
         }
         
         for (int i = 0; i < m_dv.size(); ++i)
@@ -150,11 +147,6 @@ public:
                 auto& node = psb->m_nodes[j];
 //                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;
-                }
             }
         }
     }
@@ -168,27 +160,13 @@ public:
             btSoftBody* psb = m_softBodySet[i];
             for (int j = 0; j < psb->m_nodes.size(); ++j)
             {
-                psb->m_nodes[j].m_v = m_backupVelocity[counter] + m_dv[counter];
+                psb->m_nodes[j].m_v += m_dv[counter];
                 
                 ++counter;
             }
         }
     }
     
-    void backupVelocity()
-    {
-        // 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)
-            {
-                m_backupVelocity[counter++] = psb->m_nodes[j].m_v;
-            }
-        }
-    }
-    
     bool updateNodes()
     {
         int numNodes = 0;
diff --git a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp
index c0afc98d4..82b24ccb8 100644
--- a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp
+++ b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp
@@ -53,6 +53,14 @@ void btDeformableRigidDynamicsWorld::internalSingleStepSimulation(btScalar timeS
     
 //    btSoftRigidDynamicsWorld::btDiscreteDynamicsWorld::internalSingleStepSimulation(timeStep);
     
+    // incorporate gravity into velocity and clear force
+    for (int i = 0; i < m_nonStaticRigidBodies.size(); ++i)
+    {
+        btRigidBody* rb = m_nonStaticRigidBodies[i];
+        rb->integrateVelocities(timeStep);
+    }
+    clearForces();
+    
     ///solve deformable bodies constraints
     solveDeformableBodiesConstraints(timeStep);
 
@@ -67,12 +75,7 @@ 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
     // ///////////////////////////////
 }