From 4a8d6913342b905b6cb438d596e58ed167bbe269 Mon Sep 17 00:00:00 2001
From: Martin Felis <martin@fysx.org>
Date: Sun, 15 Nov 2020 21:46:43 +0100
Subject: [PATCH] Explicitly formulating bias term.

---
 src/rbdlsim.cc | 34 ++++++++++++++++++++++++++++------
 1 file changed, 28 insertions(+), 6 deletions(-)

diff --git a/src/rbdlsim.cc b/src/rbdlsim.cc
index c40c12a..4d2c91b 100644
--- a/src/rbdlsim.cc
+++ b/src/rbdlsim.cc
@@ -293,10 +293,13 @@ void CalcImpulseVariables(
     const Vector3d& dir,
     MatrixNd* MInv,
     VectorNd* jac,
-    double* G_MInv_GT) {
+    double* G_MInv_GT,
+    double* bias_vel,
+    double restitution) {
   if (body == nullptr || body->mIsStatic) {
     jac->setZero();
     *G_MInv_GT = 0.;
+    *bias_vel = 0.;
     return;
   }
 
@@ -320,6 +323,8 @@ void CalcImpulseVariables(
   (*jac) = dir.transpose() * G_constr;
 
   *G_MInv_GT = (*jac) * (*MInv) * (*jac).transpose();
+
+  *bias_vel = (*jac) * qdot * restitution;
 }
 
 void PrepareConstraintImpulse(
@@ -333,7 +338,10 @@ void PrepareConstraintImpulse(
       cinfo.dir,
       &cinfo.MInvA,
       &cinfo.jacA,
-      &cinfo.GMInvGTA);
+      &cinfo.GMInvGTA,
+      &cinfo.biasVelocityA,
+      cinfo.effectiveRestitution);
+
   CalcImpulseVariables(
       body_b,
       cinfo.mBodyBIndex,
@@ -341,7 +349,9 @@ void PrepareConstraintImpulse(
       cinfo.dir,
       &cinfo.MInvB,
       &cinfo.jacB,
-      &cinfo.GMInvGTB);
+      &cinfo.GMInvGTB,
+      &cinfo.biasVelocityB,
+      cinfo.effectiveRestitution);
 }
 
 /// Calculates the impulse that we apply on body_b to resolve the contact.
@@ -352,19 +362,30 @@ void CalcConstraintImpulse(
     const double dt) {
   // Todo: add nonlinear effects * dt
 
+  double ref_a = 0.;
+  double ref_b = 0.;
+  double vel_a = 0.;
+  double vel_b = 0.;
+  double ref = 0.;
   double rhs = 0.;
   if (body_a && !body_a->mIsStatic) {
-    rhs += cinfo.jacA * body_a->qdot * (1.0 + cinfo.effectiveRestitution);
+    vel_a = cinfo.jacA * body_a->qdot;
+    ref_a += cinfo.jacA * body_a->qdot * (1.0 + cinfo.effectiveRestitution);
+    rhs += cinfo.jacA * body_a->qdot + cinfo.biasVelocityA;
   }
   if (body_b && !body_b->mIsStatic) {
-    rhs += -cinfo.jacB * (body_b->qdot) * (1.0 + cinfo.effectiveRestitution);
+    vel_b = cinfo.jacB * body_b->qdot;
+    ref_b += -cinfo.jacB * (body_b->qdot) * (1.0 + cinfo.effectiveRestitution);
+    rhs += -cinfo.jacB * body_b->qdot - cinfo.biasVelocityB;
   }
 
+  ref = ref_a + ref_b;
+
   double denom = cinfo.GMInvGTA + cinfo.GMInvGTB;
 
   double old_impulse = cinfo.accumImpulse;
   // TODO: is this really needed here??
-  cinfo.deltaImpulse = std::max(0., rhs / denom);
+  cinfo.deltaImpulse = rhs / denom;
   cinfo.accumImpulse = std::max(0., cinfo.accumImpulse + cinfo.deltaImpulse);
   cinfo.deltaImpulse = cinfo.accumImpulse - old_impulse;
 }
@@ -446,6 +467,7 @@ void World::resolveCollisions(double dt) {
     for (CollisionInfo& cinfo : mContactPoints) {
       CalcConstraintImpulse(cinfo.mBodyA, cinfo.mBodyB, cinfo, dt);
       ApplyConstraintImpulse(cinfo.mBodyA, cinfo.mBodyB, cinfo);
+      gLog ("Iter %d: Apply impulse: %f", i, cinfo.deltaImpulse);
     }
   }
 }