initial translation WIP

examples/ReducedDeformableDemo/BasicTest.cpp

@@ -95,7 +95,7 @@ public:
       btReducedSoftBody* rsb = static_cast<btReducedSoftBody*>(static_cast<btDeformableMultiBodyDynamicsWorld*>(m_dynamicsWorld)->getSoftBodyArray()[0]);
       if (first_step /* && !rsb->m_bUpdateRtCst*/) 
       {
-        getDeformedShape(rsb, 0, 0.5);
+        // getDeformedShape(rsb, 0, 0.5);
         first_step = false;
         rsb->updateReducedDofs();
       }
@@ -133,13 +133,14 @@ void BasicTest::initPhysics()
     m_broadphase = new btDbvtBroadphase();
     btReducedSoftBodySolver* reducedSoftBodySolver = new btReducedSoftBodySolver();
     reducedSoftBodySolver->setDamping(damping_alpha, damping_beta);
+    btVector3 gravity = btVector3(0, 0, 0);
+    reducedSoftBodySolver->setGravity(gravity);
 
     btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver();
     sol->setDeformableSolver(reducedSoftBodySolver);
     m_solver = sol;
 
     m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, reducedSoftBodySolver);
-    btVector3 gravity = btVector3(0, -10, 0);
     m_dynamicsWorld->setGravity(gravity);
     m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
 
@@ -153,9 +154,9 @@ void BasicTest::initPhysics()
         btReducedSoftBodyHelpers::readReducedDeformableInfoFromFiles(rsb, filepath.c_str());
 
         getDeformableDynamicsWorld()->addSoftBody(rsb);
-        // rsb->scale(btVector3(1, 1, 1));
-        // rsb->translate(btVector3(0, 0, 0));
         rsb->getCollisionShape()->setMargin(0.1);
+        // rsb->scale(btVector3(1, 1, 1));
+        rsb->translate(btVector3(0, 2, 0));  //TODO: add back translate and scale
         // rsb->setTotalMass(0.5);
         rsb->setStiffnessScale(1);
         rsb->m_cfg.kKHR = 1; // collision hardness with kinematic objects

src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp

@@ -83,7 +83,6 @@ void btReducedSoftBody::predictIntegratedTransform(btScalar timeStep, btTransfor
 	btTransformUtil::integrateTransform(m_worldTransform, m_linearVelocity, m_angularVelocity, timeStep, predictedTransform);
 }
 
-
 void btReducedSoftBody::updateReducedDofs()
 {
   btAssert(m_reducedDofs.size() == m_nReduced);
@@ -91,8 +90,13 @@ void btReducedSoftBody::updateReducedDofs()
   {
     m_reducedDofs[j] = 0;
     for (int i = 0; i < m_nFull; ++i)
+    {
       for (int k = 0; k < 3; ++k)
+      {
+        // std::cout << m_nodes[i].m_x[k] - m_x0[i][k] << "\n";
         m_reducedDofs[j] += m_modes[j][3 * i + k] * (m_nodes[i].m_x[k] - m_x0[i][k]);
+      }
+    }
   }
 }
 
@@ -141,6 +145,26 @@ void btReducedSoftBody::setCenterOfMassTransform(const btTransform& xform)
 	updateInertiaTensor();
 }
 
+void btReducedSoftBody::translate(const btVector3& trs)
+{
+  // translate mesh
+	btSoftBody::translate(trs);
+  updateRestNodalPositions();
+  // for (int i = 0; i < m_nFull; ++i)
+  //   for (int k = 0; k < 3; ++k)
+  //     std::cout << m_nodes[i].m_x[k] << "\t" << m_x0[i][k] << "\n";
+  
+  // update rigid frame
+  // m_worldTransform.setOrigin(trs);
+}
+
+void btReducedSoftBody::updateRestNodalPositions()
+{
+  m_x0.resize(m_nFull);
+	for (int i = 0; i < m_nFull; ++i)
+		m_x0[i] = m_nodes[i].m_x;
+}
+
 void btReducedSoftBody::updateInertiaTensor()
 {
 	m_invInertiaTensorWorld = m_worldTransform.getBasis().scaled(m_invInertiaLocal) * m_worldTransform.getBasis().transpose();
@@ -189,6 +213,11 @@ void btReducedSoftBody::applyFullSpaceImpulse(const btVector3& target_vel, int n
   applyImpulse(impulse, m_nodes[n_node].m_x);
 }
 
+void btReducedSoftBody::applyRigidGravity(const btVector3& gravity, btScalar dt)
+{
+  m_linearVelocity += dt * gravity;
+}
+
 void btReducedSoftBody::applyReducedInternalForce(tDenseArray& reduced_force, const btScalar damping_alpha, const btScalar damping_beta)
 {
   for (int r = 0; r < m_nReduced; ++r) 

src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h

@@ -19,7 +19,6 @@ class btReducedSoftBody : public btSoftBody
   // rigid frame
   btScalar m_mass;          // total mass of the rigid frame
   btScalar m_inverseMass;   // inverse of the total mass of the rigid frame
-  btVector3 m_linearVelocity;
 	btVector3 m_angularVelocity;
 	btVector3 m_linearFactor;
 	btVector3 m_angularFactor;
@@ -34,6 +33,7 @@ class btReducedSoftBody : public btSoftBody
   typedef btAlignedObjectArray<btScalar> tDenseArray;
   typedef btAlignedObjectArray<btAlignedObjectArray<btScalar> > tDenseMatrix;
 
+  btVector3 m_linearVelocity;
   //
   //  Fields
   //
@@ -74,6 +74,10 @@ class btReducedSoftBody : public btSoftBody
 
   void setStiffnessScale(const btScalar ks);
 
+  virtual void translate(const btVector3& trs);
+
+  void updateRestNodalPositions();
+
   void updateInertiaTensor();
 
   void predictIntegratedTransform(btScalar step, btTransform& predictedTransform);
@@ -101,6 +105,9 @@ class btReducedSoftBody : public btSoftBody
   // apply impulse to nodes in the full space
   void applyFullSpaceImpulse(const btVector3& target_vel, int n_node, btScalar dt, tDenseArray& reduced_force);
 
+  // apply gravity to the rigid frame
+  void applyRigidGravity(const btVector3& gravity, btScalar dt);
+
   // apply reduced force
   void applyReducedInternalForce(tDenseArray& reduced_force, const btScalar damping_alpha, const btScalar damping_beta);
 

src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodyHelpers.cpp

@@ -109,11 +109,6 @@ btReducedSoftBody* btReducedSoftBodyHelpers::createFromVtkFile(btSoftBodyWorldIn
 
 	fs.close();
 
-	// get rest position
-	rsb->m_x0.resize(rsb->m_nodes.size());
-	for (int i = 0; i < rsb->m_nodes.size(); ++i)
-		rsb->m_x0[i] = rsb->m_nodes[i].m_x;
-
 	return rsb;
 }
 
@@ -132,6 +127,9 @@ void btReducedSoftBodyHelpers::readReducedDeformableInfoFromFiles(btReducedSoftB
 	std::string modes_file = std::string(file_path) + "modes.bin";
 	btReducedSoftBodyHelpers::readBinaryModes(rsb->m_modes, rsb->m_startMode, rsb->m_nReduced, 3 * rsb->m_nFull, modes_file.c_str());	// default to 3D
 
+	// get rest position
+	rsb->updateRestNodalPositions();
+	
 	// read in full nodal mass
 	std::string M_file = std::string(file_path) + "M_diag_mat.bin";
 	btAlignedObjectArray<btScalar> mass_array;

src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp

@@ -13,17 +13,14 @@ void btReducedSoftBodySolver::setDamping(btScalar alpha, btScalar beta)
   m_dampingBeta = beta;
 }
 
+void btReducedSoftBodySolver::setGravity(const btVector3& gravity)
+{
+  m_gravity = gravity;
+}
+
 void btReducedSoftBodySolver::predictMotion(btScalar solverdt)
 {
-  applyForce();
-  
-  // apply rigid motion
-  for (int i = 0; i < m_softBodies.size(); ++i)
-  {
-    btReducedSoftBody* rsb = static_cast<btReducedSoftBody*>(m_softBodies[i]);
-
-    // rsb->predictIntegratedTransform(solverdt, rsb->getInterpolationWorldTransform());
-  }
+  applyExplicitForce();
 }
 
 void btReducedSoftBodySolver::applyForce()
@@ -51,12 +48,12 @@ void btReducedSoftBodySolver::applyForce()
       }
       if (sim_time > 2 && apply_impulse == 1) 
       {
-        rsb->applyFullSpaceImpulse(btVector3(0, -1.2, 0), 0, 2.0 * m_dt, reduced_force);
+        rsb->applyFullSpaceImpulse(btVector3(0, -1.2, 0), 0, m_dt, reduced_force);
         apply_impulse++;
       }
       if (sim_time > 3 && apply_impulse == 2) 
       {
-        rsb->applyFullSpaceImpulse(btVector3(1.1, 0, 0), 0, 2.0 * m_dt, reduced_force);
+        rsb->applyFullSpaceImpulse(btVector3(1.1, 0, 0), 0, m_dt, reduced_force);
         apply_impulse++;
       }
       if (sim_time > 4 && apply_impulse == 3) 
@@ -80,6 +77,14 @@ void btReducedSoftBodySolver::applyForce()
 
 void btReducedSoftBodySolver::applyExplicitForce()
 {
+  // apply gravity to the rigid frame
+  for (int i = 0; i < m_softBodies.size(); ++i)
+  {
+    btReducedSoftBody* rsb = static_cast<btReducedSoftBody*>(m_softBodies[i]);
+    rsb->applyRigidGravity(m_gravity, m_dt);
+  }
+
+  // apply internal forces and impulses
   applyForce();
 }
 

src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.h

@@ -12,6 +12,8 @@ class btReducedSoftBodySolver : public btDeformableBodySolver
   btScalar m_dampingAlpha;
   btScalar m_dampingBeta;
 
+  btVector3 m_gravity;
+
   void applyForce();
 
  public:
@@ -20,6 +22,8 @@ class btReducedSoftBodySolver : public btDeformableBodySolver
 
   void setDamping(btScalar alpha, btScalar beta);
 
+  void setGravity(const btVector3& gravity);
+
   virtual SolverTypes getSolverType() const
   {
     return REDUCED_DEFORMABLE_SOLVER;

src/BulletSoftBody/btSoftBody.h

@@ -1006,7 +1006,7 @@ public:
 	/* Transform															*/
 	void transform(const btTransform& trs);
 	/* Translate															*/
-	void translate(const btVector3& trs);
+	virtual void translate(const btVector3& trs);
 	/* Rotate															*/
 	void rotate(const btQuaternion& rot);
 	/* Scale																*/