collision with moving rigid body seems to be working. checking why there is side motion

examples/ReducedDeformableDemo/ReducedCollide.cpp

@@ -56,22 +56,22 @@ public:
 
     void resetCamera()
     {
-        float dist = 20;
-        float pitch = -30;
-        float yaw = 125;
-        float targetPos[3] = {-2, 0, 2};
+        float dist = 15;
+        float pitch = -10;
+        float yaw = 90;
+        float targetPos[3] = {0, 3, 0};
         m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
     }
     
     void Ctor_RbUpStack()
     {
-        float mass = 0.5;
+        float mass = 8;
         btCollisionShape* shape = new btBoxShape(btVector3(2, 2, 2));
         btTransform startTransform;
         startTransform.setIdentity();
         startTransform.setOrigin(btVector3(0,-2,0));
         btRigidBody* rb = createRigidBody(mass, startTransform, shape);
-        rb->setLinearVelocity(btVector3(0,+COLLIDING_VELOCITY, 0));
+        rb->setLinearVelocity(btVector3(0, +COLLIDING_VELOCITY, 0));
     }
     
     void stepSimulation(float deltaTime)
@@ -146,7 +146,7 @@ void ReducedCollide::initPhysics()
         getDeformableDynamicsWorld()->addSoftBody(rsb);
         rsb->getCollisionShape()->setMargin(0.1);
         // rsb->scale(btVector3(1, 1, 1));
-        rsb->translate(btVector3(0, 10, 0));  //TODO: add back translate and scale
+        rsb->translate(btVector3(0, 2, 0));  //TODO: add back translate and scale
         rsb->setStiffnessScale(10);
         rsb->setDamping(damping_alpha, damping_beta);
 

src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.cpp

@@ -40,7 +40,6 @@ void btReducedDeformableStaticConstraint::applyImpulse(const btVector3& impulse)
 	m_rsb->applyFullSpaceImpulse(impulse, m_ri, m_node->index, m_dt);
 }
 
-
 // ================= base contact constraints ===================
 btReducedDeformableRigidContactConstraint::btReducedDeformableRigidContactConstraint(
   btReducedSoftBody* rsb, 
@@ -61,12 +60,43 @@ btReducedDeformableRigidContactConstraint::btReducedDeformableRigidContactConstr
   m_impulseFactor = c.m_c0;
 	m_normalImpulseFactor = (m_impulseFactor * m_contactNormalA).dot(m_contactNormalA);
 	m_tangentImpulseFactor = 0;
+
+	m_relPosA = c.m_c1;
+
+	btRigidBody* rb = m_contact->m_cti.m_colObj ? (btRigidBody*)btRigidBody::upcast(m_contact->m_cti.m_colObj) : nullptr;
+	if (!rb)
+	{
+		btAssert(false);
+	}
+	else
+	{
+		m_collideStatic = rb->isStaticObject();
+	}
+}
+
+void btReducedDeformableRigidContactConstraint::setSolverBody(btSolverBody& solver_body)
+{
+	m_solverBody = &solver_body;
+	m_linearComponent = m_contactNormalA * m_solverBody->internalGetInvMass();
+	btVector3	torqueAxis = m_relPosA.cross(m_contactNormalA);
+	m_angularComponent = m_solverBody->m_originalBody->getInvInertiaTensorWorld() * torqueAxis;
+}
+
+btVector3 btReducedDeformableRigidContactConstraint::getVa() const
+{
+	btVector3 Va(0, 0, 0);
+	if (!m_collideStatic)
+	{
+		Va = btDeformableRigidContactConstraint::getVa();
+	}
+	return Va;
 }
 
 btScalar btReducedDeformableRigidContactConstraint::solveConstraint(const btContactSolverInfo& infoGlobal)
 {
 	btVector3 Va = getVa();
-	btVector3 deltaVa = Va - m_bufferVelocityA;
+	// btVector3 deltaVa = Va - m_bufferVelocityA;
+	btVector3 deltaVa = getDeltaVa();
 	btVector3 deltaVb = getDeltaVb();
 	std::cout << "deltaVa: " << deltaVa[0] << '\t' << deltaVa[1] << '\t' << deltaVa[2] << '\n';
 	std::cout << "deltaVb: " << deltaVb[0] << '\t' << deltaVb[1] << '\t' << deltaVb[2] << '\n';
@@ -74,6 +104,7 @@ btScalar btReducedDeformableRigidContactConstraint::solveConstraint(const btCont
 	// get delta relative velocity and magnitude (i.e., how much impulse has been applied?)
 	btVector3 deltaV_rel = deltaVa - deltaVb;
 	btScalar deltaV_rel_normal = -btDot(deltaV_rel, m_contactNormalA);
+	std::cout << "deltaV_rel_normal: " << deltaV_rel_normal << "\n";
 	
 	// get the normal impulse to be applied
 	btScalar deltaImpulse = m_rhs - deltaV_rel_normal / m_normalImpulseFactor;
@@ -150,36 +181,34 @@ btScalar btReducedDeformableRigidContactConstraint::solveConstraint(const btCont
 	applyImpulse(impulse);
 	
 	// apply impulse to the rigid/multibodies involved and change their velocities
-	const btSoftBody::sCti& cti = m_contact->m_cti;
-	if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
+	if (!m_collideStatic)
 	{
-		btRigidBody* rigidCol = 0;
-		rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
-		if (rigidCol)
+		const btSoftBody::sCti& cti = m_contact->m_cti;
+		if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
 		{
-			std::cout << "called here??\n";
-			rigidCol->applyImpulse(impulse, m_contact->m_c1);
+			m_solverBody->internalApplyImpulse(m_linearComponent, m_angularComponent, -deltaImpulse);
+			// m_solverBody->internalApplyImpulse(m_linearComponent, m_angularComponent, -deltaImpulse_tangent);
+		}
+		else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
+		{
+			btAssert(false);	//TODO: unsupported yet
+			// 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));
+			// 	}
+			// }
 		}
-	}
-	else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
-	{
-		btAssert(false);	//TODO: unsupported yet
-		// 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;
 }
@@ -192,7 +221,6 @@ btReducedDeformableNodeRigidContactConstraint::btReducedDeformableNodeRigidConta
 	btScalar dt)
   : m_node(contact.m_node), btReducedDeformableRigidContactConstraint(rsb, contact, infoGlobal, dt)
 {
-	m_relPosA = contact.m_c1;
 	m_relPosB = m_node->m_x - m_rsb->getRigidTransform().getOrigin();
 	warmStarting();
 }
@@ -233,6 +261,16 @@ btVector3 btReducedDeformableNodeRigidContactConstraint::getVb() const
 	return m_node->m_v;
 }
 
+btVector3 btReducedDeformableNodeRigidContactConstraint::getDeltaVa() const
+{
+	btVector3 deltaVa(0, 0, 0);
+	if (!m_collideStatic)
+	{
+		deltaVa = m_solverBody->internalGetDeltaLinearVelocity() + m_relPosA.cross(m_solverBody->internalGetDeltaAngularVelocity());
+	}
+	return deltaVa;
+}
+
 btVector3 btReducedDeformableNodeRigidContactConstraint::getDeltaVb() const
 {
 	return m_rsb->internalComputeNodeDeltaVelocity(m_rsb->getInterpolationWorldTransform(), m_node->index);

src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.h

@@ -31,6 +31,8 @@ class btReducedDeformableStaticConstraint : public btDeformableStaticConstraint
 class btReducedDeformableRigidContactConstraint : public btDeformableRigidContactConstraint
 {
  public:
+  bool m_collideStatic;     // flag for collision with static object
+
   btReducedSoftBody* m_rsb;
   btSolverBody* m_solverBody;
   btScalar m_dt;
@@ -48,12 +50,14 @@ class btReducedDeformableRigidContactConstraint : public btDeformableRigidContac
   btVector3 m_contactNormalA;     // for rigid body
   btVector3 m_contactNormalB;     // for reduced deformable body
   btVector3 m_contactTangent;     // tangential direction of the relative velocity
-  btVector3 m_relPosA;            // relative position of the contact point for A
+  btVector3 m_relPosA;            // relative position of the contact point for A (rigid)
   btVector3 m_relPosB;            // relative position of the contact point for B
   btMatrix3x3 m_impulseFactor;    // total impulse matrix
 
   btVector3 m_bufferVelocityA;    // velocity at the beginning of the iteration
   btVector3 m_bufferVelocityB;
+  btVector3 m_linearComponent;    // linear components for the solver body
+  btVector3 m_angularComponent;   // angular components for the solver body
 
   btReducedDeformableRigidContactConstraint(btReducedSoftBody* rsb, 
                                             const btSoftBody::DeformableRigidContact& c, 
@@ -63,8 +67,8 @@ class btReducedDeformableRigidContactConstraint : public btDeformableRigidContac
   btReducedDeformableRigidContactConstraint() {}
   virtual ~btReducedDeformableRigidContactConstraint() {}
 
-  void setupSolverBody(btRigidBody* rigid_body);
-  
+  void setSolverBody(btSolverBody& solver_body);
+
   virtual void warmStarting() {}
 
   virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal);
@@ -73,6 +77,8 @@ class btReducedDeformableRigidContactConstraint : public btDeformableRigidContac
 
   virtual void applySplitImpulse(const btVector3& impulse) {} // TODO: may need later
 
+  virtual btVector3 getVa() const;
+  virtual btVector3 getDeltaVa() const = 0;
   virtual btVector3 getDeltaVb() const = 0;
 };
 
@@ -95,6 +101,9 @@ class btReducedDeformableNodeRigidContactConstraint : public btReducedDeformable
   // get the velocity of the deformable node in contact
 	virtual btVector3 getVb() const;
 
+  // get the velocity change of the rigid body
+  virtual btVector3 getDeltaVa() const;
+
   // get velocity change of the node in contat
   virtual btVector3 getDeltaVb() const;
 

src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp

@@ -3,6 +3,7 @@
 
 btReducedSoftBodySolver::btReducedSoftBodySolver()
 {
+  m_reducedSolver = true;
   m_dampingAlpha = 0;
   m_dampingBeta = 0;
   m_gravity = btVector3(0, 0, 0);
@@ -240,6 +241,29 @@ void btReducedSoftBodySolver::setConstraints(const btContactSolverInfo& infoGlob
   }
 }
 
+void btReducedSoftBodySolver::pairConstraintWithSolverBody(btSolverBody& solverBody)
+{
+  for (int i = 0; i < m_softBodies.size(); ++i)
+  {
+    btReducedSoftBody* rsb = static_cast<btReducedSoftBody*>(m_softBodies[i]);
+
+    // node vs rigid contact
+    for (int k = 0; k < m_nodeRigidConstraints[i].size(); ++k)
+    {
+      btReducedDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[i][k];
+      constraint.setSolverBody(solverBody);
+    }
+
+    // face vs rigid contact
+    // 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);
+    // }
+  }
+}
+
 btScalar btReducedSoftBodySolver::solveContactConstraints(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal)
 {
   btScalar residualSquare = 0;
@@ -306,6 +330,7 @@ btScalar btReducedSoftBodySolver::solveContactConstraints(btCollisionObject** de
 
 void btReducedSoftBodySolver::deformableBodyInternalWriteBack()
 {
+  // reduced deformable update
   for (int i = 0; i < m_softBodies.size(); ++i)
   {
     btReducedSoftBody* rsb = static_cast<btReducedSoftBody*>(m_softBodies[i]);

src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.h

@@ -44,6 +44,9 @@ class btReducedSoftBodySolver : public btDeformableBodySolver
   // set up contact constraints
 	virtual void setConstraints(const btContactSolverInfo& infoGlobal);
 
+  // pair rigid contact constraint with solver body
+  virtual void pairConstraintWithSolverBody(btSolverBody& solverBody);
+
   // solve all constraints (fixed and contact)
   virtual btScalar solveContactConstraints(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal);
 

src/BulletSoftBody/btDeformableBodySolver.cpp

@@ -23,6 +23,7 @@ btDeformableBodySolver::btDeformableBodySolver()
 	: m_numNodes(0), m_cg(kMaxConjugateGradientIterations), m_cr(kMaxConjugateGradientIterations), m_maxNewtonIterations(1), m_newtonTolerance(1e-4), m_lineSearch(false), m_useProjection(false)
 {
 	m_objective = new btDeformableBackwardEulerObjective(m_softBodies, m_backupVelocity);
+	m_reducedSolver = false;
 }
 
 btDeformableBodySolver::~btDeformableBodySolver()

src/BulletSoftBody/btDeformableBodySolver.h

@@ -46,6 +46,7 @@ protected:
 	int m_maxNewtonIterations;                                     // max number of newton iterations
 	btScalar m_newtonTolerance;                                    // stop newton iterations if f(x) < m_newtonTolerance
 	bool m_lineSearch;                                             // If true, use newton's method with line search under implicit scheme
+	bool m_reducedSolver;																					 // flag for reduced soft body solver
 public:
 	// handles data related to objective function
 	btDeformableBackwardEulerObjective* m_objective;
@@ -199,6 +200,14 @@ public:
 		m_objective->m_projection.setLagrangeMultiplier();
 	}
 
+	virtual bool isReducedSolver()
+	{
+		return m_reducedSolver;
+	}
+	
+	// pair rigid contact constraint with solver body
+  virtual void pairConstraintWithSolverBody(btSolverBody& solverBody) {}
+
 	virtual void deformableBodyInternalWriteBack() {}
 
 	// unused functions

src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp

@@ -21,6 +21,9 @@
 btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(btCollisionObject** bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
 {
 	{
+		// pair deformable body with solver body
+		pairDeformableAndSolverBody(bodies, numBodies, deformableBodies, numDeformableBodies, infoGlobal);
+
 		///this is a special step to resolve penetrations (just for contacts)
 		solveGroupCacheFriendlySplitImpulseIterations(bodies, numBodies, deformableBodies, numDeformableBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
 
@@ -89,6 +92,12 @@ void btDeformableMultiBodyConstraintSolver::solveDeformableBodyGroup(btCollision
 
 void btDeformableMultiBodyConstraintSolver::writeToSolverBody(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal)
 {
+	// reduced soft body solver directly modifies the solver body
+	if (m_deformableSolver->isReducedSolver())
+	{
+		return;
+	}
+
 	for (int i = 0; i < numBodies; i++)
 	{
 		int bodyId = getOrInitSolverBody(*bodies[i], infoGlobal.m_timeStep);
@@ -105,6 +114,12 @@ void btDeformableMultiBodyConstraintSolver::writeToSolverBody(btCollisionObject*
 
 void btDeformableMultiBodyConstraintSolver::solverBodyWriteBack(const btContactSolverInfo& infoGlobal)
 {
+	// reduced soft body solver directly modifies the solver body
+	if (m_deformableSolver->isReducedSolver())
+	{
+		return;
+	}
+
 	for (int i = 0; i < m_tmpSolverBodyPool.size(); i++)
 	{
 		btRigidBody* body = m_tmpSolverBodyPool[i].m_originalBody;
@@ -116,6 +131,32 @@ void btDeformableMultiBodyConstraintSolver::solverBodyWriteBack(const btContactS
 	}
 }
 
+
+void btDeformableMultiBodyConstraintSolver::pairDeformableAndSolverBody(btCollisionObject** bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal)
+{
+	if (!m_deformableSolver->isReducedSolver())
+	{
+		return;
+	}
+
+	for (int i = 0; i < numBodies; i++)
+	{
+		int bodyId = getOrInitSolverBody(*bodies[i], infoGlobal.m_timeStep);
+
+		btRigidBody* body = btRigidBody::upcast(bodies[i]);
+		if (body && body->getInvMass())
+		{
+			btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId];
+			m_deformableSolver->pairConstraintWithSolverBody(solverBody);
+		}
+	}
+
+	for (int i = 0; i < numDeformableBodies; ++i)
+	{
+		//
+	}
+}
+
 void btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
 {
 	BT_PROFILE("solveGroupCacheFriendlySplitImpulseIterations");

src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h

@@ -43,6 +43,9 @@ protected:
 	// write the velocity of the underlying rigid body to the the the solver body
 	void writeToSolverBody(btCollisionObject * *bodies, int numBodies, const btContactSolverInfo& infoGlobal);
 
+	// let each deformable body knows which solver body is in constact
+	void pairDeformableAndSolverBody(btCollisionObject** bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal);
+
 	virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject * *bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer);
 
 	virtual btScalar solveDeformableGroupIterations(btCollisionObject * *bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer);

src/BulletSoftBody/btSoftBodyInternals.h

@@ -1708,7 +1708,6 @@ struct btSoftColliders
 							{
 								btMatrix3x3 rigid_impulse_factor = ImpulseMatrixNonInverse(1, n.m_effectiveMass_inv, imb, iwi, ra);
 								c.m_c0 = psb->getImpulseFactor(n.index) + rigid_impulse_factor; //impulse factor K (not the inverse)
-								// c.m_c1 = n.m_x - psb->getRigidTransform().getOrigin();
 							}
 							else
 							{