update reduced variables and full space position every time applied the force

examples/ReducedDeformableDemo/BasicTest.cpp

@@ -32,10 +32,10 @@
 // static btScalar damping_alpha = 0.1;
 // static btScalar damping_beta = 0.01;
 static btScalar damping_alpha = 0.0;
-static btScalar damping_beta = 0.01;
+static btScalar damping_beta = 0.0;
 static btScalar COLLIDING_VELOCITY = 0;
 static int start_mode = 6;
-static int num_modes = 2;
+static int num_modes = 1;
 
 class BasicTest : public CommonDeformableBodyBase
 {
@@ -107,6 +107,7 @@ public:
       // }
       
       float internalTimeStep = 1. / 60.f;
+    //   float internalTimeStep = 1e-4;
       m_dynamicsWorld->stepSimulation(deltaTime, 1, internalTimeStep);
     }
     
@@ -136,6 +137,7 @@ public:
                 for (int p = 0; p < rsb->m_fixedNodes.size(); ++p)
                 {
                     deformableWorld->getDebugDrawer()->drawSphere(rsb->m_nodes[rsb->m_fixedNodes[p]].m_x, 0.2, btVector3(1, 0, 0));
+                    // std::cout << rsb->m_nodes[rsb->m_fixedNodes[p]].m_x[0] << "\t" << rsb->m_nodes[rsb->m_fixedNodes[p]].m_x[1] << "\t" << rsb->m_nodes[rsb->m_fixedNodes[p]].m_x[2] << "\n";
                 }
                 deformableWorld->getDebugDrawer()->drawSphere(btVector3(0, 0, 0), 0.1, btVector3(1, 1, 1));
                 deformableWorld->getDebugDrawer()->drawSphere(btVector3(0, 2, 0), 0.1, btVector3(1, 1, 1));

src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp

@@ -126,6 +126,9 @@ void btReducedSoftBody::setFixedNodes()
   // m_fixedNodes.push_back(1);
   // m_fixedNodes.push_back(2);
   // m_fixedNodes.push_back(3);
+
+  // m_fixedNodes.push_back(15);
+  // m_fixedNodes.push_back(18);
 }
 
 void btReducedSoftBody::internalInitialization()
@@ -426,8 +429,10 @@ void btReducedSoftBody::applyVelocityConstraint(const btVector3& target_vel, int
   btMatrix3x3 K2 = RSARinv + ri_skew * m_interpolateInvInertiaTensorWorld * sum_multiply_A * rotation.transpose();
 
   // get impulse
-  btMatrix3x3 impulse_factor = K1 + K2;
+  btMatrix3x3 impulse_factor = K1 + K2;      //TODO: add back
+  // btMatrix3x3 impulse_factor = K1; 
   btVector3 impulse = impulse_factor.inverse() * (target_vel - m_nodes[n_node].m_v);
+  std::cout << "impulse: " << impulse[0] << '\t' << impulse[1] << '\t' << impulse[2] << '\n';
 
   // apply full space impulse
   applyFullSpaceImpulse(impulse, ri, n_node, dt);
@@ -449,6 +454,16 @@ void btReducedSoftBody::applyFullSpaceImpulse(const btVector3& impulse, const bt
   // update reduced velocity
   updateReducedVelocity(dt); // TODO: add back
 
+  // update reduced dofs
+  updateReducedDofs(dt);
+
+  // update local moment arm
+  updateLocalMomentArm();
+  updateExternalForceProjectMatrix(true);
+
+  // std::cout << "vel: " << m_reducedVelocity[0] << "\t" << m_reducedVelocity[1] << "\n";
+  // std::cout << "dofs:" << m_reducedDofs[0] << "\t" << m_reducedDofs[1] << "\n";
+
   // impulse causes rigid motion
   applyRigidImpulse(impulse, rel_pos);
 }
@@ -457,6 +472,7 @@ void btReducedSoftBody::applyFullSpaceNodalForce(const btVector3& f_ext, int n_n
 {
   // f_local = R^-1 * f_ext
   btVector3 f_local = m_rigidTransformWorld.getBasis().transpose() * f_ext;
+  std::cout << "f_ext: " << f_ext[0] << '\t'  << f_ext[1] << '\t' << f_ext[2] << '\n';
 
   // f_scaled = localInvInertia * (r_k x f_local)
   // btVector3 rk_cross_f_local = m_localMomentArm[n_node].cross(f_local);
@@ -471,18 +487,21 @@ void btReducedSoftBody::applyFullSpaceNodalForce(const btVector3& f_ext, int n_n
   f_ext_r.resize(m_nReduced, 0);
   for (int r = 0; r < m_nReduced; ++r)
   {
+    std::cout << "reduced_f before: " << m_reducedForce[r] << '\n';
     for (int k = 0; k < 3; ++k)
     {
       f_ext_r[r] += (m_projPA[r][3 * n_node + k] + m_projCq[r][3 * n_node + k]) * f_local[k];
+      // f_ext_r[r] += m_modes[r][3 * n_node + k] * f_local[k];
     }
-    std::cout << "projPA: " << m_projPA[r][0] << '\t' << m_projPA[r][1] << '\t' << m_projPA[r][2] << '\n';
-    std::cout << "projCq: " << m_projCq[r][0] << '\t' << m_projCq[r][1] << '\t' << m_projCq[r][2] << '\n';
+    // std::cout << "projPA: " << m_projPA[r][0] << '\t' << m_projPA[r][1] << '\t' << m_projPA[r][2] << '\n';
+    // std::cout << "projCq: " << m_projCq[r][0] << '\t' << m_projCq[r][1] << '\t' << m_projCq[r][2] << '\n';
 
-    std::cout << "f_ext_r: " << r << '\t' << f_ext_r[r] << '\n';
-    std::cout << "reduced_f: " << r << '\t' << m_reducedForce[r] << '\n';
     m_reducedForce[r] += f_ext_r[r];
+    std::cout << "f_ext_r: " << f_ext_r[r] << '\n';
+    std::cout << "reduced_f after: " << m_reducedForce[r] << '\n';
   }
-  // std::cout << "reduced_f: " << m_reducedForce[0] << '\t' << m_reducedForce[0] << '\n';
+  // std::cout << "reduced_f: " << m_reducedForce[0] << '\n';
+  std::cout << "gravity: " << m_mass * 10 << '\n';
 }
 
 void btReducedSoftBody::applyRigidGravity(const btVector3& gravity, btScalar dt)
@@ -501,7 +520,7 @@ void btReducedSoftBody::applyReducedInternalForce(const btScalar damping_alpha,
 
 void btReducedSoftBody::applyFixedContraints(btScalar dt)
 {
-  for (int iter = 0; iter < 1; ++iter)
+  for (int iter = 0; iter < 50; ++iter)
   {
     // btVector3 vel_error(0, 0, 0);
     for (int n = 0; n < m_fixedNodes.size(); ++n)

src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h

@@ -16,7 +16,7 @@ class btReducedSoftBody : public btSoftBody
   //  Typedefs
   //
   typedef btAlignedObjectArray<btVector3> TVStack;
-  typedef btAlignedObjectArray<btMatrix3x3> tBlockDiagMatrix;
+  // typedef btAlignedObjectArray<btMatrix3x3> tBlockDiagMatrix;
   typedef btAlignedObjectArray<btScalar> tDenseArray;
   typedef btAlignedObjectArray<btAlignedObjectArray<btScalar> > tDenseMatrix;
 

src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp

@@ -40,12 +40,19 @@ void btReducedSoftBodySolver::predictReduceDeformableMotion(btScalar solverdt)
     // rigid motion
     rsb->predictIntegratedTransform(solverdt, rsb->getInterpolationWorldTransform());
 
-    std::cout << "reduced_dofs: " << rsb->m_reducedDofs[0] << '\t' << rsb->m_reducedDofs[1] << '\n';
-    std::cout << "reduced_vels: " << rsb->m_reducedVelocity[0] << '\t' << rsb->m_reducedVelocity[1] << '\n';
+    // std::cout << "reduced_dofs: " << rsb->m_reducedDofs[0] << '\t' << rsb->m_reducedDofs[1] << '\n';
+    // std::cout << "reduced_vels: " << rsb->m_reducedVelocity[0] << '\t' << rsb->m_reducedVelocity[1] << '\n';
 
     // update reduced velocity and dofs
     rsb->updateReducedVelocity(solverdt); // TODO: add back
 
+    // update reduced dofs
+    rsb->updateReducedDofs(solverdt);
+
+    // update local moment arm
+    rsb->updateLocalMomentArm();
+    rsb->updateExternalForceProjectMatrix(true);
+
     // predict full space velocity (needed for constraints)
     rsb->mapToFullVelocity(rsb->getInterpolationWorldTransform());
 
@@ -86,7 +93,7 @@ void btReducedSoftBodySolver::applyTransforms(btScalar timeStep)
     btReducedSoftBody* rsb = static_cast<btReducedSoftBody*>(m_softBodies[i]);
 
     // update reduced dofs for the next time step
-    rsb->updateReducedDofs(timeStep); // TODO: add back
+    // rsb->updateReducedDofs(timeStep); // TODO: add back
 
     // rigid motion
     // btTransform predictedTrans;
@@ -97,7 +104,8 @@ void btReducedSoftBodySolver::applyTransforms(btScalar timeStep)
     rsb->mapToFullDofs(rsb->getRigidTransform());
 
     // end of time step clean up and update
-    rsb->updateExternalForceProjectMatrix(true);
+    // rsb->updateLocalMomentArm();
+    // rsb->updateExternalForceProjectMatrix(true);
     rsb->endOfTimeStepZeroing();
   }
   m_simTime += timeStep;