From 3377de6b819283946243c161d07966e8c8e3df04 Mon Sep 17 00:00:00 2001
From: Martin Felis <martin@fysx.org>
Date: Sun, 15 Nov 2020 21:47:17 +0100
Subject: [PATCH] Explicitly formulating bias term (missed some changes)

---
 3rdparty/rbdl/include/rbdl/SimpleMath/SimpleMath.h | 12 ++++++++++++
 include/rbdlsim.h                                  |  2 ++
 tests/CollisionTests.cc                            |  2 ++
 3 files changed, 16 insertions(+)

diff --git a/3rdparty/rbdl/include/rbdl/SimpleMath/SimpleMath.h b/3rdparty/rbdl/include/rbdl/SimpleMath/SimpleMath.h
index 3205feb..c99386d 100644
--- a/3rdparty/rbdl/include/rbdl/SimpleMath/SimpleMath.h
+++ b/3rdparty/rbdl/include/rbdl/SimpleMath/SimpleMath.h
@@ -1233,6 +1233,12 @@ struct Matrix : public MatrixBase<Matrix<ScalarType, NumRows, NumCols>, ScalarTy
       return *this;
     }
 
+  Matrix& operator+=(const ScalarType& scalar) {
+    assert (rows() == 1 && cols() == 1 && "Error: matrix dimensions do not match!");
+    this->operator()(0,0) += scalar;
+    return *this;
+  }
+
   template <typename OtherDerived>
     Matrix& operator-=(const OtherDerived& other) {
       assert (rows() == other.rows() && cols() == other.cols() && "Error: matrix dimensions do not match!");
@@ -1244,6 +1250,12 @@ struct Matrix : public MatrixBase<Matrix<ScalarType, NumRows, NumCols>, ScalarTy
       return *this;
     }
 
+  Matrix& operator-=(const ScalarType& scalar) {
+    assert (rows() == 1 && cols() == 1 && "Error: matrix dimensions do not match!");
+    this->operator()(0,0) -= scalar;
+    return *this;
+  }
+
   inline ScalarType& operator()(const size_t& i, const size_t& j) {
     return mStorage.coeff(i, j);
   }
diff --git a/include/rbdlsim.h b/include/rbdlsim.h
index f195f11..4b1ba8e 100644
--- a/include/rbdlsim.h
+++ b/include/rbdlsim.h
@@ -49,6 +49,8 @@ struct CollisionInfo {
   int mBodyBIndex;
   Vector3d posA = Vector3d::Zero();
   Vector3d posB = Vector3d::Zero();
+  double biasVelocityA = 0.;
+  double biasVelocityB = 0.;
   double accumImpulse = 0.;
   double deltaImpulse = 0.;
   Vector3d dir = Vector3d::Zero();
diff --git a/tests/CollisionTests.cc b/tests/CollisionTests.cc
index 2a60f0f..4fef3fd 100644
--- a/tests/CollisionTests.cc
+++ b/tests/CollisionTests.cc
@@ -235,6 +235,7 @@ TEST_CASE("CalcConstraintImpulse", "[Collision]") {
 
     SECTION("CheckBounce") {
       cinfo.effectiveRestitution = 1.0;
+      PrepareConstraintImpulse(&ground_body, &sphere_a_body, cinfo);
       VectorNd old_vel = sphere_a_body.qdot;
       CalcConstraintImpulse(&ground_body, &sphere_a_body, cinfo, 0);
       ApplyConstraintImpulse(&ground_body, &sphere_a_body, cinfo);
@@ -339,6 +340,7 @@ TEST_CASE("CalcConstraintImpulse", "[Collision]") {
 
     SECTION("CheckBounce") {
       cinfo.effectiveRestitution = 1.0;
+      PrepareConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo);
       VectorNd old_vel_a = sphere_a_body.qdot;
       VectorNd old_vel_b = sphere_b_body.qdot;
       CalcConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo, 0);