Sort of working bouncing simulation.

include/rbdlsim.h

@@ -23,6 +23,7 @@ struct SimShape {
   Vector3d pos;
   Quaternion orientation;
   Vector3d scale;
+  double restitution = 1.0;
 };
 
 struct SimBody {
@@ -40,16 +41,18 @@ struct SimBody {
 };
 
 struct CollisionInfo {
-  const SimBody* mBodyA = nullptr;
+  SimBody* mBodyA = nullptr;
-  const SimBody* mBodyB = nullptr;
+  SimBody* mBodyB = nullptr;
   const SimShape* mShapeA = nullptr;
   const SimShape* mShapeB = nullptr;
   int mBodyAIndex;
   int mBodyBIndex;
   Vector3d posA = Vector3d::Zero();
   Vector3d posB = Vector3d::Zero();
-  Vector3d accumImpulseA = Vector3d::Zero();
+  double accumImpulseA = 0.;
-  Vector3d accumImpulseB = Vector3d::Zero();
+  double accumImpulseB = 0.;
+  double deltaImpulseA = 0.;
+  double deltaImpulseB = 0.;
   Vector3d dir = Vector3d::Zero();
   VectorNd jacA = VectorNd::Zero(1);
   VectorNd jacB = VectorNd::Zero(1);
@@ -57,6 +60,7 @@ struct CollisionInfo {
   MatrixNd MInvB = MatrixNd::Zero(1, 1);
   double GMInvGTA = 0.;
   double GMInvGTB = 0.;
+  double effectiveRestitution = 1.0;
 
   double depth = 0.;
 };
@@ -67,6 +71,7 @@ struct World {
   std::vector<SimShape> mStaticShapes;
   std::vector<CollisionInfo> mContactPoints;
 
+  void calcUnconstrainedVelUpdate(double dt);
   void updateCollisionShapes();
   void detectCollisions();
   void resolveCollisions(double dt);

src/rbdlsim.cc

@@ -1,4 +1,5 @@
 #include "rbdlsim.h"
+#include "utils.h"
 
 #include <ccd/ccd.h>
 #include <ccd/quat.h>
@@ -255,8 +256,8 @@ bool SolveGaussSeidelProj(
 }
 
 void CalcCollisions(
-    const SimBody& body_a,
+    SimBody& body_a,
-    const SimBody& body_b,
+    SimBody& body_b,
     std::vector<CollisionInfo>& collisions) {
   collisions.clear();
 
@@ -269,6 +270,7 @@ void CalcCollisions(
       CollisionInfo cinfo;
       bool has_penetration = false;
       has_penetration = CheckPenetration(shape_a, shape_b, cinfo);
+      cinfo.effectiveRestitution = shape_a.restitution * shape_b.restitution;
 
       if (has_penetration) {
         cinfo.mBodyA = &body_a;
@@ -289,7 +291,7 @@ void CalcImpulseVariables(
     MatrixNd* MInv,
     VectorNd* jac,
     double* G_MInv_GT) {
-  if (body->mIsStatic) {
+  if (body == nullptr || body->mIsStatic) {
     *G_MInv_GT = 0.;
     return;
   }
@@ -346,21 +348,30 @@ void CalcConstraintImpulse(
   // Todo: add nonlinear effects * dt
 
   double rhs = 0.;
-  if (!body_a->mIsStatic) {
+  if (body_a && !body_a->mIsStatic) {
-    rhs += cinfo.jacA * body_a->qdot;
+    rhs += (1.0 + cinfo.effectiveRestitution) * cinfo.jacA * body_a->qdot;
   }
-  if (!body_b->mIsStatic) {
+  if (body_b && !body_b->mIsStatic) {
-    rhs += cinfo.jacB * body_b->qdot;
+    rhs += (1.0 + cinfo.effectiveRestitution) * cinfo.jacB * (body_b->qdot);
   }
 
   double denom = cinfo.GMInvGTA + cinfo.GMInvGTB;
 
-  if (!body_a->mIsStatic) {
+  if (body_a && !body_a->mIsStatic) {
-    cinfo.accumImpulseA = -rhs / denom * cinfo.dir;
+    double old_impulse = cinfo.accumImpulseA;
+    cinfo.deltaImpulseA = -rhs / denom;
+    cinfo.accumImpulseA += cinfo.deltaImpulseA;
+    cinfo.accumImpulseA = std::max(0., cinfo.accumImpulseA);
+    cinfo.deltaImpulseA = cinfo.accumImpulseA - old_impulse;
   }
 
-  if (!body_b->mIsStatic) {
+  if (body_b && !body_b->mIsStatic) {
-    cinfo.accumImpulseB = rhs / denom * cinfo.dir;
+    double old_impulse = cinfo.accumImpulseB;
+    cinfo.deltaImpulseB = std::max(0., rhs / denom);
+    cinfo.accumImpulseB += cinfo.deltaImpulseB;
+    cinfo.accumImpulseB = std::max(0., cinfo.accumImpulseB);
+    cinfo.deltaImpulseB = cinfo.accumImpulseB - old_impulse;
+    gLog ("deltaImpulse: %f", cinfo.deltaImpulseB);
   }
 }
 
@@ -374,14 +385,14 @@ void ApplyConstraintImpulse(
     SimBody* body_a,
     SimBody* body_b,
     CollisionInfo& cinfo) {
-  if (!body_a->mIsStatic) {
+  if (body_a && !body_a->mIsStatic) {
     body_a->qdot += cinfo.MInvA * cinfo.jacA.transpose()
-                    * (-cinfo.dir.transpose() * cinfo.accumImpulseB);
+                    * (-cinfo.deltaImpulseA);
   }
 
-  if (!body_b->mIsStatic) {
+  if (body_b && !body_b->mIsStatic) {
     body_b->qdot += cinfo.MInvB * cinfo.jacB.transpose()
-                    * (-cinfo.dir.transpose() * cinfo.accumImpulseB);
+                    * (-cinfo.deltaImpulseB);
   }
 }
 
@@ -471,6 +482,15 @@ void SimBody::resolveCollisions(
   qdot = qdot + Minv * (dt * tau + dt * N + G.transpose() * hlambda);
 }
 
+void World::calcUnconstrainedVelUpdate(double dt) {
+  for (SimBody& body : mBodies) {
+    ForwardDynamics(body.mModel, body.q, body.qdot, body.tau, body.qddot);
+
+    // semi-implicit eulers
+    body.qdot += dt * body.qddot;
+  }
+}
+
 void World::updateCollisionShapes() {
   for (SimBody& body : mBodies) {
     body.updateCollisionShapes();
@@ -480,21 +500,32 @@ void World::updateCollisionShapes() {
 void World::detectCollisions() {
   mContactPoints.clear();
 
-  for (SimBody& body : mBodies) {
+  for (int i = 0, n = mBodies.size(); i < n; ++i) {
-    for (SimBody::BodyCollisionInfo& body_col_info : body.mCollisionShapes) {
+    SimBody& ref_body = mBodies[i];
-      SimShape& body_shape = body_col_info.second;
+
+    // Check collisions against moving bodies
+    for (int j = i + 1; j < n; ++j) {
+      SimBody& other_body = mBodies[j];
+      CalcCollisions(ref_body, other_body, mContactPoints);
+    }
+
+    // Check collisions against static bodies
+    for (SimBody::BodyCollisionInfo& body_col_info :
+         ref_body.mCollisionShapes) {
+      SimShape& ref_body_shape = body_col_info.second;
 
       // check against all static shapes
       for (SimShape& static_shape : mStaticShapes) {
         bool has_penetration = false;
         CollisionInfo cinfo;
 
-        has_penetration = CheckPenetration(static_shape, body_shape, cinfo);
+        has_penetration = CheckPenetration(static_shape, ref_body_shape, cinfo);
+        cinfo.effectiveRestitution = ref_body_shape.restitution;
 
         if (has_penetration) {
           cinfo.mBodyA = nullptr;
           cinfo.mBodyAIndex = -1;
-          cinfo.mBodyB = &body;
+          cinfo.mBodyB = &ref_body;
           cinfo.mBodyBIndex = body_col_info.first;
           mContactPoints.push_back(cinfo);
         }
@@ -504,46 +535,15 @@ void World::detectCollisions() {
 }
 
 void World::resolveCollisions(double dt) {
-  // so far only solve Body vs World collisions
+  for (CollisionInfo& cinfo : mContactPoints) {
-  for (SimBody& body : mBodies) {
+    PrepareConstraintImpulse(cinfo.mBodyA, cinfo.mBodyB, cinfo);
-    // collect all collisions for current body
+  }
-    std::vector<CollisionInfo> body_collisions;
 
-    for (const CollisionInfo cinfo : mContactPoints) {
+  int num_iter = 20;
-      if (cinfo.mBodyA == &body) {
+  for (int i = 0; i < num_iter; i++) {
-        body_collisions.push_back(cinfo);
+    for (CollisionInfo& cinfo : mContactPoints) {
-      } else if (cinfo.mBodyB == &body) {
+      CalcConstraintImpulse(cinfo.mBodyA, cinfo.mBodyB, cinfo, dt);
-        // Make sure the collision info is expressed in terms
+      ApplyConstraintImpulse(cinfo.mBodyA, cinfo.mBodyB, cinfo);
-        // of mBodyA.
-        CollisionInfo rev_cinfo;
-
-        rev_cinfo.mBodyA = &body;
-        rev_cinfo.mShapeA = cinfo.mShapeB;
-        rev_cinfo.mBodyAIndex = cinfo.mBodyBIndex;
-
-        rev_cinfo.mBodyB = cinfo.mBodyA;
-        rev_cinfo.mShapeB = cinfo.mShapeA;
-        rev_cinfo.mBodyBIndex = cinfo.mBodyAIndex;
-
-        Vector3d temp = cinfo.posA;
-        rev_cinfo.posA = cinfo.posB;
-        rev_cinfo.posA = temp;
-        rev_cinfo.dir = -cinfo.dir;
-        rev_cinfo.depth = cinfo.depth;
-
-        body_collisions.push_back(rev_cinfo);
-      }
-    }
-
-    if (body_collisions.size() > 0) {
-      cout << "Collision at t = " << mSimTime
-           << ", pos = " << body_collisions[0].posA.transpose()
-           << ", depth = " << body_collisions[0].depth << endl
-           << " qdotpre  = " << mBodies[0].qdot.transpose() << endl;
-    }
-    body.resolveCollisions(dt, body_collisions);
-    if (body_collisions.size() > 0) {
-      cout << " qdotpost = " << mBodies[0].qdot.transpose() << endl;
     }
   }
 }
@@ -596,6 +596,7 @@ SimBody CreateSphereBody(
   shape.pos = pos;
   shape.orientation.set(0., 0., 0., 1.);
   shape.scale.set(radius, radius, radius);
+  shape.restitution = 0.3;
 
   result.mCollisionShapes.push_back(
       SimBody::BodyCollisionInfo(sphere_body, shape));

src/simulator.cc

@@ -17,6 +17,7 @@ using namespace RBDLSim;
 static World sWorld;
 static SimShape sGroundShape;
 static SimBody sSphereBody;
+static SimBody sSphereBody2;
 
 typedef SimpleMath::Matrix<float, 3, 3> Matrix33f;
 typedef SimpleMath::Matrix<float, 3, 1> Vector3f;
@@ -35,9 +36,13 @@ void simulator_init() {
   sWorld.mStaticShapes.push_back(sGroundShape);
 
   sSphereBody =
-      CreateSphereBody(10., 1., Vector3d(0., 5.405, 0.), Vector3d::Zero());
+      CreateSphereBody(1., 1., Vector3d(0., 1.405, 0.), Vector3d::Zero());
   sWorld.mBodies.push_back(sSphereBody);
 
+  sSphereBody2 =
+      CreateSphereBody(1., 1., Vector3d(0.3, 2.405, 0.), Vector3d::Zero());
+//  sWorld.mBodies.push_back(sSphereBody2);
+
   sWorld.mSimTime = 0.;
 }
 
@@ -65,6 +70,12 @@ void simulator_update(double dt) {
   }
 
   ImGui::End();
+
+  sWorld.calcUnconstrainedVelUpdate(dt);
+  sWorld.updateCollisionShapes();
+  sWorld.detectCollisions();
+  sWorld.resolveCollisions(dt);
+  sWorld.integrateWorld(dt);
 }
 
 void simulator_draw(srcmdbuf* cmdbuf) {

src/vissim.cc

@@ -166,7 +166,7 @@ void DoRender() {
   simd4x4f view;
   simd4x4f proj;
   simd4x4f_translation(&view, 0.1f, 0.1f, -0.5f);
-  simd4f eye = simd4f_create (2.f * sin(gTimer->mCurrentTime), 2 *sin(gTimer->mCurrentTime * 0.5f), 2.f * cos (gTimer->mCurrentTime), 1.f);
+  simd4f eye = simd4f_create (0.f, 1.f, 10.f, 1.f);
   simd4x4f_lookat(
       &view,
       eye,
@@ -185,7 +185,7 @@ void DoRender() {
       -50.0f,
       50.0f);
   simd4x4f_perspective(&proj,
-                       110.f *  (M_PI / 180.f), view_width / view_height, 0.1f, 50.f);
+                       70.f *  (M_PI / 180.f), view_width / view_height, 0.1f, 50.f);
   srview_set_view(gView, view);
   srview_set_proj(gView, proj);