diff --git a/include/rbdlsim.h b/include/rbdlsim.h
index 660f9e1..973152a 100644
--- a/include/rbdlsim.h
+++ b/include/rbdlsim.h
@@ -47,7 +47,7 @@ struct CollisionInfo {
   const SimShape* mShapeB = nullptr;
   int mBodyAIndex;
   int mBodyBIndex;
-  Vector3d mManifoldPoints[4];
+  Vector3d mManifoldPoints[8];
   int mNumManifoldPoints = 0;
   Vector3d posA = Vector3d::Zero();
   Vector3d posB = Vector3d::Zero();
diff --git a/src/rbdlsim.cc b/src/rbdlsim.cc
index 2025b6e..b7e2102 100644
--- a/src/rbdlsim.cc
+++ b/src/rbdlsim.cc
@@ -248,6 +248,14 @@ bool CheckPenetrationBoxVsPlane(
 
   bool result = CheckPenetrationAABBVsPlane(aabb, plane, cinfo);
 
+  if (isnan(cinfo.posA.squaredNorm())) {
+    gLog ("NaN error!");
+  }
+
+  assert (!isnan(cinfo.dir.squaredNorm()));
+  assert (!isnan(cinfo.posA.squaredNorm()));
+  assert (!isnan(cinfo.posB.squaredNorm()));
+
   cinfo.posA = shape_a_rot.transpose() * (cinfo.posA) + shape_a.pos;
   cinfo.posB = shape_a_rot.transpose() * (cinfo.posB) + shape_a.pos;
   cinfo.dir = shape_a_rot.transpose() * (cinfo.dir);
@@ -297,14 +305,14 @@ bool CheckPenetrationAABBVsPlane(
   const Vector3d& scale = shape_a.scale;
   // clang-format off
   Vector3d vertices[8] = {
-      Vector3d( scale[0] * 0.5, -scale[0] * 0.5,  scale[0] * 0.5),
-      Vector3d( scale[0] * 0.5, -scale[0] * 0.5, -scale[0] * 0.5),
-      Vector3d( scale[0] * 0.5,  scale[0] * 0.5,  scale[0] * 0.5),
-      Vector3d( scale[0] * 0.5,  scale[0] * 0.5, -scale[0] * 0.5),
-      Vector3d(-scale[0] * 0.5, -scale[0] * 0.5,  scale[0] * 0.5),
-      Vector3d(-scale[0] * 0.5, -scale[0] * 0.5, -scale[0] * 0.5),
-      Vector3d(-scale[0] * 0.5,  scale[0] * 0.5,  scale[0] * 0.5),
-      Vector3d(-scale[0] * 0.5,  scale[0] * 0.5, -scale[0] * 0.5)
+      Vector3d( scale[0] * 0.5, -scale[1] * 0.5,  scale[2] * 0.5),
+      Vector3d( scale[0] * 0.5, -scale[1] * 0.5, -scale[2] * 0.5),
+      Vector3d( scale[0] * 0.5,  scale[1] * 0.5,  scale[2] * 0.5),
+      Vector3d( scale[0] * 0.5,  scale[1] * 0.5, -scale[2] * 0.5),
+      Vector3d(-scale[0] * 0.5, -scale[1] * 0.5,  scale[2] * 0.5),
+      Vector3d(-scale[0] * 0.5, -scale[1] * 0.5, -scale[2] * 0.5),
+      Vector3d(-scale[0] * 0.5,  scale[1] * 0.5,  scale[2] * 0.5),
+      Vector3d(-scale[0] * 0.5,  scale[1] * 0.5, -scale[2] * 0.5)
   };
   // clang-format on
 
@@ -351,6 +359,9 @@ bool CheckPenetrationAABBVsPlane(
         assert(s <= 1.);
 
         cinfo.mManifoldPoints[cinfo.mNumManifoldPoints++] = v0 + s * (v1 - v0);
+        if (cinfo.mNumManifoldPoints > 4) {
+          gLog ("Have %d manifold points?!", cinfo.mNumManifoldPoints);
+        }
       }
     }
   }
@@ -476,6 +487,8 @@ void CalcImpulseVariables(
   const VectorNd& q = body->q;
   const VectorNd& qdot = body->qdot;
 
+  assert (!isnan(q.squaredNorm()));
+
   // Calculate local coordinates of the contact point
   UpdateKinematicsCustom(*model, &q, nullptr, nullptr);
   Vector3d point_local_b =
@@ -619,10 +632,15 @@ void World::detectCollisions() {
         cinfo.effectiveRestitution = ref_body_shape.restitution;
 
         if (has_penetration) {
+          if (isnan(cinfo.posA.squaredNorm())) {
+            gLog ("NaN error!");
+          }
           cinfo.mBodyA = nullptr;
           cinfo.mBodyAIndex = -1;
           cinfo.mBodyB = &ref_body;
           cinfo.mBodyBIndex = body_col_info.first;
+          assert (!isnan(cinfo.posA.squaredNorm()));
+          assert (!isnan(cinfo.posB.squaredNorm()));
           mContactPoints.push_back(cinfo);
         }
       }
diff --git a/src/simulator.cc b/src/simulator.cc
index a77216e..3190db2 100644
--- a/src/simulator.cc
+++ b/src/simulator.cc
@@ -43,9 +43,9 @@ void simulator_init() {
 
   sWorld.mStaticShapes.push_back(sGroundShape);
 
-  double restitution = 0.2;
+  double restitution = 0.01;
 
-  int num_bodies = 1;
+  int num_bodies = 5;
   for (int i = 0; i < num_bodies; i++) {
     SimBody body = CreateBoxBody(
         1.,