rotation is working

examples/ReducedDeformableDemo/BasicTest.cpp

@@ -27,12 +27,12 @@
 #include "../Utils/b3ResourcePath.h"
 
 ///The BasicTest shows the contact between volumetric deformable objects and rigid objects.
-static btScalar E = 50;
-static btScalar nu = 0.3;
+// static btScalar E = 50;
+// static btScalar nu = 0.3;
 // static btScalar damping_alpha = 0.1;
 // static btScalar damping_beta = 0.01;
-static btScalar damping_alpha = 0.0;
-static btScalar damping_beta = 0.0;
+// static btScalar damping_alpha = 0.0;
+// static btScalar damping_beta = 0.0;
 static btScalar COLLIDING_VELOCITY = 0;
 
 class BasicTest : public CommonDeformableBodyBase
@@ -69,7 +69,7 @@ public:
 
     void resetCamera()
     {
-        float dist = 20;
+        float dist = 10;
         float pitch = 0;
         float yaw = 90;
         float targetPos[3] = {0, 3, 0};
@@ -89,28 +89,17 @@ public:
     
     void stepSimulation(float deltaTime)
     {
-      btReducedSoftBody* rsb = static_cast<btReducedSoftBody*>(static_cast<btDeformableMultiBodyDynamicsWorld*>(m_dynamicsWorld)->getSoftBodyArray()[0]);
-      
       // TODO: remove this. very hacky way of adding initial deformation
+      btReducedSoftBody* rsb = static_cast<btReducedSoftBody*>(static_cast<btDeformableMultiBodyDynamicsWorld*>(m_dynamicsWorld)->getSoftBodyArray()[0]);
       if (first_step && !rsb->m_bUpdateRtCst) 
       {
         // getDeformedShape(rsb, 0, 0.5);
         first_step = false;
         rsb->updateReducedDofs();
-        // std::cout << rsb->m_reducedDofs[0] << "\n";
       }
-
-      // compute reduced dofs
-      sim_time += deltaTime;
-      // std::cout << rsb->m_eigenvalues[0] << "\t" << sim_time << "\t" << deltaTime  << "\t" << sin(rsb->m_eigenvalues[0] * sim_time) << "\n";
       
       float internalTimeStep = 1. / 60.f;
       m_dynamicsWorld->stepSimulation(deltaTime, 1, internalTimeStep);
-      // float internalTimeStep = 1;
-      // m_dynamicsWorld->stepSimulation(1, 1, internalTimeStep);
-
-      // map reduced dof back to full
-      rsb->updateFullDofs();
     }
     
     virtual void renderScene()
@@ -161,10 +150,10 @@ void BasicTest::initPhysics()
         btReducedSoftBodyHelpers::readReducedDeformableInfoFromFiles(rsb, filepath.c_str());
 
         getDeformableDynamicsWorld()->addSoftBody(rsb);
-        rsb->scale(btVector3(2, 2, 2));
-        rsb->translate(btVector3(0, 7, 0));
+        // rsb->scale(btVector3(1, 1, 1));
+        // rsb->translate(btVector3(0, 0, 0));
         rsb->getCollisionShape()->setMargin(0.1);
-        rsb->setTotalMass(0.5);
+        // rsb->setTotalMass(0.5);
         rsb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
         rsb->m_cfg.kCHR = 1; // collision hardness with rigid body
         rsb->m_cfg.kDF = 0;

src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp

@@ -31,10 +31,19 @@ void btReducedSoftBody::setReducedModes(int start_mode, int num_modes, int full_
   m_nodalMass.resize(full_size, 0);
 }
 
-void btReducedSoftBody::setMass(btScalar m)
+void btReducedSoftBody::setMass(const tDenseArray& mass_array)
 {
-  m_mass = m;
-  m_inverseMass = m > 0 ? 1.0 / m : 0;
+  // nodal mass
+  btScalar total_mass = 0;
+	for (int i = 0; i < m_nFull; ++i)
+	{
+		m_nodalMass[i] = mass_array[3 * i];
+		m_nodes[i].m_im = mass_array[3 * i] > 0 ? mass_array[3 * i] : 0;
+		total_mass += mass_array[3 * i];
+	}
+  // total rigid body mass
+  m_mass = total_mass;
+  m_inverseMass = total_mass > 0 ? 1.0 / total_mass : 0;
 }
 
 void btReducedSoftBody::predictIntegratedTransform(btScalar timeStep, btTransform& predictedTransform)
@@ -51,7 +60,7 @@ void btReducedSoftBody::updateReducedDofs()
     m_reducedDofs[j] = 0;
     for (int i = 0; i < m_nFull; ++i)
       for (int k = 0; k < 3; ++k)
-        m_reducedDofs[j] += m_modes[j][3 * i + k] * (m_nodes[i].m_x[k] - m_x0[3 * i + k]);
+        m_reducedDofs[j] += m_modes[j][3 * i + k] * (m_nodes[i].m_x[k] - m_x0[i][k]);
   }
 }
 
@@ -61,6 +70,7 @@ void btReducedSoftBody::updateFullDofs()
   btAlignedObjectArray<btVector3> delta_x;
   delta_x.resize(m_nFull);
   btVector3 origin = getWorldTransform().getOrigin();
+  btMatrix3x3 rotation = getWorldTransform().getBasis();
 
   for (int i = 0; i < m_nFull; ++i)
   {
@@ -72,9 +82,9 @@ void btReducedSoftBody::updateFullDofs()
       {
         delta_x[i][k] += m_modes[j][3 * i + k] * m_reducedDofs[j];
       }
-      // get new coordinates
-      m_nodes[i].m_x[k] = m_x0[3 * i + k] + delta_x[i][k] + origin[k]; //TODO: assume the initial origin is at (0,0,0)
     }
+    // get new coordinates
+    m_nodes[i].m_x = rotation * (m_x0[i] + delta_x[i]) + origin; //TODO: assume the initial origin is at (0,0,0)
   }
 }
 

src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h

@@ -50,18 +50,19 @@ class btReducedSoftBody : public btSoftBody
 	tDenseArray m_Mr;	// reduced mass matrix //TODO: do we need this?
   
   // full space
-  tDenseArray m_x0;									 // Rest position
+  TVStack m_x0;					     				 // Rest position
   tDenseArray m_nodalMass;           // Mass on each node
 
   //
   // Api
   //
   btReducedSoftBody(btSoftBodyWorldInfo* worldInfo, int node_count, const btVector3* x, const btScalar* m);
+
   ~btReducedSoftBody() {}
 
   void setReducedModes(int start_mode, int num_modes, int full_size);
 
-  void setMass(btScalar m);
+  void setMass(const tDenseArray& mass_array);
 
   void predictIntegratedTransform(btScalar step, btTransform& predictedTransform);
 
@@ -75,9 +76,9 @@ class btReducedSoftBody : public btSoftBody
   // compute full degree of freedoms
   void updateFullDofs();
 
-
+  //
   // rigid motion related
-
+  //
   void applyCentralImpulse(const btVector3& impulse);
 
 	void applyTorqueImpulse(const btVector3& torque);

src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodyHelpers.cpp

@@ -110,10 +110,9 @@ btReducedSoftBody* btReducedSoftBodyHelpers::createFromVtkFile(btSoftBodyWorldIn
 	fs.close();
 
 	// get rest position
-	rsb->m_x0.resize(3 * rsb->m_nodes.size());
+	rsb->m_x0.resize(rsb->m_nodes.size());
 	for (int i = 0; i < rsb->m_nodes.size(); ++i)
-		for (int k = 0; k < 3; ++k)
-			rsb->m_x0[3 * i + k] = rsb->m_nodes[i].m_x[k];
+		rsb->m_x0[i] = rsb->m_nodes[i].m_x;
 
 	return rsb;
 }
@@ -133,18 +132,11 @@ 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
 
+	// read in full nodal mass
 	std::string M_file = std::string(file_path) + "M_diag_mat.bin";
 	btAlignedObjectArray<btScalar> mass_array;
 	btReducedSoftBodyHelpers::readBinary(mass_array, 0, 3 * rsb->m_nFull, 3 * rsb->m_nFull, M_file.c_str());
-	// assign mass to nodes
-	btScalar mass = 0;
-	for (int i = 0; i < rsb->m_nodes.size(); ++i)
-	{
-		rsb->m_nodalMass[i] = mass_array[3 * i];
-		rsb->m_nodes[i].m_im = mass_array[3 * i] > 0 ? mass_array[3 * i] : 0;
-		mass += mass_array[3 * i];
-	}
-	rsb->setMass(mass);
+	rsb->setMass(mass_array);
 }
 
 // read in binary files

src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp

@@ -41,7 +41,7 @@ void btReducedSoftBodySolver::applyForce()
 
     // apply impulses to reduced deformable objects
     static btScalar sim_time = 0;
-    static btScalar target_vel = 5;
+    static btScalar target_vel = 1;
     static int apply_impulse = 0;
     if (rsb->m_reducedModel && apply_impulse < 4)
     {
@@ -114,5 +114,8 @@ void btReducedSoftBodySolver::applyTransforms(btScalar timeStep)
     rsb->predictIntegratedTransform(timeStep, rsb->getInterpolationWorldTransform());
 
     rsb->proceedToTransform(rsb->getInterpolationWorldTransform());
+
+    // map reduced dof back to full space
+    rsb->updateFullDofs();
   }
 }