diff --git a/examples/ReducedDeformableDemo/BasicTest.cpp b/examples/ReducedDeformableDemo/BasicTest.cpp
index 1b925d356..df3b0b549 100644
--- a/examples/ReducedDeformableDemo/BasicTest.cpp
+++ b/examples/ReducedDeformableDemo/BasicTest.cpp
@@ -30,7 +30,7 @@
 // static btScalar E = 50;
 // static btScalar nu = 0.3;
 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 = 1;
@@ -48,7 +48,8 @@ class BasicTest : public CommonDeformableBodyBase
       //     rsb->m_nodes[i].m_x[k] += rsb->m_modes[mode_n][3 * i + k] * scale;
 
       rsb->m_reducedDofs[mode_n] = scale;
-      rsb->mapToFullPosition(rsb->getWorldTransform());
+      rsb->m_reducedDofsBuffer[mode_n] = scale;
+      rsb->mapToFullPosition(rsb->getRigidTransform());
       std::cout << "-----------\n";
       std::cout << rsb->m_nodes[0].m_x[0] << '\t' << rsb->m_nodes[0].m_x[1] << '\t' << rsb->m_nodes[0].m_x[2] << '\n';
       std::cout << "-----------\n";
@@ -96,13 +97,13 @@ public:
     void stepSimulation(float deltaTime)
     {
       // 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, 1);
-      //   first_step = false;
-      //   // rsb->mapToReducedDofs();
-      // }
+    //   btReducedSoftBody* rsb = static_cast<btReducedSoftBody*>(static_cast<btDeformableMultiBodyDynamicsWorld*>(m_dynamicsWorld)->getSoftBodyArray()[0]);
+    //   if (first_step /* && !rsb->m_bUpdateRtCst*/) 
+    //   {
+    //     getDeformedShape(rsb, 0, 1);
+    //     first_step = false;
+    //     // rsb->mapToReducedDofs();
+    //   }
       
       float internalTimeStep = 1. / 60.f;
     //   float internalTimeStep = 1e-3;
@@ -182,7 +183,7 @@ void BasicTest::initPhysics()
         // rsb->scale(btVector3(1, 1, 1));  //TODO: add back scale
         rsb->translate(btVector3(0, 4, 0));
         // rsb->setTotalMass(0.5);
-        rsb->setStiffnessScale(100);
+        rsb->setStiffnessScale(20);
         rsb->setDamping(damping_alpha, damping_beta);
         
         // set fixed nodes
@@ -212,7 +213,7 @@ void BasicTest::initPhysics()
     getDeformableDynamicsWorld()->setUseProjection(true);
     getDeformableDynamicsWorld()->getSolverInfo().m_deformable_erp = 0.3;
     getDeformableDynamicsWorld()->getSolverInfo().m_deformable_maxErrorReduction = btScalar(200);
-    getDeformableDynamicsWorld()->getSolverInfo().m_leastSquaresResidualThreshold = 1e-6;
+    getDeformableDynamicsWorld()->getSolverInfo().m_leastSquaresResidualThreshold = 0;
     getDeformableDynamicsWorld()->getSolverInfo().m_splitImpulse = true;
     getDeformableDynamicsWorld()->getSolverInfo().m_numIterations = 100;
     // add a few rigid bodies
diff --git a/examples/ReducedDeformableDemo/FreeFall.cpp b/examples/ReducedDeformableDemo/FreeFall.cpp
index 2ddea4e98..91667fc77 100644
--- a/examples/ReducedDeformableDemo/FreeFall.cpp
+++ b/examples/ReducedDeformableDemo/FreeFall.cpp
@@ -33,7 +33,7 @@ static btScalar damping_alpha = 0.0;
 static btScalar damping_beta = 0.01;
 static btScalar COLLIDING_VELOCITY = 0;
 static int start_mode = 6;
-static int num_modes = 4;
+static int num_modes = 1;
 
 class FreeFall : public CommonDeformableBodyBase
 {
@@ -183,12 +183,12 @@ void FreeFall::initPhysics()
     }
     // create a static rigid box as the ground
     {
-        btBoxShape* groundShape = createBoxShape(btVector3(btScalar(3.), btScalar(3.), btScalar(0.1)));
+        btBoxShape* groundShape = createBoxShape(btVector3(btScalar(3.), btScalar(3.), btScalar(3.)));
         m_collisionShapes.push_back(groundShape);
 
         btTransform groundTransform;
         groundTransform.setIdentity();
-        groundTransform.setOrigin(btVector3(0, 0, 5));
+        groundTransform.setOrigin(btVector3(0, 0, 6));
         {
             btScalar mass(0.);
             createRigidBody(mass, groundTransform, groundShape, btVector4(0,0,0,0));
diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.cpp b/src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.cpp
index 17d49a63a..2357f69a3 100644
--- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.cpp
+++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.cpp
@@ -21,7 +21,7 @@ btScalar btReducedDeformableStaticConstraint::solveConstraint(const btContactSol
   
   // apply full space impulse
 	std::cout << "node: " << m_node->index << " impulse: " << impulse[0] << '\t' << impulse[1] << '\t' << impulse[2] << '\n';
-	std::cout << "impulse norm: " << impulse.norm() << "\n";
+	// std::cout << "impulse norm: " << impulse.norm() << "\n";
 
 	m_rsb->applyFullSpaceImpulse(impulse, m_ri, m_node->index, m_dt);
 
@@ -56,97 +56,113 @@ btScalar btReducedDeformableRigidContactConstraint::solveConstraint(const btCont
 	btVector3 va = getVa();
 	btVector3 vb = getVb();
 	btVector3 vr = vb - va;
-	btScalar dn = btDot(vr, cti.m_normal) + m_total_normal_dv.dot(cti.m_normal) * infoGlobal.m_deformable_cfm;
-	if (m_penetration > 0)
-	{
-		dn += m_penetration / infoGlobal.m_timeStep;
-	}
-	if (!infoGlobal.m_splitImpulse)
-	{
-		dn += m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep;
-	}
+	btScalar dn = btDot(vr, cti.m_normal);
+	// btScalar dn = btDot(vr, cti.m_normal) + m_total_normal_dv.dot(cti.m_normal) * infoGlobal.m_deformable_cfm;
+	// if (m_penetration > 0)
+	// {
+	// 	dn += m_penetration / infoGlobal.m_timeStep;
+	// }
+	// if (!infoGlobal.m_splitImpulse)
+	// {
+	// 	dn += m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep;
+	// }
 	// dn is the normal component of velocity diffrerence. Approximates the residual.
-	btVector3 impulse = m_contact->m_c0 * (vr + m_total_normal_dv * infoGlobal.m_deformable_cfm + ((m_penetration > 0) ? m_penetration / infoGlobal.m_timeStep * cti.m_normal : btVector3(0, 0, 0)));
-	if (!infoGlobal.m_splitImpulse)
-	{
-		impulse += m_contact->m_c0 * (m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep * cti.m_normal);
-	}
-	btVector3 impulse_normal = m_contact->m_c0 * (cti.m_normal * dn);
-	btVector3 impulse_tangent = impulse - impulse_normal;
-	if (dn > 0)
-	{
-		return 0;
-	}
-	m_binding = true;
-	btScalar residualSquare = dn * dn;
-	btVector3 old_total_tangent_dv = m_total_tangent_dv;
-	// m_c5 is the inverse mass of the deformable node/face
-	m_total_normal_dv -= m_contact->m_c5 * impulse_normal;
-	m_total_tangent_dv -= m_contact->m_c5 * impulse_tangent;
+	btVector3 vr_tangent = vr - dn * cti.m_normal;
+	btVector3 delta_vr = vr_tangent - vr;
 
-	if (m_total_normal_dv.dot(cti.m_normal) < 0)
-	{
-		// separating in the normal direction
-		m_binding = false;
-		m_static = false;
-		impulse_tangent.setZero();
-	}
-	else
-	{
-		if (m_total_normal_dv.norm() * m_contact->m_c3 < m_total_tangent_dv.norm())
-		{
-			// dynamic friction
-			// with dynamic friction, the impulse are still applied to the two objects colliding, however, it does not pose a constraint in the cg solve, hence the change to dv merely serves to update velocity in the contact iterations.
-			m_static = false;
-			if (m_total_tangent_dv.safeNorm() < SIMD_EPSILON)
-			{
-				m_total_tangent_dv = btVector3(0, 0, 0);
-			}
-			else
-			{
-				m_total_tangent_dv = m_total_tangent_dv.normalized() * m_total_normal_dv.safeNorm() * m_contact->m_c3;
-			}
-			//            impulse_tangent = -btScalar(1)/m_contact->m_c2 * (m_total_tangent_dv - old_total_tangent_dv);
-			impulse_tangent = m_contact->m_c5.inverse() * (old_total_tangent_dv - m_total_tangent_dv);
-		}
-		else
-		{
-			// static friction
-			m_static = true;
-		}
-	}
-	impulse = impulse_normal + impulse_tangent;
+
+	btScalar residualSquare = dn * dn;
+	btVector3 impulse = (m_contact->m_c0 * delta_vr);
+	// btVector3 impulse_normal = m_contact->m_c0 * (cti.m_normal * dn);
+	// btVector3 impulse_tangent = impulse - impulse_normal;
+	std::cout << "impulse normal: " << impulse[0] << '\t' << impulse[1] << '\t' << impulse[2] << '\n';
+	if (dn > 0)
+		return 0;
+	// btVector3 impulse = m_contact->m_c0 * (vr + m_total_normal_dv * infoGlobal.m_deformable_cfm + ((m_penetration > 0) ? m_penetration / infoGlobal.m_timeStep * cti.m_normal : btVector3(0, 0, 0)));
+	// if (!infoGlobal.m_splitImpulse)
+	// {
+	// 	impulse += m_contact->m_c0 * (m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep * cti.m_normal);
+	// }
+	// btVector3 impulse_normal = m_contact->m_c0 * (cti.m_normal * dn);
+	// btVector3 impulse_tangent = impulse - impulse_normal;
+	// if (dn > 0)
+	// {
+	// 	return 0;
+	// }
+	// m_binding = true;
+	// btScalar residualSquare = dn * dn;
+	// btVector3 old_total_tangent_dv = m_total_tangent_dv;
+	// // m_c5 is the inverse mass of the deformable node/face
+	// m_total_normal_dv -= m_contact->m_c5 * impulse_normal;
+	// m_total_tangent_dv -= m_contact->m_c5 * impulse_tangent;
+
+	// if (m_total_normal_dv.dot(cti.m_normal) < 0)
+	// {
+	// 	// separating in the normal direction
+	// 	m_binding = false;
+	// 	m_static = false;
+	// 	impulse_tangent.setZero();
+	// }
+	// else
+	// {
+	// 	if (m_total_normal_dv.norm() * m_contact->m_c3 < m_total_tangent_dv.norm())
+	// 	{
+	// 		// dynamic friction
+	// 		// with dynamic friction, the impulse are still applied to the two objects colliding, however, it does not pose a constraint in the cg solve, hence the change to dv merely serves to update velocity in the contact iterations.
+	// 		m_static = false;
+	// 		if (m_total_tangent_dv.safeNorm() < SIMD_EPSILON)
+	// 		{
+	// 			m_total_tangent_dv = btVector3(0, 0, 0);
+	// 		}
+	// 		else
+	// 		{
+	// 			m_total_tangent_dv = m_total_tangent_dv.normalized() * m_total_normal_dv.safeNorm() * m_contact->m_c3;
+	// 		}
+	// 		//            impulse_tangent = -btScalar(1)/m_contact->m_c2 * (m_total_tangent_dv - old_total_tangent_dv);
+	// 		impulse_tangent = m_contact->m_c5.inverse() * (old_total_tangent_dv - m_total_tangent_dv);
+	// 	}
+	// 	else
+	// 	{
+	// 		// static friction
+	// 		m_static = true;
+	// 	}
+	// }
+	// impulse = impulse_normal + impulse_tangent;
 	// apply impulse to deformable nodes involved and change their velocities
-	applyImpulse(impulse); // TODO: apply impulse?
+	// impulse = btVector3(0, 0, -1e-2);
+	// impulse_normal = btVector3(0, 0, 0);
+	// impulse_tangent = btVector3(0, 0, 0);
+	applyImpulse(impulse);
+	// applyImpulse(impulse); // TODO: apply impulse?
 	// apply impulse to the rigid/multibodies involved and change their velocities
-	if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
-	{
-		btRigidBody* rigidCol = 0;
-		rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
-		if (rigidCol)
-		{
-			rigidCol->applyImpulse(impulse, m_contact->m_c1);
-		}
-	}
-	else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
-	{
-		btMultiBodyLinkCollider* multibodyLinkCol = 0;
-		multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
-		if (multibodyLinkCol)
-		{
-			const btScalar* deltaV_normal = &m_contact->jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
-			// apply normal component of the impulse
-			multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_normal, impulse.dot(cti.m_normal));
-			if (impulse_tangent.norm() > SIMD_EPSILON)
-			{
-				// apply tangential component of the impulse
-				const btScalar* deltaV_t1 = &m_contact->jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
-				multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t1, impulse.dot(m_contact->t1));
-				const btScalar* deltaV_t2 = &m_contact->jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
-				multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t2, impulse.dot(m_contact->t2));
-			}
-		}
-	}
+	// if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
+	// {
+	// 	btRigidBody* rigidCol = 0;
+	// 	rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
+	// 	if (rigidCol)
+	// 	{
+	// 		rigidCol->applyImpulse(impulse, m_contact->m_c1);
+	// 	}
+	// }
+	// else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
+	// {
+	// 	btMultiBodyLinkCollider* multibodyLinkCol = 0;
+	// 	multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
+	// 	if (multibodyLinkCol)
+	// 	{
+	// 		const btScalar* deltaV_normal = &m_contact->jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
+	// 		// apply normal component of the impulse
+	// 		multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_normal, impulse.dot(cti.m_normal));
+	// 		if (impulse_tangent.norm() > SIMD_EPSILON)
+	// 		{
+	// 			// apply tangential component of the impulse
+	// 			const btScalar* deltaV_t1 = &m_contact->jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
+	// 			multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t1, impulse.dot(m_contact->t1));
+	// 			const btScalar* deltaV_t2 = &m_contact->jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
+	// 			multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t2, impulse.dot(m_contact->t2));
+	// 		}
+	// 	}
+	// }
 	return residualSquare;
 }
 
@@ -176,7 +192,10 @@ btVector3 btReducedDeformableNodeRigidContactConstraint::getDv(const btSoftBody:
 
 void btReducedDeformableNodeRigidContactConstraint::applyImpulse(const btVector3& impulse)
 {
+	std::cout << "impulse applied: " << impulse[0] << '\t' << impulse[1] << '\t' << impulse[2] << '\n';
   m_rsb->applyFullSpaceImpulse(impulse, m_contact->m_c1, m_node->index, m_dt);
+	m_rsb->mapToFullVelocity(m_rsb->getInterpolationWorldTransform());
+	std::cout << "node: " << m_node->index << " vel: " << m_node->m_v[0] << '\t' << m_node->m_v[1] << '\t' << m_node->m_v[2] << '\n';
 }
 
 // ================= face vs rigid constraints ===================
diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp
index d6718c77c..f3e6cd4ff 100644
--- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp
+++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp
@@ -278,11 +278,12 @@ void btReducedSoftBody::updateReducedVelocity(btScalar solverdt, bool explicit_f
       delta_v = solverdt * mass_inv * (m_reducedForceDamping[r] + m_reducedForceExternal[r]);
     }
     // delta_v = solverdt * mass_inv * (m_reducedForceElastic[r] + m_reducedForceDamping[r] + m_reducedForceExternal[r]);
-    std::cout << "delta_v: " << delta_v << '\n';
+    // std::cout << "delta_v: " << delta_v << '\n';
     // m_reducedVelocity[r] = m_reducedVelocityBuffer[r] + delta_v;
     m_reducedVelocity[r] += delta_v;
   }
-  std::cout << "force: " << m_reducedForceElastic[0] << '\t' << m_reducedForceExternal[0] << '\n';
+  // std::cout << "reduce_vel: " << m_reducedVelocity[0] << '\n';
+  // std::cout << "force: " << m_reducedForceElastic[0] << '\t' << m_reducedForceDamping[0] << '\t' << m_reducedForceExternal[0] << '\n';
 }
 
 void btReducedSoftBody::mapToFullVelocity(const btTransform& ref_trans)
@@ -448,6 +449,7 @@ btMatrix3x3 btReducedSoftBody::getImpulseFactor(int n_node)
 
   btMatrix3x3 K2 = RSARinv + ri_skew * m_interpolateInvInertiaTensorWorld * sum_multiply_A * rotation.transpose();
 
+  // return K1;  //TODO: change back
   return K1 + K2;
 }
 
diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp
index efc05f8e5..f88e68a7c 100644
--- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp
+++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp
@@ -95,11 +95,11 @@ void btReducedSoftBodySolver::predictReduceDeformableMotion(btScalar solverdt)
     rsb->predictIntegratedTransform(solverdt, rsb->getInterpolationWorldTransform());
 
     // update reduced dofs at time^*
-    rsb->updateReducedDofs(solverdt);
+    // rsb->updateReducedDofs(solverdt);
 
     // update local moment arm at time^*
-    rsb->updateLocalMomentArm();
-    rsb->updateExternalForceProjectMatrix(true);
+    // rsb->updateLocalMomentArm();
+    // rsb->updateExternalForceProjectMatrix(true);
 
     // predict full space velocity at time^* (needed for constraints)
     rsb->mapToFullVelocity(rsb->getInterpolationWorldTransform());
@@ -131,7 +131,7 @@ void btReducedSoftBodySolver::applyExplicitForce(btScalar solverdt)
 
     // add internal force (elastic force & damping force)
     rsb->applyReducedElasticForce(rsb->m_reducedDofsBuffer);
-    // rsb->applyReducedDampingForce(rsb->m_reducedVelocityBuffer);
+    rsb->applyReducedDampingForce(rsb->m_reducedVelocityBuffer);
 
     // get reduced velocity at time^* 
     rsb->updateReducedVelocity(solverdt, true);
@@ -167,7 +167,9 @@ void btReducedSoftBodySolver::applyTransforms(btScalar timeStep)
     rsb->endOfTimeStepZeroing();
   }
 
-  // exit(100);
+  // static int count = 0;
+  // if (count > 0) exit(100);
+  // count++;
 }
 
 void btReducedSoftBodySolver::setConstraints(const btContactSolverInfo& infoGlobal)
@@ -192,22 +194,22 @@ void btReducedSoftBodySolver::setConstraints(const btContactSolverInfo& infoGlob
 		}
     btAssert(rsb->m_fixedNodes.size() == m_staticConstraints[i].size());
 
-    // set Deformable Node vs. Rigid constraint //TODO: add back contact
-		// for (int j = 0; j < rsb->m_nodeRigidContacts.size(); ++j)
-		// {
-		// 	const btSoftBody::DeformableNodeRigidContact& contact = rsb->m_nodeRigidContacts[j];
-		// 	// skip fixed points
-		// 	if (contact.m_node->m_im == 0)
-		// 	{
-		// 		continue;
-		// 	}
-		// 	btReducedDeformableNodeRigidContactConstraint constraint(rsb, contact, infoGlobal, m_dt);
-		// 	m_nodeRigidConstraints[i].push_back(constraint);
-    //   rsb->m_contactNodesList.push_back(contact.m_node->index);
-		// }
-    // std::cout << "#contact nodes: " << m_nodeRigidConstraints[i].size() << "\n";
+    // set Deformable Node vs. Rigid constraint
+		for (int j = 0; j < rsb->m_nodeRigidContacts.size(); ++j)
+		{
+			const btSoftBody::DeformableNodeRigidContact& contact = rsb->m_nodeRigidContacts[j];
+			// skip fixed points
+			if (contact.m_node->m_im == 0)
+			{
+				continue;
+			}
+			btReducedDeformableNodeRigidContactConstraint constraint(rsb, contact, infoGlobal, m_dt);
+			m_nodeRigidConstraints[i].push_back(constraint);
+      rsb->m_contactNodesList.push_back(contact.m_node->index);
+		}
+    std::cout << "#contact nodes: " << m_nodeRigidConstraints[i].size() << "\n";
 
-    // set Deformable Face vs. Rigid constraint
+    // // set Deformable Face vs. Rigid constraint
 		// for (int j = 0; j < rsb->m_faceRigidContacts.size(); ++j)
 		// {
 		// 	const btSoftBody::DeformableFaceRigidContact& contact = rsb->m_faceRigidContacts[j];
@@ -230,39 +232,56 @@ btScalar btReducedSoftBodySolver::solveContactConstraints(btCollisionObject** de
   // handle fixed constraint
   for (int i = 0; i < m_softBodies.size(); ++i)
   {
+    btReducedSoftBody* rsb = static_cast<btReducedSoftBody*>(m_softBodies[i]);
+
+    btAlignedObjectArray<btScalar> residual;
+    residual.resize(m_staticConstraints[i].size(), 0);
+
     for (int k = 0; k < m_staticConstraints[i].size(); ++k)
     {
       btReducedDeformableStaticConstraint& constraint = m_staticConstraints[i][k];
       btScalar localResidualSquare = constraint.solveConstraint(infoGlobal);
       residualSquare = btMax(residualSquare, localResidualSquare);
+
+      btVector3 error;
+      error.setZero();
+      std::cout << "fixed_nodes: ";
+      for (int p = 0; p < rsb->m_fixedNodes.size(); ++p)
+      {
+        std::cout << rsb->m_nodes[rsb->m_fixedNodes[p]].m_v.norm() << '\t';
+        error += rsb->m_nodes[rsb->m_fixedNodes[p]].m_v;
+      }
+      std::cout << '\n';
+      std::cout << "norm: " << error.norm() << "\n";
     }
+
   }
 
   // handle contact constraint
-	// for (int i = 0; i < numDeformableBodies; ++i)
-	// {
-	// 	for (int j = 0; j < m_softBodies.size(); ++j)
-	// 	{
-	// 		btReducedSoftBody* rsb = static_cast<btReducedSoftBody*>(m_softBodies[i]);
-	// 		if (rsb != deformableBodies[i])
-	// 		{
-	// 			continue;
-	// 		}
+	for (int i = 0; i < numDeformableBodies; ++i)
+	{
+		for (int j = 0; j < m_softBodies.size(); ++j)
+		{
+			btReducedSoftBody* rsb = static_cast<btReducedSoftBody*>(m_softBodies[i]);
+			if (rsb != deformableBodies[i])
+			{
+				continue;
+			}
 
-  //     // node vs rigid contact
-	// 		for (int k = 0; k < m_nodeRigidConstraints[j].size(); ++k)
-	// 		{
-	// 			btReducedDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[j][k];
-	// 			btScalar localResidualSquare = constraint.solveConstraint(infoGlobal);
-	// 			residualSquare = btMax(residualSquare, localResidualSquare);
-	// 		}
-	// 		// for (int k = 0; k < m_faceRigidConstraints[j].size(); ++k)
-	// 		// {
-	// 		// 	btReducedDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[j][k];
-	// 		// 	btScalar localResidualSquare = constraint.solveConstraint(infoGlobal);
-	// 		// 	residualSquare = btMax(residualSquare, localResidualSquare);
-	// 		// }
-	// 	}
-	// }
+      // node vs rigid contact
+			for (int k = 0; k < m_nodeRigidConstraints[j].size(); ++k)
+			{
+				btReducedDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[j][k];
+				btScalar localResidualSquare = constraint.solveConstraint(infoGlobal);
+				residualSquare = btMax(residualSquare, localResidualSquare);
+			}
+			// for (int k = 0; k < m_faceRigidConstraints[j].size(); ++k)
+			// {
+			// 	btReducedDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[j][k];
+			// 	btScalar localResidualSquare = constraint.solveConstraint(infoGlobal);
+			// 	residualSquare = btMax(residualSquare, localResidualSquare);
+			// }
+		}
+	}
 	return residualSquare;
 }
\ No newline at end of file