much better convergence after separating impulse in different directions

examples/ReducedDeformableDemo/Springboard.cpp

@@ -27,7 +27,7 @@
 #include "../Utils/b3ResourcePath.h"
 
 static btScalar damping_alpha = 0.0;
-static btScalar damping_beta = 0.0;
+static btScalar damping_beta = 0.0001;
 static int start_mode = 6;
 static int num_modes = 20;
 
@@ -79,6 +79,14 @@ public:
     {      
       float internalTimeStep = 1. / 60.f;
       m_dynamicsWorld->stepSimulation(deltaTime, 1, internalTimeStep);
+
+      {
+        sim_time += internalTimeStep;
+        btReducedSoftBody* rsb = static_cast<btReducedSoftBody*>(getDeformableDynamicsWorld()->getSoftBodyArray()[0]);
+        std::ofstream myfile("fixed_node.txt", std::ios_base::app);
+        myfile << sim_time << "\t" << rsb->m_nodes[0].m_x[0] << "\t" << rsb->m_nodes[0].m_x[1] << "\t" << rsb->m_nodes[0].m_x[2] << "\n";
+        myfile.close();
+      }
     }
     
     virtual void renderScene()
@@ -173,7 +181,7 @@ void Springboard::initPhysics()
     getDeformableDynamicsWorld()->getSolverInfo().m_splitImpulse = false;
     getDeformableDynamicsWorld()->getSolverInfo().m_numIterations = 100;
     // add a few rigid bodies
-    // Ctor_RbUpStack();
+    Ctor_RbUpStack();
 
     // create a static rigid box as the ground
     // {

src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.cpp

@@ -7,35 +7,38 @@ btReducedDeformableStaticConstraint::btReducedDeformableStaticConstraint(
   btSoftBody::Node* node,
 	const btVector3& ri,
 	const btVector3& x0,
+	const btVector3& dir,
   const btContactSolverInfo& infoGlobal,
 	btScalar dt)
-  : m_rsb(rsb), m_ri(ri), m_x0(x0), m_dt(dt), btDeformableStaticConstraint(node, infoGlobal)
+  : m_rsb(rsb), m_ri(ri), m_targetPos(x0), m_impulseDirection(dir), m_dt(dt), btDeformableStaticConstraint(node, infoGlobal)
 {
-	// get impulse
-  m_impulseFactorInv = rsb->getImpulseFactor(m_node->index).inverse();
+	m_erp = 0.2;
+	m_appliedImpulse = 0;
 
-	btVector3 vel_error = -m_node->m_v;
-	btVector3 pos_error = m_x0 - m_node->m_x;
-	std::cout << "pos_errors: " << pos_error[0] << "\t" << pos_error[1] << "\t" << pos_error[2] << "\n";
+	// get impulse factor
+  m_impulseFactorMatrix = rsb->getImpulseFactor(m_node->index);
+	m_impulseFactor = (m_impulseFactorMatrix * m_impulseDirection).dot(m_impulseDirection);
 
-	m_appliedImpulse = btVector3(0, 0, 0);
-	m_rhs = m_impulseFactorInv * (vel_error + 0.2 * pos_error / m_dt);
+	btScalar vel_error = btDot(-m_node->m_v, m_impulseDirection);
+	btScalar pos_error = btDot(m_targetPos - m_node->m_x, m_impulseDirection);
+
+	m_rhs = (vel_error + m_erp * pos_error / m_dt) / m_impulseFactor;
 }
 
 btScalar btReducedDeformableStaticConstraint::solveConstraint(const btContactSolverInfo& infoGlobal)
 {
 	// target velocity of fixed constraint is 0
 	btVector3 deltaVa = getDeltaVa();
-	btVector3 rel_vel = deltaVa;
-  btVector3 deltaImpulse = m_rhs - m_impulseFactorInv * rel_vel;
+	btScalar deltaV_rel = btDot(deltaVa, m_impulseDirection);
+  btScalar deltaImpulse = m_rhs - deltaV_rel / m_impulseFactor;
 	m_appliedImpulse = m_appliedImpulse + deltaImpulse;
 
-	applyImpulse(deltaImpulse);
-
-	btVector3 deltaV = m_rsb->getImpulseFactor(m_node->index) * deltaImpulse;
+	btVector3 impulse = deltaImpulse * m_impulseDirection;
+	applyImpulse(impulse);
 
 	// calculate residual
-	btScalar residualSquare = btDot(deltaV, deltaV);
+	btScalar residualSquare = m_impulseFactor * deltaImpulse;
+	residualSquare *= residualSquare;
 	std::cout << "residualSquare: " << residualSquare << "\n";
 
 	return residualSquare;

src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.h

@@ -7,16 +7,20 @@ class btReducedDeformableStaticConstraint : public btDeformableStaticConstraint
  public:
   btReducedSoftBody* m_rsb;
   btScalar m_dt;
-  btMatrix3x3 m_impulseFactorInv;
   btVector3 m_ri;
-  btVector3 m_x0;
-  btVector3 m_rhs;
-  btVector3 m_appliedImpulse;
+  btVector3 m_targetPos;
+  btVector3 m_impulseDirection;
+  btMatrix3x3 m_impulseFactorMatrix;
+  btScalar m_impulseFactor;
+  btScalar m_rhs;
+  btScalar m_appliedImpulse;
+  btScalar m_erp;
 
   btReducedDeformableStaticConstraint(btReducedSoftBody* rsb, 
                                       btSoftBody::Node* node,
                                       const btVector3& ri,
                                       const btVector3& x0,
+                                      const btVector3& dir,
                                       const btContactSolverInfo& infoGlobal,
                                       btScalar dt);
 	// btReducedDeformableStaticConstraint(const btReducedDeformableStaticConstraint& other);

src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp

@@ -201,8 +201,13 @@ void btReducedSoftBodySolver::setConstraints(const btContactSolverInfo& infoGlob
       int i_node = rsb->m_fixedNodes[j];
 			if (rsb->m_nodes[i_node].m_im == 0)
 			{
-				btReducedDeformableStaticConstraint static_constraint(rsb, &rsb->m_nodes[i_node], rsb->getRelativePos(i_node), rsb->m_x0[i_node], infoGlobal, m_dt);
-				m_staticConstraints[i].push_back(static_constraint);
+        for (int k = 0; k < 3; ++k)
+        {
+          btVector3 dir(0, 0, 0);
+          dir[k] = 1;
+          btReducedDeformableStaticConstraint static_constraint(rsb, &rsb->m_nodes[i_node], rsb->getRelativePos(i_node), rsb->m_x0[i_node], dir, infoGlobal, m_dt);
+          m_staticConstraints[i].push_back(static_constraint);
+        }
 			}
 		}
     btAssert(rsb->m_fixedNodes.size() == m_staticConstraints[i].size());