re-work the simulation time loop in order to support fixed constraints. still wip

examples/ReducedDeformableDemo/BasicTest.cpp

@@ -95,9 +95,9 @@ 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();
+        rsb->mapToReducedDofs();
       }
       
       float internalTimeStep = 1. / 60.f;
@@ -138,7 +138,7 @@ void BasicTest::initPhysics()
     m_broadphase = new btDbvtBroadphase();
     btReducedSoftBodySolver* reducedSoftBodySolver = new btReducedSoftBodySolver();
     reducedSoftBodySolver->setDamping(damping_alpha, damping_beta);
-    btVector3 gravity = btVector3(0, -9.8, 0);
+    btVector3 gravity = btVector3(0, 0, 0);
     reducedSoftBodySolver->setGravity(gravity);
 
     btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver();
@@ -164,7 +164,7 @@ void BasicTest::initPhysics()
         rsb->translate(btVector3(0, 4, 0));  //TODO: add back translate and scale
         // rsb->setTotalMass(0.5);
         rsb->setStiffnessScale(1);
-        // rsb->setFixedNodes();
+        rsb->setFixedNodes();
         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;
@@ -175,7 +175,7 @@ void BasicTest::initPhysics()
         
         // rsb->setVelocity(btVector3(0, -COLLIDING_VELOCITY, 0));
         // rsb->setRigidVelocity(btVector3(0, 1, 0));
-        rsb->setRigidAngularVelocity(btVector3(10, 0, 0));
+        // rsb->setRigidAngularVelocity(btVector3(1, 0, 0));
         
         // btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity);
         // getDeformableDynamicsWorld()->addForce(rsb, gravity_force);

src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp

@@ -34,7 +34,9 @@ void btReducedSoftBody::setReducedModes(int start_mode, int num_modes, int full_
   m_nFull = full_size;
   m_reducedDofs.resize(m_nReduced, 0);
   m_reducedVelocity.resize(m_nReduced, 0);
+  m_reducedForce.resize(m_nReduced, 0);
   m_nodalMass.resize(full_size, 0);
+  endOfTimeStepZeroing();
 }
 
 void btReducedSoftBody::setMassProps(const tDenseArray& mass_array)
@@ -95,12 +97,20 @@ void btReducedSoftBody::setFixedNodes()
   m_fixedNodes.push_back(0);
 }
 
+void btReducedSoftBody::endOfTimeStepZeroing()
+{
+  for (int i = 0; i < m_nReduced; ++i)
+  {
+    m_reducedForce[i] = 0;
+  }
+}
+
 void btReducedSoftBody::predictIntegratedTransform(btScalar timeStep, btTransform& predictedTransform)
 {
 	btTransformUtil::integrateTransform(m_rigidTransformWorld, m_linearVelocity, m_angularVelocity, timeStep, predictedTransform);
 }
 
-void btReducedSoftBody::updateReducedDofs()
+void btReducedSoftBody::mapToReducedDofs()
 {
   btAssert(m_reducedDofs.size() == m_nReduced);
   for (int j = 0; j < m_nReduced; ++j) 
@@ -117,7 +127,23 @@ void btReducedSoftBody::updateReducedDofs()
   }
 }
 
-void btReducedSoftBody::updateFullDofs()
+void btReducedSoftBody::updateReducedDofs(btScalar solverdt)
+{
+  for (int r = 0; r < m_nReduced; ++r)
+  { 
+    m_reducedDofs[r] += solverdt * m_reducedVelocity[r];
+  }
+}
+
+void btReducedSoftBody::updateFullDofs(btScalar solverdt)
+{
+  for (int i = 0; i < m_nFull; ++i)
+  {
+    m_nodes[i].m_x += solverdt * m_nodes[i].m_v;
+  }
+}
+
+void btReducedSoftBody::mapToFullDofs()
 {
   btAssert(m_nodes.size() == m_nFull);
   TVStack delta_x;
@@ -141,6 +167,51 @@ void btReducedSoftBody::updateFullDofs()
   }
 }
 
+void btReducedSoftBody::updateReducedVelocity(btScalar solverdt)
+{
+  // update reduced velocity
+  for (int r = 0; r < m_nReduced; ++r)
+  {
+    btScalar mass_inv = (m_Mr[r] == 0) ? 0 : 1.0 / m_Mr[r]; // TODO: this might be redundant, because Mr is identity
+    btScalar delta_v = solverdt * mass_inv * m_reducedForce[r];
+    m_reducedVelocity[r] -= delta_v; //! check this
+  }
+}
+
+void btReducedSoftBody::updateFullVelocity(btScalar solverdt)
+{
+  TVStack v_from_reduced;
+  v_from_reduced.resize(m_nFull);
+  // btVector3 current_com = m_rigidTransformWorld.getOrigin() - m_initialOrigin;  
+  for (int i = 0; i < m_nFull; ++i)
+  {
+    btVector3 r = m_nodes[i].m_x - m_rigidTransformWorld.getOrigin();
+
+    // compute velocity contributed by the reduced velocity
+    for (int k = 0; k < 3; ++k)
+    {
+      v_from_reduced[i][k] = 0;
+      for (int r = 0; r < m_nReduced; ++r)
+      {
+        v_from_reduced[i][k] += m_modes[r][3 * i + k] * m_reducedVelocity[r];
+      }
+    }
+
+    // get new velocity
+    m_nodes[i].m_v = m_angularVelocity.cross(r) + 
+                     m_rigidTransformWorld.getBasis() * v_from_reduced[i] +
+                     m_linearVelocity;
+  }
+}
+
+void btReducedSoftBody::updateMeshNodePositions(btScalar solverdt)
+{
+  for (int i = 0; i < m_nFull; ++i)
+  {
+    m_nodes[i].m_x = solverdt * m_nodes[i].m_v;
+  }
+}
+
 void btReducedSoftBody::proceedToTransform(const btTransform& newTrans)
 {
 	setCenterOfMassTransform(newTrans);
@@ -215,19 +286,29 @@ void btReducedSoftBody::applyImpulse(const btVector3& impulse, const btVector3&
   }
 }
 
-void btReducedSoftBody::applyFullSpaceImpulse(const btVector3& target_vel, int n_node, btScalar dt, tDenseArray& reduced_force)
+void btReducedSoftBody::applyFullSpaceImpulse(const btVector3& target_vel, int n_node, btScalar dt)
 {
+  // zero the reduced force vector
+  endOfTimeStepZeroing();
+
   // impulse leads to the deformation in the reduced space
   btVector3 impulse = m_nodalMass[n_node] / dt * (target_vel - m_nodes[n_node].m_v);
   for (int i = 0; i < m_nReduced; ++i)
   {
     for (int k = 0; k < 3; ++k)
     {
-      reduced_force[i] += m_modes[i][3 * n_node + k] * impulse[k];
+      m_reducedForce[i] = m_modes[i][3 * n_node + k] * impulse[k];
     }
   }
+
   // impulse causes rigid motion
   applyImpulse(impulse, m_nodes[n_node].m_x);
+
+  // apply impulse to the reduced velocity
+  updateReducedVelocity(dt);
+
+  // apply impulse force, update velocity
+  updateFullVelocity(dt);
 }
 
 void btReducedSoftBody::applyRigidGravity(const btVector3& gravity, btScalar dt)
@@ -235,27 +316,27 @@ void btReducedSoftBody::applyRigidGravity(const btVector3& gravity, btScalar dt)
   // update rigid frame velocity
   m_linearVelocity += dt * gravity;
 
-  // update nodal velocity
+  // // update nodal velocity
-  for (int i = 0; i < m_nFull; ++i)
+  // for (int i = 0; i < m_nFull; ++i)
-  {
+  // {
-    m_nodes[i].m_v = m_nodes[i].m_v + dt * gravity;
+  //   m_nodes[i].m_v += dt * gravity;
-  }
+  // }
 }
 
-void btReducedSoftBody::applyReducedInternalForce(tDenseArray& reduced_force, const btScalar damping_alpha, const btScalar damping_beta)
+void btReducedSoftBody::applyReducedInternalForce(const btScalar damping_alpha, const btScalar damping_beta)
 {
   for (int r = 0; r < m_nReduced; ++r) 
   {
-    reduced_force[r] += m_ksScale * m_Kr[r] * (m_reducedDofs[r] + damping_beta * m_reducedVelocity[r]);
+    m_reducedForce[r] += m_ksScale * m_Kr[r] * (m_reducedDofs[r] + damping_beta * m_reducedVelocity[r]);
   }
 }
 
-void btReducedSoftBody::applyFixedContraints(btScalar dt, tDenseArray& reduced_force)
+void btReducedSoftBody::applyFixedContraints(btScalar dt)
 {
   for (int n = 0; n < m_fixedNodes.size(); ++n)
   {
     // std::cout << reduced_force[0] << "\t" << reduced_force[1] << "\n";
-    applyFullSpaceImpulse(btVector3(0, 0, 0), m_fixedNodes[n], dt, reduced_force);
+    applyFullSpaceImpulse(btVector3(0, 0, 0), m_fixedNodes[n], dt);
     // std::cout << reduced_force[0] << "\t" << reduced_force[1] << "\n";
   }
 }

src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h

@@ -49,6 +49,7 @@ class btReducedSoftBody : public btSoftBody
 	tDenseMatrix m_modes;														// modes of the reduced deformable model. Each inner array is a mode, outer array size = n_modes
 	tDenseArray m_reducedDofs;				   // Reduced degree of freedom
 	tDenseArray m_reducedVelocity;		   // Reduced velocity array
+  tDenseArray m_reducedForce;          // reduced force
 	tDenseArray m_eigenvalues;		// eigenvalues of the reduce deformable model
 	tDenseArray m_Kr;	// reduced stiffness matrix
 	tDenseArray m_Mr;	// reduced mass matrix //TODO: do we need this?
@@ -89,15 +90,32 @@ class btReducedSoftBody : public btSoftBody
 
   void predictIntegratedTransform(btScalar step, btTransform& predictedTransform);
 
+  void endOfTimeStepZeroing();
+
   //
   // reduced dof related
   //
 
   // compute reduced degree of freedoms
-  void updateReducedDofs();
+  void updateReducedDofs(btScalar solverdt);
+
+  // map to reduced degree of freedoms
+  void mapToReducedDofs();
 
   // compute full degree of freedoms
-  void updateFullDofs();
+  void updateFullDofs(btScalar solverdt);
+
+  // map to full degree of freedoms
+  void mapToFullDofs();
+
+  // compute reduced velocity update
+  void updateReducedVelocity(btScalar solverdt);
+
+  // compute full space velocity from the reduced velocity
+  void updateFullVelocity(btScalar solverdt);
+
+  // update the full space mesh positions
+  void updateMeshNodePositions(btScalar solverdt);
 
   //
   // rigid motion related
@@ -110,16 +128,16 @@ class btReducedSoftBody : public btSoftBody
 	void applyImpulse(const btVector3& impulse, const btVector3& rel_pos);
 
   // apply impulse to nodes in the full space
-  void applyFullSpaceImpulse(const btVector3& target_vel, int n_node, btScalar dt, tDenseArray& reduced_force);
+  void applyFullSpaceImpulse(const btVector3& target_vel, int n_node, btScalar dt);
 
   // apply fixed contraints to the nodes
-  void applyFixedContraints(btScalar dt, tDenseArray& reduced_force);
+  void applyFixedContraints(btScalar dt);
 
   // 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);
+  void applyReducedInternalForce(const btScalar damping_alpha, const btScalar damping_beta);
 
   void proceedToTransform(const btTransform& newTrans);
 

src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp

@@ -21,93 +21,70 @@ void btReducedSoftBodySolver::setGravity(const btVector3& gravity)
 
 void btReducedSoftBodySolver::predictMotion(btScalar solverdt)
 {
-  applyExplicitForce();
+  applyExplicitForce(solverdt);
+
+  // predict new mesh location
+  predictReduceDeformableMotion(solverdt);
 }
 
-void btReducedSoftBodySolver::applyForce()
+void btReducedSoftBodySolver::predictReduceDeformableMotion(btScalar solverdt)
 {
   for (int i = 0; i < m_softBodies.size(); ++i)
   {
     btReducedSoftBody* rsb = static_cast<btReducedSoftBody*>(m_softBodies[i]);
 
-    // get reduced force
+    rsb->updateReducedVelocity(solverdt);
-    btAlignedObjectArray<btScalar> reduced_force;
+
-    reduced_force.resize(rsb->m_reducedDofs.size(), 0);
+    // map reduced velocity to full space velocity
+    rsb->updateFullVelocity(solverdt);
+
+    // update mesh nodal position
+    // rsb->updateMeshNodePositions(solverdt);
+  }
+}
+
+void btReducedSoftBodySolver::applyExplicitForce(btScalar solverdt)
+{
+  for (int i = 0; i < m_softBodies.size(); ++i)
+  {
+    btReducedSoftBody* rsb = static_cast<btReducedSoftBody*>(m_softBodies[i]);
+
+    // apply gravity to the rigid frame
+    rsb->applyRigidGravity(m_gravity, solverdt);
 
     // add internal force (elastic force & damping force)
-    rsb->applyReducedInternalForce(reduced_force, m_dampingAlpha, m_dampingBeta);
+    rsb->applyReducedInternalForce(m_dampingAlpha, m_dampingBeta);
-
-    // apply impulses to reduced deformable objects
-    static btScalar sim_time = 0;
-    static int apply_impulse = 0;
-    // if (rsb->m_reducedModel && apply_impulse < 4)
-    // {
-    //   if (sim_time > 1 && apply_impulse == 0) 
-    //   {
-    //     rsb->applyFullSpaceImpulse(btVector3(0, 1, 0), 0, 2.0 * m_dt, reduced_force);
-    //     apply_impulse++;
-    //   }
-    //   if (sim_time > 2 && apply_impulse == 1) 
-    //   {
-    //     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, m_dt, reduced_force);
-    //     apply_impulse++;
-    //   }
-    //   if (sim_time > 4 && apply_impulse == 3) 
-    //   {
-    //     rsb->applyFullSpaceImpulse(btVector3(-1, 0, 0), 0, 2.0 * m_dt, reduced_force);
-    //     apply_impulse++;
-    //   }
-    // }
-
-    // apply fixed contraints
-    // rsb->applyFixedContraints(m_dt, reduced_force); //TODO: solver iteratively with other constraints
-
-    // update reduced velocity
-    for (int r = 0; r < rsb->m_reducedDofs.size(); ++r)
-    {
-      btScalar mass_inv = (rsb->m_Mr[r] == 0) ? 0 : 1.0 / rsb->m_Mr[r];
-      btScalar delta_v = m_dt * mass_inv * reduced_force[r];
-      rsb->m_reducedVelocity[r] -= delta_v;
-    }
   }
 }
 
-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();
-}
-
 void btReducedSoftBodySolver::applyTransforms(btScalar timeStep)
 {
   for (int i = 0; i < m_softBodies.size(); ++i)
   {
     btReducedSoftBody* rsb = static_cast<btReducedSoftBody*>(m_softBodies[i]);
 
-    for (int r = 0; r < rsb->m_reducedDofs.size(); ++r)
+    // update reduced dofs for the next time step
-      rsb->m_reducedDofs[r] += timeStep * rsb->m_reducedVelocity[r];
+    rsb->updateReducedDofs(timeStep);
 
     // rigid motion
     btTransform predictedTrans;
     rsb->predictIntegratedTransform(timeStep, predictedTrans);
-    // std::cout << predictedTrans.getOrigin()[0] << '\t' << predictedTrans.getOrigin()[1] << '\t' << predictedTrans.getOrigin()[2] << '\n';
     rsb->proceedToTransform(predictedTrans);
-    // std::cout << rsb->getWorldTransform().getOrigin()[0] << '\t' << rsb->getWorldTransform().getOrigin()[1] << '\t' << rsb->getWorldTransform().getOrigin()[2] << '\n';
-    // std::cout << "----------\n";
 
-    // map reduced dof back to full space
+    // update mesh nodal positions for the next time step
-    rsb->updateFullDofs();
+    rsb->updateFullDofs(timeStep);
+
+    // end of time step clean up
+    rsb->endOfTimeStepZeroing();
   }
 }
+
+void btReducedSoftBodySolver::solveConstraints(btScalar timeStep)
+{
+  // for (int i = 0; i < m_softBodies.size(); ++i)
+  // {
+  //   btReducedSoftBody* rsb = static_cast<btReducedSoftBody*>(m_softBodies[i]);
+
+  //   rsb->applyFixedContraints(timeStep);
+  // }
+}

src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.h

@@ -14,7 +14,9 @@ class btReducedSoftBodySolver : public btDeformableBodySolver
 
   btVector3 m_gravity;
 
-  void applyForce();
+  void predictReduceDeformableMotion(btScalar solverdt);
+
+  void applyExplicitForce(btScalar solverdt);
 
  public:
   btReducedSoftBodySolver();
@@ -31,10 +33,17 @@ class btReducedSoftBodySolver : public btDeformableBodySolver
 
   virtual void predictMotion(btScalar solverdt);
 
-  virtual void applyExplicitForce();
-
   virtual void applyTransforms(btScalar timeStep);
 
+  virtual void solveConstraints(btScalar timeStep);
+
+  // virtual void setProjection() {}
+
+  // virtual void setLagrangeMultiplier() {}
+
+  // virtual void setupDeformableSolve(bool implicit);
+
+  // virtual void solveDeformableConstraints(btScalar solverdt);
 };
 
 #endif // BT_REDUCED_SOFT_BODY_DYNAMICS_WORLD_H

src/BulletSoftBody/btDeformableBodySolver.h

@@ -85,7 +85,7 @@ public:
 	void backupVelocity();
 
 	// set m_dv and m_backupVelocity to desired value to prepare for momentum solve
-	void setupDeformableSolve(bool implicit);
+	virtual void setupDeformableSolve(bool implicit);
 
 	// set the current velocity to that backed up in m_backupVelocity
 	void revertVelocity();
@@ -199,9 +199,11 @@ public:
 		m_objective->m_projection.setLagrangeMultiplier();
 	}
 
+	virtual void solveConstraints(btScalar timeStep) {}
+
 	// unused functions
 	virtual void optimize(btAlignedObjectArray<btSoftBody*>& softBodies, bool forceUpdate = false) {}
-	virtual void solveConstraints(btScalar dt) {}
+	// virtual void solveConstraints(btScalar dt) {}
 	virtual bool checkInitialized() { return true; }
 	virtual void copyBackToSoftBodies(bool bMove = true) {}
 };

src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp

@@ -302,30 +302,33 @@ void btDeformableMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
 
 void btDeformableMultiBodyDynamicsWorld::solveConstraints(btScalar timeStep)
 {
-	BT_PROFILE("btDeformableMultiBodyDynamicsWorld::solveConstraints");
+	// BT_PROFILE("btDeformableMultiBodyDynamicsWorld::solveConstraints");
-	// save v_{n+1}^* velocity after explicit forces
+	// // save v_{n+1}^* velocity after explicit forces
-	m_deformableBodySolver->backupVelocity();
+	// m_deformableBodySolver->backupVelocity();
 
-	// set up constraints among multibodies and between multibodies and deformable bodies
+	// // set up constraints among multibodies and between multibodies and deformable bodies
-	setupConstraints();
+	// setupConstraints();
 
-	// solve contact constraints
+	// // solve contact constraints
-	solveContactConstraints();
+	// solveContactConstraints();
 
-	// set up the directions in which the velocity does not change in the momentum solve
+	// // set up the directions in which the velocity does not change in the momentum solve
-	if (m_useProjection)
+	// if (m_useProjection)
-		m_deformableBodySolver->setProjection();
+	// 	m_deformableBodySolver->setProjection();
-	else
+	// else
-		m_deformableBodySolver->setLagrangeMultiplier();
+	// 	m_deformableBodySolver->setLagrangeMultiplier();
 
-	// for explicit scheme, m_backupVelocity = v_{n+1}^*
+	// // for explicit scheme, m_backupVelocity = v_{n+1}^*
-	// for implicit scheme, m_backupVelocity = v_n
+	// // for implicit scheme, m_backupVelocity = v_n
-	// Here, set dv = v_{n+1} - v_n for nodes in contact
+	// // Here, set dv = v_{n+1} - v_n for nodes in contact
-	m_deformableBodySolver->setupDeformableSolve(m_implicit);
+	// m_deformableBodySolver->setupDeformableSolve(m_implicit);
 
-	// At this point, dv should be golden for nodes in contact
+	// // At this point, dv should be golden for nodes in contact
-	// proceed to solve deformable momentum equation
+	// // proceed to solve deformable momentum equation
-	m_deformableBodySolver->solveDeformableConstraints(timeStep);
+	// m_deformableBodySolver->solveDeformableConstraints(timeStep);
+	
+	// TODO: need better design
+	m_deformableBodySolver->solveConstraints(timeStep);
 }
 
 void btDeformableMultiBodyDynamicsWorld::setupConstraints()