diff --git a/include/fcl/narrowphase/narrowphase.h b/include/fcl/narrowphase/narrowphase.h
index 634f3f2d7250c42116ec0e46e810ddbcd755ab5c..fbb5ff0a1c475f5ea8494e1c6eeccc8f8b9865c4 100644
--- a/include/fcl/narrowphase/narrowphase.h
+++ b/include/fcl/narrowphase/narrowphase.h
@@ -477,7 +477,7 @@ struct GJKSolver_indep
             w0 += shape.support(epa.result.c[i]->d, 0) * epa.result.p[i];
           }
           if(penetration_depth) *penetration_depth = -epa.depth;
-          if(normal) *normal = -epa.normal;
+          if(normal) *normal = epa.normal;
           if(contact_points) *contact_points = tf1.transform(w0 - epa.normal*(epa.depth *0.5));
           return true;
         }
diff --git a/src/narrowphase/gjk_libccd.cpp b/src/narrowphase/gjk_libccd.cpp
index 76f2a928a3d49d1a48c81ebdc937b66f5e500782..5f19800c28875dbea52ba1d5612e10b6d276dc4b 100644
--- a/src/narrowphase/gjk_libccd.cpp
+++ b/src/narrowphase/gjk_libccd.cpp
@@ -776,13 +776,13 @@ bool GJKCollide(void* obj1, ccd_support_fn supp1, ccd_center_fn cen1,
   }
 
 
-  /// libccd returns dir and pos in world space and dir is pointing from object 2 to object 1
+  /// libccd returns dir and pos in world space and dir is pointing from object 1 to object 2
   res = ccdMPRPenetration(obj1, obj2, &ccd, &depth, &dir, &pos);
   if(res == 0)
   {
     contact_points->setValue(ccdVec3X(&pos), ccdVec3Y(&pos), ccdVec3Z(&pos));
     *penetration_depth = depth;
-    normal->setValue(-ccdVec3X(&dir), -ccdVec3Y(&dir), -ccdVec3Z(&dir));
+    normal->setValue(ccdVec3X(&dir), ccdVec3Y(&dir), ccdVec3Z(&dir));
 
     return true;
   }
diff --git a/src/narrowphase/narrowphase.cpp b/src/narrowphase/narrowphase.cpp
index ec2ad841994d2a7a3be292117f9c1dfb307ae2f9..bcd28b638cba5b733d9292c8bc6e7926fb6b758d 100644
--- a/src/narrowphase/narrowphase.cpp
+++ b/src/narrowphase/narrowphase.cpp
@@ -270,13 +270,16 @@ bool sphereSphereIntersect(const Sphere& s1, const Transform3f& tf1,
                            const Sphere& s2, const Transform3f& tf2,
                            Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal)
 {
-  Vec3f diff = tf1.transform(Vec3f()) - tf2.transform(Vec3f());
+  Vec3f diff = tf2.transform(Vec3f()) - tf1.transform(Vec3f());
   FCL_REAL len = diff.length();
   if(len > s1.radius + s2.radius)
     return false;
 
   if(penetration_depth) 
     *penetration_depth = s1.radius + s2.radius - len;
+
+  // If the centers of two sphere are at the same position, the normal is (0, 0, 0).
+  // Otherwise, normal is pointing from center of object 1 to center of object 2
   if(normal) 
   {
     if(len > 0)
@@ -286,7 +289,7 @@ bool sphereSphereIntersect(const Sphere& s1, const Transform3f& tf1,
   }
 
   if(contact_points)
-    *contact_points = tf1.transform(Vec3f()) - diff * s1.radius / (s1.radius + s2.radius);
+    *contact_points = tf1.transform(Vec3f()) + diff * s1.radius / (s1.radius + s2.radius);
   
   return true;
 }
@@ -424,7 +427,7 @@ bool sphereTriangleIntersect(const Sphere& s, const Transform3f& tf,
 
   if(has_contact)
   {
-    Vec3f contact_to_center = center - contact_point;
+    Vec3f contact_to_center = contact_point - center;
     FCL_REAL distance_sqr = contact_to_center.sqrLength();
 
     if(distance_sqr < radius_with_threshold * radius_with_threshold)
@@ -438,7 +441,7 @@ bool sphereTriangleIntersect(const Sphere& s, const Transform3f& tf,
       }
       else
       {
-        if(normal_) *normal_ = normal;
+        if(normal_) *normal_ = -normal;
         if(contact_points) *contact_points = contact_point;
         if(penetration_depth) *penetration_depth = -radius;
       }
diff --git a/test/test_fcl_geometric_shapes.cpp b/test/test_fcl_geometric_shapes.cpp
index 7f034ea75db3e33e91940a2d341cf2fa82737e48..48f6545c692701747f5c9ca7e52e1337c4e94c5e 100644
--- a/test/test_fcl_geometric_shapes.cpp
+++ b/test/test_fcl_geometric_shapes.cpp
@@ -65,7 +65,7 @@ BOOST_AUTO_TEST_CASE(gjkcache)
 
   TranslationMotion motion(Transform3f(Vec3f(-20.0, -20.0, -20.0)), Transform3f(Vec3f(20.0, 20.0, 20.0)));
 
-  int N = 1000;  
+  int N = 1000;
   FCL_REAL dt = 1.0 / (N - 1);
 
   /// test exploiting spatial coherence
@@ -283,85 +283,81 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_spheresphere)
   Sphere s1(20);
   Sphere s2(10);
 
+  Transform3f tf1;
+  Transform3f tf2;
+
   Transform3f transform;
   generateRandomTransform(extents, transform);
-  Transform3f identity;
 
-  CollisionRequest request;
-  CollisionResult result;
-  bool res;
+  // Vec3f point;
+  // FCL_REAL depth;
+  Vec3f normal;
 
-  res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), NULL, NULL, NULL);
-  BOOST_CHECK_FALSE(res);
-  result.clear();
-  res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(40, 0, 0)), request, result) > 0);
-  BOOST_CHECK_FALSE(res);
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(40, 0, 0));
+  testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
 
-  res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)), NULL, NULL, NULL);
-  BOOST_CHECK_FALSE(res);
-  result.clear();
-  res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(40, 0, 0)), request, result) > 0);
-  BOOST_CHECK_FALSE(res);
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(40, 0, 0));
+  testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
 
-  res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(30, 0, 0)), NULL, NULL, NULL);
-  BOOST_CHECK(res);
-  result.clear();
-  res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(30, 0, 0)), request, result) > 0);
-  BOOST_CHECK(res);
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(30, 0, 0));
+  normal.setValue(1, 0, 0);
+  testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal);
 
-  res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(30.01, 0, 0)), NULL, NULL, NULL);
-  BOOST_CHECK_FALSE(res);
-  result.clear();
-  res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(30.01, 0, 0)), request, result) > 0);
-  BOOST_CHECK_FALSE(res);
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(30.01, 0, 0));
+  testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
 
-  res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(29.9, 0, 0)), NULL, NULL, NULL);
-  BOOST_CHECK(res);
-  result.clear();
-  res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(29.9, 0, 0)), request, result) > 0);
-  BOOST_CHECK(res);
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(30.01, 0, 0));
+  normal = transform.getRotation() * Vec3f(1, 0, 0);
+  testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
 
-  res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(29.9, 0, 0)), NULL, NULL, NULL);
-  BOOST_CHECK(res);
-  result.clear();
-  res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(29.9, 0, 0)), request, result) > 0);
-  BOOST_CHECK(res);
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(29.9, 0, 0));
+  normal.setValue(1, 0, 0);
+  testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal);
 
-  res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL, NULL, NULL);
-  BOOST_CHECK(res);
-  result.clear();
-  res = (collide(&s1, Transform3f(), &s2, Transform3f(), request, result) > 0);
-  BOOST_CHECK(res);
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(29.9, 0, 0));
+  normal = transform.getRotation() * Vec3f(1, 0, 0);
+  testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal);
 
-  res = solver1.shapeIntersect(s1, transform, s2, transform, NULL, NULL, NULL);
-  BOOST_CHECK(res);
-  result.clear();
-  res = (collide(&s1, transform, &s2, transform, request, result) > 0);
-  BOOST_CHECK(res);
+  tf1 = Transform3f();
+  tf2 = Transform3f();
+  normal.setZero();  // If the centers of two sphere are at the same position, the normal is (0, 0, 0)
+  testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal);
 
-  res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(-29.9, 0, 0)), NULL, NULL, NULL);
-  BOOST_CHECK(res);
-  result.clear();
-  res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(-29.9, 0, 0)), request, result) > 0);
-  BOOST_CHECK(res);
+  tf1 = transform;
+  tf2 = transform;
+  normal.setZero();  // If the centers of two sphere are at the same position, the normal is (0, 0, 0)
+  testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal);
 
-  res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(-29.9, 0, 0)), NULL, NULL, NULL);
-  BOOST_CHECK(res);
-  result.clear();
-  res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(-29.9, 0, 0)), request, result) > 0);
-  BOOST_CHECK(res);
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(-29.9, 0, 0));
+  normal.setValue(-1, 0, 0);
+  testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal);
 
-  res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(-30, 0, 0)), NULL, NULL, NULL);
-  BOOST_CHECK(res);
-  result.clear();
-  res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(-30, 0, 0)), request, result) > 0);
-  BOOST_CHECK(res);
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(-29.9, 0, 0));
+  normal = transform.getRotation() * Vec3f(-1, 0, 0);
+  testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal);
 
-  res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(-30.01, 0, 0)), NULL, NULL, NULL);
-  BOOST_CHECK_FALSE(res);
-  result.clear();
-  res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(-30.01, 0, 0)), request, result) > 0);
-  BOOST_CHECK_FALSE(res);
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(-30.0, 0, 0));
+  normal.setValue(-1, 0, 0);
+  testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(-30.01, 0, 0));
+  normal.setValue(-1, 0, 0);
+  testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
+
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(-30.01, 0, 0));
+  testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
 }
 
 BOOST_AUTO_TEST_CASE(shapeIntersection_boxbox)
@@ -369,52 +365,49 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_boxbox)
   Box s1(20, 40, 50);
   Box s2(10, 10, 10);
 
+  Transform3f tf1;
+  Transform3f tf2;
+
   Transform3f transform;
   generateRandomTransform(extents, transform);
-  Transform3f identity;
 
-  CollisionRequest request;
-  CollisionResult result;
+  // Vec3f point;
+  // FCL_REAL depth;
+  Vec3f normal;
 
-  bool res;
+  Quaternion3f q;
+  q.fromAxisAngle(Vec3f(0, 0, 1), (FCL_REAL)3.140 / 6);
 
-  res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL, NULL, NULL);
-  BOOST_CHECK(res);
-  result.clear();
-  res = (collide(&s1, Transform3f(), &s2, Transform3f(), request, result) > 0);
-  BOOST_CHECK(res);
+  tf1 = Transform3f();
+  tf2 = Transform3f();
+  // TODO: Need convention for normal when the centers of two objects are at same position. The current result is (1, 0, 0).
+  normal.setValue(1, 0, 0);
+  testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal);
 
-  res = solver1.shapeIntersect(s1, transform, s2, transform, NULL, NULL, NULL);
-  BOOST_CHECK(res);
-  result.clear();
-  res = (collide(&s1, transform, &s2, transform, request, result) > 0);
-  BOOST_CHECK(res);
+  tf1 = transform;
+  tf2 = transform;
+  // TODO: Need convention for normal when the centers of two objects are at same position. The current result is (1, 0, 0).
+  normal.setValue(1, 0, 0);
+  testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal);
 
-  res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(15, 0, 0)), NULL, NULL, NULL);
-  BOOST_CHECK(res);
-  result.clear();
-  res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(15, 0, 0)), request, result) > 0);
-  BOOST_CHECK(res);
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(15, 0, 0));
+  normal.setValue(1, 0, 0);
+  testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal);
 
-  res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(15.01, 0, 0)), NULL, NULL, NULL);
-  BOOST_CHECK_FALSE(res);
-  result.clear();
-  res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(15.01, 0, 0)), request, result) > 0);
-  BOOST_CHECK_FALSE(res);
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(15.01, 0, 0));
+  testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
 
-  Quaternion3f q;
-  q.fromAxisAngle(Vec3f(0, 0, 1), (FCL_REAL)3.140 / 6);
-  res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(q), NULL, NULL, NULL);
-  BOOST_CHECK(res);
-  result.clear();
-  res = (collide(&s1, Transform3f(), &s2, Transform3f(q), request, result) > 0);
-  BOOST_CHECK(res);
+  tf1 = Transform3f();
+  tf2 = Transform3f(q);
+  normal = Transform3f(q).getRotation() * Vec3f(1, 0, 0);
+  testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal);
 
-  res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(q), NULL, NULL, NULL);
-  BOOST_CHECK(res);
-  result.clear();
-  res = (collide(&s1, transform, &s2, transform * Transform3f(q), request, result) > 0);
-  BOOST_CHECK(res);
+  tf1 = transform;
+  tf2 = transform * Transform3f(q);
+  normal = Transform3f(q).getRotation() * Vec3f(1, 0, 0);
+  testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal);
 }
 
 BOOST_AUTO_TEST_CASE(shapeIntersection_spherebox)
@@ -422,51 +415,101 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_spherebox)
   Sphere s1(20);
   Box s2(5, 5, 5);
 
+  Transform3f tf1;
+  Transform3f tf2;
+
   Transform3f transform;
   generateRandomTransform(extents, transform);
-  Transform3f identity;
 
-  CollisionRequest request;
-  CollisionResult result;
+  // Vec3f point;
+  // FCL_REAL depth;
+  Vec3f normal;
 
-  bool res;
+  tf1 = Transform3f();
+  tf2 = Transform3f();
+  // TODO: Need convention for normal when the centers of two objects are at same position. The current result is (-1, 0, 0).
+  normal.setValue(-1, 0, 0);
+  testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal);
 
-  res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL, NULL, NULL);
-  BOOST_CHECK(res);
-  result.clear();
-  res = (collide(&s1, Transform3f(), &s2, Transform3f(), request, result) > 0);
-  BOOST_CHECK(res);
+  tf1 = transform;
+  tf2 = transform;
+  // TODO: Need convention for normal when the centers of two objects are at same position. The current result is (-0.9985590945508502, 0.02998909000838618, -0.04450156368325561).
+  normal.setValue(-0.9985590945508502, 0.02998909000838618, -0.04450156368325561);
+  testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal);
 
-  res = solver1.shapeIntersect(s1, transform, s2, transform, NULL, NULL, NULL);
-  BOOST_CHECK(res);
-  result.clear();
-  res = (collide(&s1, transform, &s2, transform, request, result) > 0);
-  BOOST_CHECK(res);
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(22.5, 0, 0));
+  testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
 
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(22.501, 0, 0));
+  testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
 
-  res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(22.5, 0, 0)), NULL, NULL, NULL);
-  BOOST_CHECK_FALSE(res);
-  result.clear();
-  res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(22.5, 0, 0)), request, result) > 0);
-  BOOST_CHECK_FALSE(res);
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(22.4, 0, 0));
+  normal.setValue(1, 0, 0);
+  testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal);
 
-  res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(22.501, 0, 0)), NULL, NULL, NULL);
-  BOOST_CHECK_FALSE(res);
-  result.clear();
-  res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(22.501, 0, 0)), request, result) > 0);
-  BOOST_CHECK_FALSE(res);
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(22.4, 0, 0));
+  normal = transform.getRotation() * Vec3f(1, 0, 0);
+  testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal);
+}
 
-  res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(22.4, 0, 0)), NULL, NULL, NULL);
-  BOOST_CHECK(res);
-  result.clear();
-  res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(22.4, 0, 0)), request, result) > 0);
-  BOOST_CHECK(res);
+BOOST_AUTO_TEST_CASE(shapeIntersection_spherecapsule)
+{
+  Sphere s1(20);
+  Capsule s2(5, 10);
 
-  res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(22.4, 0, 0)), NULL, NULL, NULL);
-  BOOST_CHECK(res);
-  result.clear();
-  res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(22.4, 0, 0)), request, result) > 0);
-  BOOST_CHECK(res);
+  Transform3f tf1;
+  Transform3f tf2;
+
+  Transform3f transform;
+  generateRandomTransform(extents, transform);
+
+  // Vec3f point;
+  // FCL_REAL depth;
+  Vec3f normal;
+
+  tf1 = Transform3f();
+  tf2 = Transform3f();
+  // TODO: Need convention for normal when the centers of two objects are at same position.
+  testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, NULL);
+
+  tf1 = transform;
+  tf2 = transform;
+  // TODO: Need convention for normal when the centers of two objects are at same position.
+  testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, NULL);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(24.9, 0, 0));
+  normal.setValue(1, 0, 0);
+  testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal);
+
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(24.9, 0, 0));
+  normal = transform.getRotation() * Vec3f(1, 0, 0);
+  testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(25, 0, 0));
+  normal.setValue(1, 0, 0);
+  testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal);
+
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(25, 0, 0));
+  normal = transform.getRotation() * Vec3f(1, 0, 0);
+  testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(25.1, 0, 0));
+  normal.setValue(1, 0, 0);
+  testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
+
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(25.1, 0, 0));
+  normal = transform.getRotation() * Vec3f(1, 0, 0);
+  testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
 }
 
 BOOST_AUTO_TEST_CASE(shapeIntersection_cylindercylinder)
@@ -474,50 +517,43 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_cylindercylinder)
   Cylinder s1(5, 10);
   Cylinder s2(5, 10);
 
+  Transform3f tf1;
+  Transform3f tf2;
+
   Transform3f transform;
   generateRandomTransform(extents, transform);
-  Transform3f identity;
-
-  CollisionRequest request;
-  CollisionResult result;
 
-  bool res;
+  // Vec3f point;
+  // FCL_REAL depth;
+  Vec3f normal;
 
-  res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL, NULL, NULL);
-  BOOST_CHECK(res);
-  result.clear();
-  res = (collide(&s1, Transform3f(), &s2, Transform3f(), request, result) > 0);
-  BOOST_CHECK(res);
+  tf1 = Transform3f();
+  tf2 = Transform3f();
+  // TODO: Need convention for normal when the centers of two objects are at same position.
+  testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, NULL);
 
-  res = solver1.shapeIntersect(s1, transform, s2, transform, NULL, NULL, NULL);
-  BOOST_CHECK(res);
-  result.clear();
-  res = (collide(&s1, transform, &s2, transform, request, result) > 0);
-  BOOST_CHECK(res);
+  tf1 = transform;
+  tf2 = transform;
+  // TODO: Need convention for normal when the centers of two objects are at same position.
+  testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, NULL);
 
-  res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(9.9, 0, 0)), NULL, NULL, NULL);
-  BOOST_CHECK(res);
-  result.clear();
-  res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(9.9, 0, 0)), request, result) > 0);
-  BOOST_CHECK(res);
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(9.9, 0, 0));
+  normal.setValue(1, 0, 0);
+  testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal);
 
-  res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(9.9, 0, 0)), NULL, NULL, NULL);
-  BOOST_CHECK(res);
-  result.clear();
-  res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(9.9, 0, 0)), request, result) > 0);
-  BOOST_CHECK(res);
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(9.9, 0, 0));
+  normal = transform.getRotation() * Vec3f(1, 0, 0);
+  testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal);
 
-  res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(10, 0, 0)), NULL, NULL, NULL);
-  BOOST_CHECK_FALSE(res);
-  result.clear();
-  res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(10, 0, 0)), request, result) > 0);
-  BOOST_CHECK_FALSE(res);
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(10.01, 0, 0));
+  testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
 
-  res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(10.01, 0, 0)), NULL, NULL, NULL);
-  BOOST_CHECK_FALSE(res);
-  result.clear();
-  res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(10.01, 0, 0)), request, result) > 0);
-  BOOST_CHECK_FALSE(res);
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(10.01, 0, 0));
+  testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
 }
 
 BOOST_AUTO_TEST_CASE(shapeIntersection_conecone)
@@ -525,62 +561,53 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_conecone)
   Cone s1(5, 10);
   Cone s2(5, 10);
 
+  Transform3f tf1;
+  Transform3f tf2;
+
   Transform3f transform;
   generateRandomTransform(extents, transform);
-  Transform3f identity;
-
-  CollisionRequest request;
-  CollisionResult result;
 
-  bool res;
+  // Vec3f point;
+  // FCL_REAL depth;
+  Vec3f normal;
 
-  res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL, NULL, NULL);
-  BOOST_CHECK(res);
-  result.clear();
-  res = (collide(&s1, Transform3f(), &s2, Transform3f(), request, result) > 0);
-  BOOST_CHECK(res);
+  tf1 = Transform3f();
+  tf2 = Transform3f();
+  // TODO: Need convention for normal when the centers of two objects are at same position.
+  testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, NULL);
 
-  res = solver1.shapeIntersect(s1, transform, s2, transform, NULL, NULL, NULL);
-  BOOST_CHECK(res);
-  result.clear();
-  res = (collide(&s1, transform, &s2, transform, request, result) > 0);
-  BOOST_CHECK(res);
+  tf1 = transform;
+  tf2 = transform;
+  // TODO: Need convention for normal when the centers of two objects are at same position.
+  testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, NULL);
 
-  res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(9.9, 0, 0)), NULL, NULL, NULL);
-  BOOST_CHECK(res);
-  result.clear();
-  res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(9.9, 0, 0)), request, result) > 0);
-  BOOST_CHECK(res);
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(9.9, 0, 0));
+  normal.setValue(1, 0, 0);
+  testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal);
 
-  res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(9.9, 0, 0)), NULL, NULL, NULL);
-  BOOST_CHECK(res);
-  result.clear();
-  res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(9.9, 0, 0)), request, result) > 0);
-  BOOST_CHECK(res);
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(9.9, 0, 0));
+  normal = transform.getRotation() * Vec3f(1, 0, 0);
+  testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal);
 
-  res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(10.001, 0, 0)), NULL, NULL, NULL);
-  BOOST_CHECK_FALSE(res);
-  result.clear();
-  res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(10.001, 0, 0)), request, result) > 0);
-  BOOST_CHECK_FALSE(res);
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(10.001, 0, 0));
+  testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
 
-  res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(10.001, 0, 0)), NULL, NULL, NULL);
-  BOOST_CHECK_FALSE(res);
-  result.clear();
-  res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(10.001, 0, 0)), request, result) > 0);
-  BOOST_CHECK_FALSE(res);
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(10.001, 0, 0));
+  testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
 
-  res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(0, 0, 9.9)), NULL, NULL, NULL);
-  BOOST_CHECK(res);
-  result.clear();
-  res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(0, 0, 9.9)), request, result) > 0);
-  BOOST_CHECK(res);
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(0, 0, 9.9));
+  normal.setValue(0, 0, 1);
+  testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal);
 
-  res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(0, 0, 9.9)), NULL, NULL, NULL);
-  BOOST_CHECK(res);
-  result.clear();
-  res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(0, 0, 9.9)), request, result) > 0);
-  BOOST_CHECK(res);
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(0, 0, 9.9));
+  normal = transform.getRotation() * Vec3f(0, 0, 1);
+  testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal);
 }
 
 BOOST_AUTO_TEST_CASE(shapeIntersection_conecylinder)
@@ -588,79 +615,146 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_conecylinder)
   Cylinder s1(5, 10);
   Cone s2(5, 10);
 
+  Transform3f tf1;
+  Transform3f tf2;
+
   Transform3f transform;
   generateRandomTransform(extents, transform);
-  Transform3f identity;
 
-  CollisionRequest request;
-  CollisionResult result;
+  // Vec3f point;
+  // FCL_REAL depth;
+  Vec3f normal;
+
+  tf1 = Transform3f();
+  tf2 = Transform3f();
+  // TODO: Need convention for normal when the centers of two objects are at same position.
+  testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, NULL);
+
+  tf1 = transform;
+  tf2 = transform;
+  // TODO: Need convention for normal when the centers of two objects are at same position.
+  testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, NULL);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(9.9, 0, 0));
+  normal.setValue(1, 0, 0);
+  testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal, false, 0.061);
+
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(9.9, 0, 0));
+  normal = transform.getRotation() * Vec3f(1, 0, 0);
+  testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal, false, 0.46);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(10.01, 0, 0));
+  testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
+
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(10.01, 0, 0));
+  testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(0, 0, 9.9));
+  normal.setValue(0, 0, 1);
+  testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal);
+
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(0, 0, 9.9));
+  normal = transform.getRotation() * Vec3f(0, 0, 1);
+  testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(0, 0, 10.01));
+  testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
+
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(0, 0, 10.01));
+  testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
+}
 
+BOOST_AUTO_TEST_CASE(shapeIntersection_spheretriangle)
+{
+  Sphere s(10);
+  Vec3f t[3];
+  t[0].setValue(20, 0, 0);
+  t[1].setValue(-20, 0, 0);
+  t[2].setValue(0, 20, 0);
+
+  Transform3f transform;
+  generateRandomTransform(extents, transform);
+
+  Vec3f normal;
   bool res;
 
-  res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL, NULL, NULL);
-  BOOST_CHECK(res);
-  result.clear();
-  res = (collide(&s1, Transform3f(), &s2, Transform3f(), request, result) > 0);
+  res = solver1.shapeTriangleIntersect(s, Transform3f(), t[0], t[1], t[2], NULL, NULL, NULL);
   BOOST_CHECK(res);
 
-  res = solver1.shapeIntersect(s1, transform, s2, transform, NULL, NULL, NULL);
-  BOOST_CHECK(res);
-  result.clear();
-  res = (collide(&s1, transform, &s2, transform, request, result) > 0);
+  res =  solver1.shapeTriangleIntersect(s, transform, t[0], t[1], t[2], transform, NULL, NULL, NULL);
   BOOST_CHECK(res);
 
-  res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(9.9, 0, 0)), NULL, NULL, NULL);
+
+  t[0].setValue(30, 0, 0);
+  t[1].setValue(9.9, -20, 0);
+  t[2].setValue(9.9, 20, 0);
+  res = solver1.shapeTriangleIntersect(s, Transform3f(), t[0], t[1], t[2], NULL, NULL, NULL);
   BOOST_CHECK(res);
-  result.clear();
-  res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(9.9, 0, 0)), request, result) > 0);
+
+  res =  solver1.shapeTriangleIntersect(s, transform, t[0], t[1], t[2], transform, NULL, NULL, NULL);
   BOOST_CHECK(res);
 
-  res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(9.9, 0, 0)), NULL, NULL, NULL);
+  res = solver1.shapeTriangleIntersect(s, Transform3f(), t[0], t[1], t[2], NULL, NULL, &normal);
   BOOST_CHECK(res);
-  result.clear();
-  res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(9.9, 0, 0)), request, result) > 0);
+  BOOST_CHECK(normal.equal(Vec3f(1, 0, 0), 1e-9));
+
+  res =  solver1.shapeTriangleIntersect(s, transform, t[0], t[1], t[2], transform, NULL, NULL, &normal);
   BOOST_CHECK(res);
+  BOOST_CHECK(normal.equal(transform.getRotation() * Vec3f(1, 0, 0), 1e-9));
+}
 
-  res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(10.01, 0, 0)), NULL, NULL, NULL);
-  BOOST_CHECK_FALSE(res);
-  result.clear();
-  res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(10, 0, 0)), request, result) > 0);
-  BOOST_CHECK_FALSE(res);
+BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacetriangle)
+{
+  Halfspace hs(Vec3f(1, 0, 0), 0);
+  Vec3f t[3];
+  t[0].setValue(20, 0, 0);
+  t[1].setValue(-20, 0, 0);
+  t[2].setValue(0, 20, 0);
 
-  res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(10.01, 0, 0)), NULL, NULL, NULL);
-  BOOST_CHECK_FALSE(res);
-  result.clear();
-  res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(10.01, 0, 0)), request, result) > 0);
-  BOOST_CHECK_FALSE(res);
+  Transform3f transform;
+  generateRandomTransform(extents, transform);
 
-  res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(0, 0, 9.9)), NULL, NULL, NULL);
+  // Vec3f point;
+  // FCL_REAL depth;
+  Vec3f normal;
+  bool res;
+
+  res = solver1.shapeTriangleIntersect(hs, Transform3f(), t[0], t[1], t[2], Transform3f(), NULL, NULL, NULL);
   BOOST_CHECK(res);
-  result.clear();
-  res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(0, 0, 9.9)), request, result) > 0);
+
+  res =  solver1.shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, NULL, NULL, NULL);
   BOOST_CHECK(res);
 
-  res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(0, 0, 9.9)), NULL, NULL, NULL);
+
+  t[0].setValue(20, 0, 0);
+  t[1].setValue(0, -20, 0);
+  t[2].setValue(0, 20, 0);
+  res = solver1.shapeTriangleIntersect(hs, Transform3f(), t[0], t[1], t[2], Transform3f(), NULL, NULL, NULL);
   BOOST_CHECK(res);
-  result.clear();
-  res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(0, 0, 9.9)), request, result) > 0);
+
+  res =  solver1.shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, NULL, NULL, NULL);
   BOOST_CHECK(res);
 
-  res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(0, 0, 10.01)), NULL, NULL, NULL);
-  BOOST_CHECK_FALSE(res);
-  result.clear();
-  res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(0, 0, 10)), request, result) > 0);
-  BOOST_CHECK_FALSE(res);
+  res = solver1.shapeTriangleIntersect(hs, Transform3f(), t[0], t[1], t[2], Transform3f(), NULL, NULL, &normal);
+  BOOST_CHECK(res);
+  BOOST_CHECK(normal.equal(Vec3f(1, 0, 0), 1e-9));
 
-  res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(0, 0, 10.01)), NULL, NULL, NULL);
-  BOOST_CHECK_FALSE(res);
-  result.clear();
-  res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(0, 0, 10.01)), request, result) > 0);
-  BOOST_CHECK_FALSE(res);
+  res =  solver1.shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, NULL, NULL, &normal);
+  BOOST_CHECK(res);
+  BOOST_CHECK(normal.equal(transform.getRotation() * Vec3f(1, 0, 0), 1e-9));
 }
 
-BOOST_AUTO_TEST_CASE(shapeIntersection_spheretriangle)
+BOOST_AUTO_TEST_CASE(shapeIntersection_planetriangle)
 {
-  Sphere s(10);
+  Plane hs(Vec3f(1, 0, 0), 0);
   Vec3f t[3];
   t[0].setValue(20, 0, 0);
   t[1].setValue(-20, 0, 0);
@@ -668,25 +762,35 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_spheretriangle)
 
   Transform3f transform;
   generateRandomTransform(extents, transform);
-  Transform3f identity;
 
+  // Vec3f point;
+  // FCL_REAL depth;
+  Vec3f normal;
   bool res;
 
-  res = solver1.shapeTriangleIntersect(s, Transform3f(), t[0], t[1], t[2], NULL, NULL, NULL);
+  res = solver1.shapeTriangleIntersect(hs, Transform3f(), t[0], t[1], t[2], Transform3f(), NULL, NULL, NULL);
   BOOST_CHECK(res);
 
-  res =  solver1.shapeTriangleIntersect(s, transform, t[0], t[1], t[2], transform, NULL, NULL, NULL);
+  res =  solver1.shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, NULL, NULL, NULL);
   BOOST_CHECK(res);
 
 
-  t[0].setValue(30, 0, 0);
-  t[1].setValue(9.9, -20, 0);
-  t[2].setValue(9.9, 20, 0);
-  res = solver1.shapeTriangleIntersect(s, Transform3f(), t[0], t[1], t[2], NULL, NULL, NULL);
+  t[0].setValue(20, 0, 0);
+  t[1].setValue(-0.1, -20, 0);
+  t[2].setValue(-0.1, 20, 0);
+  res = solver1.shapeTriangleIntersect(hs, Transform3f(), t[0], t[1], t[2], Transform3f(), NULL, NULL, NULL);
   BOOST_CHECK(res);
 
-  res =  solver1.shapeTriangleIntersect(s, transform, t[0], t[1], t[2], transform, NULL, NULL, NULL);
+  res =  solver1.shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, NULL, NULL, NULL);
+  BOOST_CHECK(res);
+
+  res = solver1.shapeTriangleIntersect(hs, Transform3f(), t[0], t[1], t[2], Transform3f(), NULL, NULL, &normal);
   BOOST_CHECK(res);
+  BOOST_CHECK(normal.equal(Vec3f(1, 0, 0), 1e-9));
+
+  res =  solver1.shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, NULL, NULL, &normal);
+  BOOST_CHECK(res);
+  BOOST_CHECK(normal.equal(transform.getRotation() * Vec3f(1, 0, 0), 1e-9));
 }
 
 BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacesphere)
@@ -774,69 +878,80 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planesphere)
   Sphere s(10);
   Plane hs(Vec3f(1, 0, 0), 0);
 
+  Transform3f tf1;
+  Transform3f tf2;
+
   Transform3f transform;
   generateRandomTransform(extents, transform);
 
   Vec3f contact;
   FCL_REAL depth;
   Vec3f normal;
-  bool res;
-
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 10) < 0.001);
-  BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)) || normal.equal(Vec3f(1, 0, 0)));
-  BOOST_CHECK(contact.equal(Vec3f(0, 0, 0)));
 
-  res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 10) < 0.001);
-  BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))) || normal.equal(transform.getQuatRotation().transform(Vec3f(1, 0, 0))));
-  BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, 0))));
+  tf1 = Transform3f();
+  tf2 = Transform3f();
+  contact.setZero();
+  depth = 10;
+  normal.setValue(1, 0, 0);  // (1, 0, 0) or (-1, 0, 0)
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(5, 0, 0)), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 5) < 0.001);
-  BOOST_CHECK(normal.equal(Vec3f(1, 0, 0)));
-  BOOST_CHECK(contact.equal(Vec3f(5, 0, 0)));
+  tf1 = transform;
+  tf2 = transform;
+  contact = transform.transform(Vec3f(0, 0, 0));
+  depth = 10;
+  normal = transform.getRotation() * Vec3f(1, 0, 0);  // (1, 0, 0) or (-1, 0, 0)
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
-  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(5, 0, 0)), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 5) < 0.001);
-  BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(1, 0, 0))));
-  BOOST_CHECK(contact.equal(transform.transform(Vec3f(5, 0, 0))));
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(5, 0, 0));
+  contact.setValue(5, 0, 0);
+  depth = 5;
+  normal.setValue(1, 0, 0);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-5, 0, 0)), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 5) < 0.001);
-  BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)));
-  BOOST_CHECK(contact.equal(Vec3f(-5, 0, 0)));
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(5, 0, 0));
+  contact = transform.transform(Vec3f(5, 0, 0));
+  depth = 5;
+  normal = transform.getRotation() * Vec3f(1, 0, 0);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
-  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-5, 0, 0)), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 5) < 0.001);
-  BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
-  BOOST_CHECK(contact.equal(transform.transform(Vec3f(-5, 0, 0))));
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(-5, 0, 0));
+  contact.setValue(-5, 0, 0);
+  depth = 5;
+  normal.setValue(-1, 0, 0);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(-5, 0, 0));
+  contact = transform.transform(Vec3f(-5, 0, 0));
+  depth = 5;
+  normal = transform.getRotation() * Vec3f(-1, 0, 0);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-10.1, 0, 0)), &contact, &depth, &normal);
-  BOOST_CHECK_FALSE(res);
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(-10.1, 0, 0));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
 
-  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-10.1, 0, 0)), &contact, &depth, &normal);
-  BOOST_CHECK_FALSE(res);
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(-10.1, 0, 0));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
 
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(10.1, 0, 0)), &contact, &depth, &normal);
-  BOOST_CHECK_FALSE(res);
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(10.1, 0, 0));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
 
-  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(10.1, 0, 0)), &contact, &depth, &normal);
-  BOOST_CHECK_FALSE(res);
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(10.1, 0, 0));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
 }
 
 BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacebox)
 {
   Box s(5, 10, 20);
   Halfspace hs(Vec3f(1, 0, 0), 0);
-  
+
   Transform3f tf1;
   Transform3f tf2;
 
@@ -920,65 +1035,78 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planebox)
 {
   Box s(5, 10, 20);
   Plane hs(Vec3f(1, 0, 0), 0);
-  
+
+  Transform3f tf1;
+  Transform3f tf2;
+
   Transform3f transform;
   generateRandomTransform(extents, transform);
 
   Vec3f contact;
   FCL_REAL depth;
   Vec3f normal;
-  bool res;
 
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
-  BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)) || normal.equal(Vec3f(1, 0, 0)));
-  BOOST_CHECK(contact.equal(Vec3f(0, 0, 0)));
+  tf1 = Transform3f();
+  tf2 = Transform3f();
+  contact.setValue(0, 0, 0);
+  depth = 2.5;
+  normal.setValue(1, 0, 0);  // (1, 0, 0) or (-1, 0, 0)
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
-  res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
-  BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))) || normal.equal(transform.getQuatRotation().transform(Vec3f(1, 0, 0))));
-  BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, 0))));
+  tf1 = transform;
+  tf2 = transform;
+  contact = transform.transform(Vec3f(0, 0, 0));
+  depth = 2.5;
+  normal = transform.getRotation() * Vec3f(1, 0, 0);  // (1, 0, 0) or (-1, 0, 0)
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(1.25, 0, 0)), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 1.25) < 0.001);
-  BOOST_CHECK(normal.equal(Vec3f(1, 0, 0)));
-  BOOST_CHECK(contact.equal(Vec3f(1.25, 0, 0)));
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(1.25, 0, 0));
+  contact.setValue(1.25, 0, 0);
+  depth = 1.25;
+  normal.setValue(1, 0, 0);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
-  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(1.25, 0, 0)), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 1.25) < 0.001);
-  BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(1, 0, 0))));
-  BOOST_CHECK(contact.equal(transform.transform(Vec3f(1.25, 0, 0))));
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(1.25, 0, 0));
+  contact = transform.transform(Vec3f(1.25, 0, 0));
+  depth = 1.25;
+  normal = transform.getRotation() * Vec3f(1, 0, 0);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-1.25, 0, 0)), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 1.25) < 0.001);
-  BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)));
-  BOOST_CHECK(contact.equal(Vec3f(-1.25, 0, 0)));
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(-1.25, 0, 0));
+  contact.setValue(-1.25, 0, 0);
+  depth = 1.25;
+  normal.setValue(-1, 0, 0);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
-  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-1.25, 0, 0)), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 1.25) < 0.001);
-  BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
-  BOOST_CHECK(contact.equal(transform.transform(Vec3f(-1.25, 0, 0))));
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(-1.25, 0, 0));
+  contact = transform.transform(Vec3f(-1.25, 0, 0));
+  depth = 1.25;
+  normal = transform.getRotation() * Vec3f(-1, 0, 0);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(2.51, 0, 0)), &contact, &depth, &normal);
-  BOOST_CHECK_FALSE(res);
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(2.51, 0, 0));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
 
-  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(2.51, 0, 0)), &contact, &depth, &normal);
-  BOOST_CHECK_FALSE(res);
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(2.51, 0, 0));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
 
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-2.51, 0, 0)), &contact, &depth, &normal);
-  BOOST_CHECK_FALSE(res);
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(-2.51, 0, 0));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
 
-  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-2.51, 0, 0)), &contact, &depth, &normal);
-  BOOST_CHECK_FALSE(res);
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(-2.51, 0, 0));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
 
-  res = solver1.shapeIntersect(s, Transform3f(transform.getQuatRotation()), hs, Transform3f(), &contact, &depth, &normal);
-  BOOST_CHECK(res);
+  tf1 = Transform3f(transform.getQuatRotation());
+  tf2 = Transform3f();
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true);
 }
 
 BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecapsule)
@@ -1007,7 +1135,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecapsule)
   tf2 = transform;
   contact = transform.transform(Vec3f(-2.5, 0, 0));
   depth = 5;
-  normal = transform.getQuatRotation().transform(Vec3f(-1, 0, 0));
+  normal = transform.getRotation() * Vec3f(-1, 0, 0);
   testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
@@ -1021,7 +1149,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecapsule)
   tf2 = transform * Transform3f(Vec3f(2.5, 0, 0));
   contact = transform.transform(Vec3f(-1.25, 0, 0));
   depth = 7.5;
-  normal = transform.getQuatRotation().transform(Vec3f(-1, 0, 0));
+  normal = transform.getRotation() * Vec3f(-1, 0, 0);
   testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
@@ -1035,7 +1163,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecapsule)
   tf2 = transform * Transform3f(Vec3f(-2.5, 0, 0));
   contact = transform.transform(Vec3f(-3.75, 0, 0));
   depth = 2.5;
-  normal = transform.getQuatRotation().transform(Vec3f(-1, 0, 0));
+  normal = transform.getRotation() * Vec3f(-1, 0, 0);
   testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
@@ -1049,7 +1177,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecapsule)
   tf2 = transform * Transform3f(Vec3f(5.1, 0, 0));
   contact = transform.transform(Vec3f(0.05, 0, 0));
   depth = 10.1;
-  normal = transform.getQuatRotation().transform(Vec3f(-1, 0, 0));
+  normal = transform.getRotation() * Vec3f(-1, 0, 0);
   testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
@@ -1076,7 +1204,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecapsule)
   tf2 = transform;
   contact = transform.transform(Vec3f(0, -2.5, 0));
   depth = 5;
-  normal = transform.getQuatRotation().transform(Vec3f(0, -1, 0));
+  normal = transform.getRotation() * Vec3f(0, -1, 0);
   testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
@@ -1090,7 +1218,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecapsule)
   tf2 = transform * Transform3f(Vec3f(0, 2.5, 0));
   contact = transform.transform(Vec3f(0, -1.25, 0));
   depth = 7.5;
-  normal = transform.getQuatRotation().transform(Vec3f(0, -1, 0));
+  normal = transform.getRotation() * Vec3f(0, -1, 0);
   testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
@@ -1104,7 +1232,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecapsule)
   tf2 = transform * Transform3f(Vec3f(0, -2.5, 0));
   contact = transform.transform(Vec3f(0, -3.75, 0));
   depth = 2.5;
-  normal = transform.getQuatRotation().transform(Vec3f(0, -1, 0));
+  normal = transform.getRotation() * Vec3f(0, -1, 0);
   testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
@@ -1118,7 +1246,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecapsule)
   tf2 = transform * Transform3f(Vec3f(0, 5.1, 0));
   contact = transform.transform(Vec3f(0, 0.05, 0));
   depth = 10.1;
-  normal = transform.getQuatRotation().transform(Vec3f(0, -1, 0));
+  normal = transform.getRotation() * Vec3f(0, -1, 0);
   testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
@@ -1145,7 +1273,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecapsule)
   tf2 = transform;
   contact = transform.transform(Vec3f(0, 0, -5));
   depth = 10;
-  normal = transform.getQuatRotation().transform(Vec3f(0, 0, -1));
+  normal = transform.getRotation() * Vec3f(0, 0, -1);
   testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
@@ -1159,7 +1287,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecapsule)
   tf2 = transform * Transform3f(Vec3f(0, 0, 2.5));
   contact = transform.transform(Vec3f(0, 0, -3.75));
   depth = 12.5;
-  normal = transform.getQuatRotation().transform(Vec3f(0, 0, -1));
+  normal = transform.getRotation() * Vec3f(0, 0, -1);
   testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
@@ -1173,7 +1301,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecapsule)
   tf2 = transform * Transform3f(Vec3f(0, 0, -2.5));
   contact = transform.transform(Vec3f(0, 0, -6.25));
   depth = 7.5;
-  normal = transform.getQuatRotation().transform(Vec3f(0, 0, -1));
+  normal = transform.getRotation() * Vec3f(0, 0, -1);
   testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
@@ -1187,7 +1315,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecapsule)
   tf2 = transform * Transform3f(Vec3f(0, 0, 10.1));
   contact = transform.transform(Vec3f(0, 0, 0.05));
   depth = 20.1;
-  normal = transform.getQuatRotation().transform(Vec3f(0, 0, -1));
+  normal = transform.getRotation() * Vec3f(0, 0, -1);
   testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
@@ -1204,167 +1332,199 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecapsule)
   Capsule s(5, 10);
   Plane hs(Vec3f(1, 0, 0), 0);
 
+  Transform3f tf1;
+  Transform3f tf2;
+
   Transform3f transform;
   generateRandomTransform(extents, transform);
 
   Vec3f contact;
   FCL_REAL depth;
   Vec3f normal;
-  bool res;
 
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 5) < 0.001);
-  BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)) || normal.equal(Vec3f(1, 0, 0)));
-  BOOST_CHECK(contact.equal(Vec3f(0, 0, 0)));
+  tf1 = Transform3f();
+  tf2 = Transform3f();
+  contact.setValue(0, 0, 0);
+  depth = 5;
+  normal.setValue(1, 0, 0);  // (1, 0, 0) or (-1, 0, 0)
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true);
 
-  res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 5) < 0.001);
-  BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))) || normal.equal(transform.getQuatRotation().transform(Vec3f(1, 0, 0))));
-  BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, 0))));
+  tf1 = transform;
+  tf2 = transform;
+  contact = transform.transform(Vec3f(0, 0, 0));
+  depth = 5;
+  normal = transform.getRotation() * Vec3f(1, 0, 0);  // (1, 0, 0) or (-1, 0, 0)
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true);
 
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(2.5, 0, 0)), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
-  BOOST_CHECK(normal.equal(Vec3f(1, 0, 0)));
-  BOOST_CHECK(contact.equal(Vec3f(2.5, 0, 0)));
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(2.5, 0, 0));
+  contact.setValue(2.5, 0, 0);
+  depth = 2.5;
+  normal.setValue(1, 0, 0);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
-  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(2.5, 0, 0)), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
-  BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(1, 0, 0))));
-  BOOST_CHECK(contact.equal(transform.transform(Vec3f(2.5, 0, 0))));
-  
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-2.5, 0, 0)), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
-  BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)));
-  BOOST_CHECK(contact.equal(Vec3f(-2.5, 0, 0)));
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(2.5, 0, 0));
+  contact = transform.transform(Vec3f(2.5, 0, 0));
+  depth = 2.5;
+  normal = transform.getRotation() * Vec3f(1, 0, 0);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
-  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-2.5, 0, 0)), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
-  BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
-  BOOST_CHECK(contact.equal(transform.transform(Vec3f(-2.5, 0, 0))));
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(-2.5, 0, 0));
+  contact.setValue(-2.5, 0, 0);
+  depth = 2.5;
+  normal.setValue(-1, 0, 0);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(5.1, 0, 0)), &contact, &depth, &normal);
-  BOOST_CHECK_FALSE(res);
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(-2.5, 0, 0));
+  contact = transform.transform(Vec3f(-2.5, 0, 0));
+  depth = 2.5;
+  normal = transform.getRotation() * Vec3f(-1, 0, 0);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
-  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(5.1, 0, 0)), &contact, &depth, &normal);
-  BOOST_CHECK_FALSE(res);
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(5.1, 0, 0));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
 
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-5.1, 0, 0)), &contact, &depth, &normal);
-  BOOST_CHECK_FALSE(res);
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(5.1, 0, 0));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
 
-  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-5.1, 0, 0)), &contact, &depth, &normal);
-  BOOST_CHECK_FALSE(res);
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(-5.1, 0, 0));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
+
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(-5.1, 0, 0));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
 
 
 
 
   hs = Plane(Vec3f(0, 1, 0), 0);
 
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 5) < 0.001);
-  BOOST_CHECK(normal.equal(Vec3f(0, -1, 0)) || normal.equal(Vec3f(0, 1, 0)));
-  BOOST_CHECK(contact.equal(Vec3f(0, 0, 0)));
+  tf1 = Transform3f();
+  tf2 = Transform3f();
+  contact.setValue(0, 0, 0);
+  depth = 5;
+  normal.setValue(0, 1, 0);  // (0, 1, 0) or (0, -1, 0)
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true);
 
-  res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 5) < 0.001);
-  BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0))) || normal.equal(transform.getQuatRotation().transform(Vec3f(0, 1, 0))));
-  BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, 0))));
+  tf1 = transform;
+  tf2 = transform;
+  contact = transform.transform(Vec3f(0, 0, 0));
+  depth = 5;
+  normal = transform.getRotation() * Vec3f(0, 1, 0);  // (0, 1, 0) or (0, -1, 0)
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true);
 
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 2.5, 0)), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
-  BOOST_CHECK(normal.equal(Vec3f(0, 1, 0)));
-  BOOST_CHECK(contact.equal(Vec3f(0, 2.5, 0)));
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(0, 2.5, 0));
+  contact.setValue(0, 2.5, 0);
+  depth = 2.5;
+  normal.setValue(0, 1, 0);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
-  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 2.5, 0)), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
-  BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 1, 0))));
-  BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 2.5, 0))));
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(0, 2.5, 0));
+  contact = transform.transform(Vec3f(0, 2.5, 0));
+  depth = 2.5;
+  normal = transform.getRotation() * Vec3f(0, 1, 0);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, -2.5, 0)), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
-  BOOST_CHECK(normal.equal(Vec3f(0, -1, 0)));
-  BOOST_CHECK(contact.equal(Vec3f(0, -2.5, 0)));
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(0, -2.5, 0));
+  contact.setValue(0, -2.5, 0);
+  depth = 2.5;
+  normal.setValue(0, -1, 0);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
-  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, -2.5, 0)), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
-  BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0))));
-  BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, -2.5, 0))));
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(0, -2.5, 0));
+  contact = transform.transform(Vec3f(0, -2.5, 0));
+  depth = 2.5;
+  normal = transform.getRotation() * Vec3f(0, -1, 0);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 5.1, 0)), &contact, &depth, &normal);
-  BOOST_CHECK_FALSE(res);
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(0, 5.1, 0));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
 
-  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 5.1, 0)), &contact, &depth, &normal);
-  BOOST_CHECK_FALSE(res);
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(0, 5.1, 0));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
 
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, -5.1, 0)), &contact, &depth, &normal);
-  BOOST_CHECK_FALSE(res);
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(0, -5.1, 0));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
 
-  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, -5.1, 0)), &contact, &depth, &normal);
-  BOOST_CHECK_FALSE(res);
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(0, -5.1, 0));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
 
 
 
 
   hs = Plane(Vec3f(0, 0, 1), 0);
 
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 10) < 0.001);
-  BOOST_CHECK(normal.equal(Vec3f(0, 0, -1)) || normal.equal(Vec3f(0, 0, 1)));
-  BOOST_CHECK(contact.equal(Vec3f(0, 0, 0)));
+  tf1 = Transform3f();
+  tf2 = Transform3f();
+  contact.setValue(0, 0, 0);
+  depth = 10;
+  normal.setValue(0, 0, 1);  // (0, 0, 1) or (0, 0, -1)
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true);
 
-  res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 10) < 0.001);
-  BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1))) || normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, 1))));
-  BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, 0))));
+  tf1 = transform;
+  tf2 = transform;
+  contact = transform.transform(Vec3f(0, 0, 0));
+  depth = 10;
+  normal = transform.getRotation() * Vec3f(0, 0, 1);  // (0, 0, 1) or (0, 0, -1)
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true);
 
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, 2.5)), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 7.5) < 0.001);
-  BOOST_CHECK(normal.equal(Vec3f(0, 0, 1)));
-  BOOST_CHECK(contact.equal(Vec3f(0, 0, 2.5)));
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(0, 0, 2.5));
+  contact.setValue(0, 0, 2.5);
+  depth = 7.5;
+  normal.setValue(0, 0, 1);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
-  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, 2.5)), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 7.5) < 0.001);
-  BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, 1))));
-  BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, 2.5))));
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(0, 0, 2.5));
+  contact = transform.transform(Vec3f(0, 0, 2.5));
+  depth = 7.5;
+  normal = transform.getRotation() * Vec3f(0, 0, 1);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, -2.5)), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 7.5) < 0.001);
-  BOOST_CHECK(normal.equal(Vec3f(0, 0, -1)));
-  BOOST_CHECK(contact.equal(Vec3f(0, 0, -2.5)));
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(0, 0, -2.5));
+  contact.setValue(0, 0, -2.5);
+  depth = 7.5;
+  normal.setValue(0, 0, -1);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
-  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, -2.5)), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 7.5) < 0.001);
-  BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1))));
-  BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, -2.5))));
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(0, 0, -2.5));
+  contact = transform.transform(Vec3f(0, 0, -2.5));
+  depth = 7.5;
+  normal = transform.getRotation() * Vec3f(0, 0, -1);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, 10.1)), &contact, &depth, &normal);
-  BOOST_CHECK_FALSE(res);
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(0, 0, 10.1));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
 
-  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, 10.1)), &contact, &depth, &normal);
-  BOOST_CHECK_FALSE(res);
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(0, 0, 10.1));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
 
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, -10.1)), &contact, &depth, &normal);
-  BOOST_CHECK_FALSE(res);
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(0, 0, -10.1));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
 
-  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, -10.1)), &contact, &depth, &normal);
-  BOOST_CHECK_FALSE(res);
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(0, 0, -10.1));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
 }
 
 BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecylinder)
@@ -1381,7 +1541,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecylinder)
   Vec3f contact;
   FCL_REAL depth;
   Vec3f normal;
-  
+
   tf1 = Transform3f();
   tf2 = Transform3f();
   contact.setValue(-2.5, 0, 0);
@@ -1393,7 +1553,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecylinder)
   tf2 = transform;
   contact = transform.transform(Vec3f(-2.5, 0, 0));
   depth = 5;
-  normal = transform.getQuatRotation().transform(Vec3f(-1, 0, 0));
+  normal = transform.getRotation() * Vec3f(-1, 0, 0);
   testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
@@ -1407,7 +1567,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecylinder)
   tf2 = transform * Transform3f(Vec3f(2.5, 0, 0));
   contact = transform.transform(Vec3f(-1.25, 0, 0));
   depth = 7.5;
-  normal = transform.getQuatRotation().transform(Vec3f(-1, 0, 0));
+  normal = transform.getRotation() * Vec3f(-1, 0, 0);
   testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
@@ -1421,7 +1581,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecylinder)
   tf2 = transform * Transform3f(Vec3f(-2.5, 0, 0));
   contact = transform.transform(Vec3f(-3.75, 0, 0));
   depth = 2.5;
-  normal = transform.getQuatRotation().transform(Vec3f(-1, 0, 0));
+  normal = transform.getRotation() * Vec3f(-1, 0, 0);
   testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
@@ -1435,7 +1595,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecylinder)
   tf2 = transform * Transform3f(Vec3f(5.1, 0, 0));
   contact = transform.transform(Vec3f(0.05, 0, 0));
   depth = 10.1;
-  normal = transform.getQuatRotation().transform(Vec3f(-1, 0, 0));
+  normal = transform.getRotation() * Vec3f(-1, 0, 0);
   testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
@@ -1462,7 +1622,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecylinder)
   tf2 = transform;
   contact = transform.transform(Vec3f(0, -2.5, 0));
   depth = 5;
-  normal = transform.getQuatRotation().transform(Vec3f(0, -1, 0));
+  normal = transform.getRotation() * Vec3f(0, -1, 0);
   testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
@@ -1476,7 +1636,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecylinder)
   tf2 = transform * Transform3f(Vec3f(0, 2.5, 0));
   contact = transform.transform(Vec3f(0, -1.25, 0));
   depth = 7.5;
-  normal = transform.getQuatRotation().transform(Vec3f(0, -1, 0));
+  normal = transform.getRotation() * Vec3f(0, -1, 0);
   testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
@@ -1490,7 +1650,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecylinder)
   tf2 = transform * Transform3f(Vec3f(0, -2.5, 0));
   contact = transform.transform(Vec3f(0, -3.75, 0));
   depth = 2.5;
-  normal = transform.getQuatRotation().transform(Vec3f(0, -1, 0));
+  normal = transform.getRotation() * Vec3f(0, -1, 0);
   testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
@@ -1504,7 +1664,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecylinder)
   tf2 = transform * Transform3f(Vec3f(0, 5.1, 0));
   contact = transform.transform(Vec3f(0, 0.05, 0));
   depth = 10.1;
-  normal = transform.getQuatRotation().transform(Vec3f(0, -1, 0));
+  normal = transform.getRotation() * Vec3f(0, -1, 0);
   testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
@@ -1531,7 +1691,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecylinder)
   tf2 = transform;
   contact = transform.transform(Vec3f(0, 0, -2.5));
   depth = 5;
-  normal = transform.getQuatRotation().transform(Vec3f(0, 0, -1));
+  normal = transform.getRotation() * Vec3f(0, 0, -1);
   testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
@@ -1545,7 +1705,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecylinder)
   tf2 = transform * Transform3f(Vec3f(0, 0, 2.5));
   contact = transform.transform(Vec3f(0, 0, -1.25));
   depth = 7.5;
-  normal = transform.getQuatRotation().transform(Vec3f(0, 0, -1));
+  normal = transform.getRotation() * Vec3f(0, 0, -1);
   testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
@@ -1559,7 +1719,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecylinder)
   tf2 = transform * Transform3f(Vec3f(0, 0, -2.5));
   contact = transform.transform(Vec3f(0, 0, -3.75));
   depth = 2.5;
-  normal = transform.getQuatRotation().transform(Vec3f(0, 0, -1));
+  normal = transform.getRotation() * Vec3f(0, 0, -1);
   testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
@@ -1573,7 +1733,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecylinder)
   tf2 = transform * Transform3f(Vec3f(0, 0, 5.1));
   contact = transform.transform(Vec3f(0, 0, 0.05));
   depth = 10.1;
-  normal = transform.getQuatRotation().transform(Vec3f(0, 0, -1));
+  normal = transform.getRotation() * Vec3f(0, 0, -1);
   testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
@@ -1590,167 +1750,199 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecylinder)
   Cylinder s(5, 10);
   Plane hs(Vec3f(1, 0, 0), 0);
 
+  Transform3f tf1;
+  Transform3f tf2;
+
   Transform3f transform;
   generateRandomTransform(extents, transform);
 
   Vec3f contact;
   FCL_REAL depth;
   Vec3f normal;
-  bool res;
 
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 5) < 0.001);
-  BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)) || normal.equal(Vec3f(1, 0, 0)));
-  BOOST_CHECK(contact.equal(Vec3f(0, 0, 0)));
+  tf1 = Transform3f();
+  tf2 = Transform3f();
+  contact.setValue(0, 0, 0);
+  depth = 5;
+  normal.setValue(1, 0, 0);  // (1, 0, 0) or (-1, 0, 0)
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true);
 
-  res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 5) < 0.001);
-  BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))) || normal.equal(transform.getQuatRotation().transform(Vec3f(1, 0, 0))));
-  BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, 0))));
+  tf1 = transform;
+  tf2 = transform;
+  contact = transform.transform(Vec3f(0, 0, 0));
+  depth = 5;
+  normal = transform.getRotation() * Vec3f(1, 0, 0);  // (1, 0, 0) or (-1, 0, 0)
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true);
 
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(2.5, 0, 0)), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
-  BOOST_CHECK(normal.equal(Vec3f(1, 0, 0)));
-  BOOST_CHECK(contact.equal(Vec3f(2.5, 0, 0)));
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(2.5, 0, 0));
+  contact.setValue(2.5, 0, 0);
+  depth = 2.5;
+  normal.setValue(1, 0, 0);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
-  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(2.5, 0, 0)), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
-  BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(1, 0, 0))));
-  BOOST_CHECK(contact.equal(transform.transform(Vec3f(2.5, 0, 0))));
-  
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-2.5, 0, 0)), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
-  BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)));
-  BOOST_CHECK(contact.equal(Vec3f(-2.5, 0, 0)));
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(2.5, 0, 0));
+  contact = transform.transform(Vec3f(2.5, 0, 0));
+  depth = 2.5;
+  normal = transform.getRotation() * Vec3f(1, 0, 0);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
-  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-2.5, 0, 0)), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
-  BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
-  BOOST_CHECK(contact.equal(transform.transform(Vec3f(-2.5, 0, 0))));
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(-2.5, 0, 0));
+  contact.setValue(-2.5, 0, 0);
+  depth = 2.5;
+  normal.setValue(-1, 0, 0);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(5.1, 0, 0)), &contact, &depth, &normal);
-  BOOST_CHECK_FALSE(res);
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(-2.5, 0, 0));
+  contact = transform.transform(Vec3f(-2.5, 0, 0));
+  depth = 2.5;
+  normal = transform.getRotation() * Vec3f(-1, 0, 0);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
-  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(5.1, 0, 0)), &contact, &depth, &normal);
-  BOOST_CHECK_FALSE(res);
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(5.1, 0, 0));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
 
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-5.1, 0, 0)), &contact, &depth, &normal);
-  BOOST_CHECK_FALSE(res);
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(5.1, 0, 0));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
 
-  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-5.1, 0, 0)), &contact, &depth, &normal);
-  BOOST_CHECK_FALSE(res);
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(-5.1, 0, 0));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
+
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(-5.1, 0, 0));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
 
 
 
 
   hs = Plane(Vec3f(0, 1, 0), 0);
 
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 5) < 0.001);
-  BOOST_CHECK(normal.equal(Vec3f(0, -1, 0)) || normal.equal(Vec3f(0, 1, 0)));
-  BOOST_CHECK(contact.equal(Vec3f(0, 0, 0)));
+  tf1 = Transform3f();
+  tf2 = Transform3f();
+  contact.setValue(0, 0, 0);
+  depth = 5;
+  normal.setValue(0, 1, 0);  // (1, 0, 0) or (-1, 0, 0)
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true);
 
-  res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 5) < 0.001);
-  BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0))) || normal.equal(transform.getQuatRotation().transform(Vec3f(0, 1, 0))));
-  BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, 0))));
+  tf1 = transform;
+  tf2 = transform;
+  contact = transform.transform(Vec3f(0, 0, 0));
+  depth = 5;
+  normal = transform.getRotation() * Vec3f(0, 1, 0);  // (1, 0, 0) or (-1, 0, 0)
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true);
 
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 2.5, 0)), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
-  BOOST_CHECK(normal.equal(Vec3f(0, 1, 0)));
-  BOOST_CHECK(contact.equal(Vec3f(0, 2.5, 0)));
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(0, 2.5, 0));
+  contact.setValue(0, 2.5, 0);
+  depth = 2.5;
+  normal.setValue(0, 1, 0);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
-  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 2.5, 0)), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
-  BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 1, 0))));
-  BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 2.5, 0))));
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(0, 2.5, 0));
+  contact = transform.transform(Vec3f(0, 2.5, 0));
+  depth = 2.5;
+  normal = transform.getRotation() * Vec3f(0, 1, 0);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, -2.5, 0)), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
-  BOOST_CHECK(normal.equal(Vec3f(0, -1, 0)));
-  BOOST_CHECK(contact.equal(Vec3f(0, -2.5, 0)));
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(0, -2.5, 0));
+  contact.setValue(0, -2.5, 0);
+  depth = 2.5;
+  normal.setValue(0, -1, 0);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
-  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, -2.5, 0)), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
-  BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0))));
-  BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, -2.5, 0))));
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(0, -2.5, 0));
+  contact = transform.transform(Vec3f(0, -2.5, 0));
+  depth = 2.5;
+  normal = transform.getRotation() * Vec3f(0, -1, 0);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 5.1, 0)), &contact, &depth, &normal);
-  BOOST_CHECK_FALSE(res);
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(0, 5.1, 0));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
 
-  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 5.1, 0)), &contact, &depth, &normal);
-  BOOST_CHECK_FALSE(res);
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(0, 5.1, 0));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
 
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, -5.1, 0)), &contact, &depth, &normal);
-  BOOST_CHECK_FALSE(res);
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(0, -5.1, 0));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
 
-  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, -5.1, 0)), &contact, &depth, &normal);
-  BOOST_CHECK_FALSE(res);
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(0, -5.1, 0));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
 
 
 
 
   hs = Plane(Vec3f(0, 0, 1), 0);
 
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 5) < 0.001);
-  BOOST_CHECK(normal.equal(Vec3f(0, 0, -1)) || normal.equal(Vec3f(0, 0, 1)));
-  BOOST_CHECK(contact.equal(Vec3f(0, 0, 0)));
+  tf1 = Transform3f();
+  tf2 = Transform3f();
+  contact.setValue(0, 0, 0);
+  depth = 5;
+  normal.setValue(0, 0, 1);  // (1, 0, 0) or (-1, 0, 0)
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true);
 
-  res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 5) < 0.001);
-  BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1))) || normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, 1))));
-  BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, 0))));
+  tf1 = transform;
+  tf2 = transform;
+  contact = transform.transform(Vec3f(0, 0, 0));
+  depth = 5;
+  normal = transform.getRotation() * Vec3f(0, 0, 1);  // (1, 0, 0) or (-1, 0, 0)
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true);
 
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, 2.5)), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
-  BOOST_CHECK(normal.equal(Vec3f(0, 0, 1)));
-  BOOST_CHECK(contact.equal(Vec3f(0, 0, 2.5)));
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(0, 0, 2.5));
+  contact.setValue(0, 0, 2.5);
+  depth = 2.5;
+  normal.setValue(0, 0, 1);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
-  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, 2.5)), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
-  BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, 1))));
-  BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, 2.5))));
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(0, 0, 2.5));
+  contact = transform.transform(Vec3f(0, 0, 2.5));
+  depth = 2.5;
+  normal = transform.getRotation() * Vec3f(0, 0, 1);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, -2.5)), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
-  BOOST_CHECK(normal.equal(Vec3f(0, 0, -1)));
-  BOOST_CHECK(contact.equal(Vec3f(0, 0, -2.5)));
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(0, 0, -2.5));
+  contact.setValue(0, 0, -2.5);
+  depth = 2.5;
+  normal.setValue(0, 0, -1);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
-  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, -2.5)), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
-  BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1))));
-  BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, -2.5))));
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(0, 0, -2.5));
+  contact = transform.transform(Vec3f(0, 0, -2.5));
+  depth = 2.5;
+  normal = transform.getRotation() * Vec3f(0, 0, -1);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, 10.1)), &contact, &depth, &normal);
-  BOOST_CHECK_FALSE(res);
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(0, 0, 10.1));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
 
-  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, 10.1)), &contact, &depth, &normal);
-  BOOST_CHECK_FALSE(res);
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(0, 0, 10.1));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
 
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, -10.1)), &contact, &depth, &normal);
-  BOOST_CHECK_FALSE(res);
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(0, 0, -10.1));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
 
-  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, -10.1)), &contact, &depth, &normal);
-  BOOST_CHECK_FALSE(res);
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(0, 0, -10.1));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
 }
 
 
@@ -1768,7 +1960,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecone)
   Vec3f contact;
   FCL_REAL depth;
   Vec3f normal;
-  
+
   tf1 = Transform3f();
   tf2 = Transform3f();
   contact.setValue(-2.5, 0, -5);
@@ -1780,7 +1972,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecone)
   tf2 = transform;
   contact = transform.transform(Vec3f(-2.5, 0, -5));
   depth = 5;
-  normal = transform.getQuatRotation().transform(Vec3f(-1, 0, 0));
+  normal = transform.getRotation() * Vec3f(-1, 0, 0);
   testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
@@ -1794,7 +1986,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecone)
   tf2 = transform * Transform3f(Vec3f(2.5, 0, 0));
   contact = transform.transform(Vec3f(-1.25, 0, -5));
   depth = 7.5;
-  normal = transform.getQuatRotation().transform(Vec3f(-1, 0, 0));
+  normal = transform.getRotation() * Vec3f(-1, 0, 0);
   testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
@@ -1808,7 +2000,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecone)
   tf2 = transform * Transform3f(Vec3f(-2.5, 0, 0));
   contact = transform.transform(Vec3f(-3.75, 0, -5));
   depth = 2.5;
-  normal = transform.getQuatRotation().transform(Vec3f(-1, 0, 0));
+  normal = transform.getRotation() * Vec3f(-1, 0, 0);
   testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
@@ -1822,7 +2014,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecone)
   tf2 = transform * Transform3f(Vec3f(5.1, 0, 0));
   contact = transform.transform(Vec3f(0.05, 0, -5));
   depth = 10.1;
-  normal = transform.getQuatRotation().transform(Vec3f(-1, 0, 0));
+  normal = transform.getRotation() * Vec3f(-1, 0, 0);
   testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
@@ -1849,7 +2041,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecone)
   tf2 = transform;
   contact = transform.transform(Vec3f(0, -2.5, -5));
   depth = 5;
-  normal = transform.getQuatRotation().transform(Vec3f(0, -1, 0));
+  normal = transform.getRotation() * Vec3f(0, -1, 0);
   testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
@@ -1863,7 +2055,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecone)
   tf2 = transform * Transform3f(Vec3f(0, 2.5, 0));
   contact = transform.transform(Vec3f(0, -1.25, -5));
   depth = 7.5;
-  normal = transform.getQuatRotation().transform(Vec3f(0, -1, 0));
+  normal = transform.getRotation() * Vec3f(0, -1, 0);
   testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
@@ -1877,7 +2069,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecone)
   tf2 = transform * Transform3f(Vec3f(0, -2.5, 0));
   contact = transform.transform(Vec3f(0, -3.75, -5));
   depth = 2.5;
-  normal = transform.getQuatRotation().transform(Vec3f(0, -1, 0));
+  normal = transform.getRotation() * Vec3f(0, -1, 0);
   testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
@@ -1891,7 +2083,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecone)
   tf2 = transform * Transform3f(Vec3f(0, 5.1, 0));
   contact = transform.transform(Vec3f(0, 0.05, -5));
   depth = 10.1;
-  normal = transform.getQuatRotation().transform(Vec3f(0, -1, 0));
+  normal = transform.getRotation() * Vec3f(0, -1, 0);
   testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
@@ -1918,7 +2110,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecone)
   tf2 = transform;
   contact = transform.transform(Vec3f(0, 0, -2.5));
   depth = 5;
-  normal = transform.getQuatRotation().transform(Vec3f(0, 0, -1));
+  normal = transform.getRotation() * Vec3f(0, 0, -1);
   testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
@@ -1932,7 +2124,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecone)
   tf2 = transform * Transform3f(Vec3f(0, 0, 2.5));
   contact = transform.transform(Vec3f(0, 0, -1.25));
   depth = 7.5;
-  normal = transform.getQuatRotation().transform(Vec3f(0, 0, -1));
+  normal = transform.getRotation() * Vec3f(0, 0, -1);
   testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
@@ -1946,7 +2138,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecone)
   tf2 = transform * Transform3f(Vec3f(0, 0, -2.5));
   contact = transform.transform(Vec3f(0, 0, -3.75));
   depth = 2.5;
-  normal = transform.getQuatRotation().transform(Vec3f(0, 0, -1));
+  normal = transform.getRotation() * Vec3f(0, 0, -1);
   testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
@@ -1960,7 +2152,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecone)
   tf2 = transform * Transform3f(Vec3f(0, 0, 5.1));
   contact = transform.transform(Vec3f(0, 0, 0.05));
   depth = 10.1;
-  normal = transform.getQuatRotation().transform(Vec3f(0, 0, -1));
+  normal = transform.getRotation() * Vec3f(0, 0, -1);
   testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
@@ -1977,173 +2169,205 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecone)
   Cone s(5, 10);
   Plane hs(Vec3f(1, 0, 0), 0);
 
+  Transform3f tf1;
+  Transform3f tf2;
+
   Transform3f transform;
   generateRandomTransform(extents, transform);
 
   Vec3f contact;
   FCL_REAL depth;
   Vec3f normal;
-  bool res;
 
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 5) < 0.001);
-  BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)) || normal.equal(Vec3f(1, 0, 0)));
-  BOOST_CHECK(contact.equal(Vec3f(0, 0, 0)));
+  tf1 = Transform3f();
+  tf2 = Transform3f();
+  contact.setValue(0, 0, 0);
+  depth = 5;
+  normal.setValue(1, 0, 0);  // (1, 0, 0) or (-1, 0, 0)
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true);
 
-  res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 5) < 0.001);
-  BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))) || normal.equal(transform.getQuatRotation().transform(Vec3f(1, 0, 0))));
-  BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, 0))));
+  tf1 = transform;
+  tf2 = transform;
+  contact = transform.transform(Vec3f(0, 0, 0));
+  depth = 5;
+  normal = transform.getRotation() * Vec3f(-1, 0, 0);  // (1, 0, 0) or (-1, 0, 0)
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true);
 
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(2.5, 0, 0)), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
-  BOOST_CHECK(normal.equal(Vec3f(1, 0, 0)));
-  BOOST_CHECK(contact.equal(Vec3f(2.5, 0, -2.5)));
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(2.5, 0, 0));
+  contact.setValue(2.5, 0, -2.5);
+  depth = 2.5;
+  normal.setValue(1, 0, 0);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
-  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(2.5, 0, 0)), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
-  BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(1, 0, 0))));
-  BOOST_CHECK(contact.equal(transform.transform(Vec3f(2.5, 0, -2.5))));
-  
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-2.5, 0, 0)), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
-  BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)));
-  BOOST_CHECK(contact.equal(Vec3f(-2.5, 0, -2.5)));
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(2.5, 0, 0));
+  contact = transform.transform(Vec3f(2.5, 0, -2.5));
+  depth = 2.5;
+  normal = transform.getRotation() * Vec3f(1, 0, 0);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
-  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-2.5, 0, 0)), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
-  BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
-  BOOST_CHECK(contact.equal(transform.transform(Vec3f(-2.5, 0, -2.5))));
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(-2.5, 0, 0));
+  contact.setValue(-2.5, 0, -2.5);
+  depth = 2.5;
+  normal.setValue(-1, 0, 0);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(5.1, 0, 0)), &contact, &depth, &normal);
-  BOOST_CHECK_FALSE(res);
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(-2.5, 0, 0));
+  contact = transform.transform(Vec3f(-2.5, 0, -2.5));
+  depth = 2.5;
+  normal = transform.getRotation() * Vec3f(-1, 0, 0);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
-  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(5.1, 0, 0)), &contact, &depth, &normal);
-  BOOST_CHECK_FALSE(res);
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(5.1, 0, 0));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
 
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-5.1, 0, 0)), &contact, &depth, &normal);
-  BOOST_CHECK_FALSE(res);
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(5.1, 0, 0));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
 
-  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-5.1, 0, 0)), &contact, &depth, &normal);
-  BOOST_CHECK_FALSE(res);
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(-5.1, 0, 0));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
+
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(-5.1, 0, 0));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
 
 
 
 
   hs = Plane(Vec3f(0, 1, 0), 0);
 
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 5) < 0.001);
-  BOOST_CHECK(normal.equal(Vec3f(0, -1, 0)) || normal.equal(Vec3f(0, 1, 0)));
-  BOOST_CHECK(contact.equal(Vec3f(0, 0, 0)));
+  tf1 = Transform3f();
+  tf2 = Transform3f();
+  contact.setValue(0, 0, 0);
+  depth = 5;
+  normal.setValue(0, 1, 0);  // (1, 0, 0) or (-1, 0, 0)
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true);
 
-  res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 5) < 0.001);
-  BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0))) || normal.equal(transform.getQuatRotation().transform(Vec3f(0, 1, 0))));
-  BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, 0))));
+  tf1 = transform;
+  tf2 = transform;
+  contact = transform.transform(Vec3f(0, 0, 0));
+  depth = 5;
+  normal = transform.getRotation() * Vec3f(0, 1, 0);  // (1, 0, 0) or (-1, 0, 0)
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true);
 
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 2.5, 0)), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
-  BOOST_CHECK(normal.equal(Vec3f(0, 1, 0)));
-  BOOST_CHECK(contact.equal(Vec3f(0, 2.5, -2.5)));
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(0, 2.5, 0));
+  contact.setValue(0, 2.5, -2.5);
+  depth = 2.5;
+  normal.setValue(0, 1, 0);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
-  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 2.5, 0)), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
-  BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 1, 0))));
-  BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 2.5, -2.5))));
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(0, 2.5, 0));
+  contact = transform.transform(Vec3f(0, 2.5, -2.5));
+  depth = 2.5;
+  normal = transform.getRotation() * Vec3f(0, 1, 0);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, -2.5, 0)), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
-  BOOST_CHECK(normal.equal(Vec3f(0, -1, 0)));
-  BOOST_CHECK(contact.equal(Vec3f(0, -2.5, -2.5)));
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(0, -2.5, 0));
+  contact.setValue(0, -2.5, -2.5);
+  depth = 2.5;
+  normal.setValue(0, -1, 0);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
-  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, -2.5, 0)), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
-  BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0))));
-  BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, -2.5, -2.5))));
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(0, -2.5, 0));
+  contact = transform.transform(Vec3f(0, -2.5, -2.5));
+  depth = 2.5;
+  normal = transform.getRotation() * Vec3f(0, -1, 0);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 5.1, 0)), &contact, &depth, &normal);
-  BOOST_CHECK_FALSE(res);
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(0, 5.1, 0));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
 
-  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 5.1, 0)), &contact, &depth, &normal);
-  BOOST_CHECK_FALSE(res);
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(0, 5.1, 0));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
 
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, -5.1, 0)), &contact, &depth, &normal);
-  BOOST_CHECK_FALSE(res);
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(0, -5.1, 0));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
 
-  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, -5.1, 0)), &contact, &depth, &normal);
-  BOOST_CHECK_FALSE(res);
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(0, -5.1, 0));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
 
 
 
 
   hs = Plane(Vec3f(0, 0, 1), 0);
 
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 5) < 0.001);
-  BOOST_CHECK(normal.equal(Vec3f(0, 0, -1)) || normal.equal(Vec3f(0, 0, 1)));
-  BOOST_CHECK(contact.equal(Vec3f(0, 0, 0)));
+  tf1 = Transform3f();
+  tf2 = Transform3f();
+  contact.setValue(0, 0, 0);
+  depth = 5;
+  normal.setValue(0, 0, 1);  // (1, 0, 0) or (-1, 0, 0)
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true);
 
-  res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 5) < 0.001);
-  BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1))) || normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, 1))));
-  BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, 0))));
+  tf1 = transform;
+  tf2 = transform;
+  contact = transform.transform(Vec3f(0, 0, 0));
+  depth = 5;
+  normal = transform.getRotation() * Vec3f(0, 0, 1);  // (1, 0, 0) or (-1, 0, 0)
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true);
 
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, 2.5)), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
-  BOOST_CHECK(normal.equal(Vec3f(0, 0, 1)));
-  BOOST_CHECK(contact.equal(Vec3f(0, 0, 2.5)));
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(0, 0, 2.5));
+  contact.setValue(0, 0, 2.5);
+  depth = 2.5;
+  normal.setValue(0, 0, 1);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
-  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, 2.5)), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
-  BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, 1))));
-  BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, 2.5))));
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(0, 0, 2.5));
+  contact = transform.transform(Vec3f(0, 0, 2.5));
+  depth = 2.5;
+  normal = transform.getRotation() * Vec3f(0, 0, 1);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, -2.5)), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
-  BOOST_CHECK(normal.equal(Vec3f(0, 0, -1)));
-  BOOST_CHECK(contact.equal(Vec3f(0, 0, -2.5)));
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(0, 0, -2.5));
+  contact.setValue(0, 0, -2.5);
+  depth = 2.5;
+  normal.setValue(0, 0, -1);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
-  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, -2.5)), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
-  BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1))));
-  BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, -2.5))));
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(0, 0, -2.5));
+  contact = transform.transform(Vec3f(0, 0, -2.5));
+  depth = 2.5;
+  normal = transform.getRotation() * Vec3f(0, 0, -1);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, 10.1)), &contact, &depth, &normal);
-  BOOST_CHECK_FALSE(res);
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(0, 0, 10.1));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
 
-  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, 10.1)), &contact, &depth, &normal);
-  BOOST_CHECK_FALSE(res);
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(0, 0, 10.1));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
 
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, -10.1)), &contact, &depth, &normal);
-  BOOST_CHECK_FALSE(res);
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(0, 0, -10.1));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
 
-  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, -10.1)), &contact, &depth, &normal);
-  BOOST_CHECK_FALSE(res);
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(0, 0, -10.1));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
 }
 
 
 
 BOOST_AUTO_TEST_CASE(shapeDistance_spheresphere)
-{  
+{
   Sphere s1(20);
   Sphere s2(10);
 
@@ -2205,7 +2429,7 @@ BOOST_AUTO_TEST_CASE(shapeDistance_spheresphere)
 }
 
 BOOST_AUTO_TEST_CASE(shapeDistance_boxbox)
-{                      
+{
   Box s1(20, 40, 50);
   Box s2(10, 10, 10);
   Vec3f closest_p1, closest_p2;
@@ -2292,7 +2516,7 @@ BOOST_AUTO_TEST_CASE(shapeDistance_boxsphere)
   res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(22.6, 0, 0)), &dist);
   BOOST_CHECK(fabs(dist - 0.1) < 0.001);
   BOOST_CHECK(res);
-  
+
   res = solver1.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(22.6, 0, 0)), &dist);
   BOOST_CHECK(fabs(dist - 0.1) < 0.05);
   BOOST_CHECK(res);
@@ -2300,7 +2524,7 @@ BOOST_AUTO_TEST_CASE(shapeDistance_boxsphere)
   res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), &dist);
   BOOST_CHECK(fabs(dist - 17.5) < 0.001);
   BOOST_CHECK(res);
-  
+
   res = solver1.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)), &dist);
   BOOST_CHECK(fabs(dist - 17.5) < 0.001);
   BOOST_CHECK(res);
@@ -2423,214 +2647,224 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spheresphere)
   Sphere s1(20);
   Sphere s2(10);
 
+  Transform3f tf1;
+  Transform3f tf2;
+
   Transform3f transform;
   generateRandomTransform(extents, transform);
 
-  CollisionRequest request;
-  CollisionResult result;
+//  Vec3f contact;
+//  FCL_REAL depth;
+  Vec3f normal;
 
-  Vec3f contact;
-  FCL_REAL penetration_depth;
-  Vec3f normal;  
-  bool res;
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(40, 0, 0));
+  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, false);
 
-  request.gjk_solver_type = GST_INDEP; // use indep GJK solver
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(40, 0, 0));
+  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, false);
 
-  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), NULL, NULL, NULL);
-  BOOST_CHECK_FALSE(res); 
-  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), &contact, &penetration_depth, &normal);
-  BOOST_CHECK_FALSE(res);
-  result.clear();
-  res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(40, 0, 0)), request, result) > 0);
-  BOOST_CHECK_FALSE(res);
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(30, 0, 0));
+  normal.setValue(1, 0, 0);
+  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
 
-  res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)), NULL, NULL, NULL);
-  BOOST_CHECK_FALSE(res);
-  res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)), &contact, &penetration_depth, &normal);
-  BOOST_CHECK_FALSE(res);
-  result.clear();
-  res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(40, 0, 0)), request, result) > 0);
-  BOOST_CHECK_FALSE(res);
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(30.01, 0, 0));
+  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, false);
 
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(30.01, 0, 0));
+  normal = transform.getRotation() * Vec3f(1, 0, 0);
+  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, false);
 
-  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(30, 0, 0)), NULL, NULL, NULL);
-  BOOST_CHECK(res);
-  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(30, 0, 0)), &contact, &penetration_depth, &normal);
-  BOOST_CHECK(res);
-  result.clear();
-  res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(30, 0, 0)), request, result) > 0);
-  BOOST_CHECK(res);
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(29.9, 0, 0));
+  normal.setValue(1, 0, 0);
+  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
 
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(29.9, 0, 0));
+  normal = transform.getRotation() * Vec3f(1, 0, 0);
+  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
 
-  res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(30.01, 0, 0)), NULL, NULL, NULL);
-  BOOST_CHECK_FALSE(res);
-  res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(30.01, 0, 0)), &contact, &penetration_depth, &normal);
-  BOOST_CHECK_FALSE(res);
-  result.clear();
-  res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(30.01, 0, 0)), request, result) > 0);
-  BOOST_CHECK_FALSE(res);
+  tf1 = Transform3f();
+  tf2 = Transform3f();
+  normal.setZero();  // If the centers of two sphere are at the same position, the normal is (0, 0, 0)
+  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
 
+  tf1 = transform;
+  tf2 = transform;
+  normal.setZero();  // If the centers of two sphere are at the same position, the normal is (0, 0, 0)
+  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
 
-  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(29.9, 0, 0)), NULL, NULL, NULL);
-  BOOST_CHECK(res);
-  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(29.9, 0, 0)), &contact, &penetration_depth, &normal);
-  BOOST_CHECK(res);
-  result.clear();
-  res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(29.9, 0, 0)), request, result) > 0);
-  BOOST_CHECK(res);
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(-29.9, 0, 0));
+  normal.setValue(-1, 0, 0);
+  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
 
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(-29.9, 0, 0));
+  normal = transform.getRotation() * Vec3f(-1, 0, 0);
+  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
 
-  res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(29.9, 0, 0)), NULL, NULL, NULL);
-  BOOST_CHECK(res);
-  res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(29.9, 0, 0)), &contact, &penetration_depth, &normal);
-  BOOST_CHECK(res);
-  result.clear();
-  res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(29.9, 0, 0)), request, result) > 0);
-  BOOST_CHECK(res);
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(-30.0, 0, 0));
+  normal.setValue(-1, 0, 0);
+  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
 
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(-30.01, 0, 0));
+  normal.setValue(-1, 0, 0);
+  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, false);
 
-  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL, NULL, NULL);
-  BOOST_CHECK(res);
-  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(), &contact, &penetration_depth, &normal);
-  BOOST_CHECK(res);
-  result.clear();
-  res = (collide(&s1, Transform3f(), &s2, Transform3f(), request, result) > 0);
-  BOOST_CHECK(res);
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(-30.01, 0, 0));
+  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, false);
+}
 
+BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_boxbox)
+{
+  Box s1(20, 40, 50);
+  Box s2(10, 10, 10);
 
-  res = solver2.shapeIntersect(s1, transform, s2, transform, NULL, NULL, NULL);
-  BOOST_CHECK(res);
-  res = solver2.shapeIntersect(s1, transform, s2, transform, &contact, &penetration_depth, &normal);
-  BOOST_CHECK(res);
-  result.clear();
-  res = (collide(&s1, transform, &s2, transform, request, result) > 0);
-  BOOST_CHECK(res);
+  Transform3f tf1;
+  Transform3f tf2;
 
+  Transform3f transform;
+  generateRandomTransform(extents, transform);
 
-  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(-29.9, 0, 0)), NULL, NULL, NULL);
-  BOOST_CHECK(res);
-  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(-29.9, 0, 0)), &contact, &penetration_depth, &normal);
-  BOOST_CHECK(res);
-  result.clear();
-  res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(-29.9, 0, 0)), request, result) > 0);
-  BOOST_CHECK(res);
+  // Vec3f point;
+  // FCL_REAL depth;
+  Vec3f normal;
 
+  Quaternion3f q;
+  q.fromAxisAngle(Vec3f(0, 0, 1), (FCL_REAL)3.140 / 6);
 
+  tf1 = Transform3f();
+  tf2 = Transform3f();
+  // TODO: Need convention for normal when the centers of two objects are at same position. The current result is (1, 0, 0).
+  normal.setValue(1, 0, 0);
+  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
 
-  res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(-29.9, 0, 0)), NULL, NULL, NULL);
-  BOOST_CHECK(res);
-  res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(-29.9, 0, 0)), &contact, &penetration_depth, &normal);
-  BOOST_CHECK(res);
-  result.clear();
-  res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(-29.9, 0, 0)), request, result) > 0);
-  BOOST_CHECK(res);
+  tf1 = transform;
+  tf2 = transform;
+  // TODO: Need convention for normal when the centers of two objects are at same position. The current result is (1, 0, 0).
+  normal.setValue(1, 0, 0);
+  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
 
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(15, 0, 0));
+  normal.setValue(1, 0, 0);
+  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
 
-  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(-30, 0, 0)), NULL, NULL, NULL);
-  BOOST_CHECK(res);
-  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(-30, 0, 0)), &contact, &penetration_depth, &normal);
-  BOOST_CHECK(res);
-  result.clear();
-  res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(-30, 0, 0)), request, result) > 0);
-  BOOST_CHECK(res);
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(15.01, 0, 0));
+  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, false);
 
-  res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(-30.01, 0, 0)), NULL, NULL, NULL);
-  BOOST_CHECK_FALSE(res);
-  res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(-30.01, 0, 0)), &contact, &penetration_depth, &normal);
-  BOOST_CHECK_FALSE(res);
-  result.clear();
-  res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(-30.01, 0, 0)), request, result) > 0);
-  BOOST_CHECK_FALSE(res);
+  tf1 = Transform3f();
+  tf2 = Transform3f(q);
+  normal = Transform3f(q).getRotation() * Vec3f(1, 0, 0);
+  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
+
+  tf1 = transform;
+  tf2 = transform * Transform3f(q);
+  normal = Transform3f(q).getRotation() * Vec3f(1, 0, 0);
+  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
 }
 
-BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_boxbox)
+BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spherebox)
 {
-  Box s1(20, 40, 50);
-  Box s2(10, 10, 10);
+  Sphere s1(20);
+  Box s2(5, 5, 5);
+
+  Transform3f tf1;
+  Transform3f tf2;
 
   Transform3f transform;
   generateRandomTransform(extents, transform);
 
-  Vec3f contact;
-  FCL_REAL penetration_depth;
-  Vec3f normal;  
-  bool res;
+  // Vec3f point;
+  // FCL_REAL depth;
+  Vec3f normal;
 
-  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL, NULL, NULL);
-  BOOST_CHECK(res);
-  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(), &contact, &penetration_depth, &normal);
-  BOOST_CHECK(res);
+  tf1 = Transform3f();
+  tf2 = Transform3f();
+  // TODO: Need convention for normal when the centers of two objects are at same position.
+  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
 
-  res = solver2.shapeIntersect(s1, transform, s2, transform, NULL, NULL, NULL);
-  BOOST_CHECK(res);
-  res = solver2.shapeIntersect(s1, transform, s2, transform, &contact, &penetration_depth, &normal);
-  BOOST_CHECK(res);
+  tf1 = transform;
+  tf2 = transform;
+  // TODO: Need convention for normal when the centers of two objects are at same position.
+  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
 
-  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(15, 0, 0)), NULL, NULL, NULL);
-  BOOST_CHECK(res);
-  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(15, 0, 0)), &contact, &penetration_depth, &normal);
-  BOOST_CHECK(res);
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(22.5, 0, 0));
+  normal.setValue(1, 0, 0);
+  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal, false, 1e-7);  // built-in GJK solver requires larger tolerance than libccd
 
-  res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(15.01, 0, 0)), NULL, NULL, NULL);
-  BOOST_CHECK_FALSE(res);
-  res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(15.01, 0, 0)), &contact, &penetration_depth, &normal);
-  BOOST_CHECK_FALSE(res);
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(22.51, 0, 0));
+  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, false);
 
-  Quaternion3f q;
-  q.fromAxisAngle(Vec3f(0, 0, 1), (FCL_REAL)3.140 / 6);
-  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(q), NULL, NULL, NULL);
-  BOOST_CHECK(res);
-  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(q), &contact, &penetration_depth, &normal);
-  BOOST_CHECK(res);
-  
-  res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(q), NULL, NULL, NULL);
-  BOOST_CHECK(res);
-  res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(q), &contact, &penetration_depth, &normal);
-  BOOST_CHECK(res);
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(22.4, 0, 0));
+  normal.setValue(1, 0, 0);
+  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal, false, 1e-2);  // built-in GJK solver requires larger tolerance than libccd
+
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(22.4, 0, 0));
+  normal = transform.getRotation() * Vec3f(1, 0, 0);
+  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
+  // built-in GJK solver returns incorrect normal.
+  // testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
 }
 
-BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spherebox)
+BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spherecapsule)
 {
   Sphere s1(20);
-  Box s2(5, 5, 5);
+  Capsule s2(5, 10);
+
+  Transform3f tf1;
+  Transform3f tf2;
 
   Transform3f transform;
   generateRandomTransform(extents, transform);
 
-  Vec3f contact;
-  FCL_REAL penetration_depth;
-  Vec3f normal;  
-  bool res;
+  // Vec3f point;
+  // FCL_REAL depth;
+  Vec3f normal;
 
-  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL, NULL, NULL);
-  BOOST_CHECK(res);
-  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(), &contact, &penetration_depth, &normal);
-  BOOST_CHECK(res);
+  tf1 = Transform3f();
+  tf2 = Transform3f();
+  // TODO: Need convention for normal when the centers of two objects are at same position.
+  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
 
-  res = solver2.shapeIntersect(s1, transform, s2, transform, NULL, NULL, NULL);
-  BOOST_CHECK(res);
-  res = solver2.shapeIntersect(s1, transform, s2, transform, &contact, &penetration_depth, &normal);
-  BOOST_CHECK(res);
+  tf1 = transform;
+  tf2 = transform;
+  // TODO: Need convention for normal when the centers of two objects are at same position.
+  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
 
-  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(22.5, 0, 0)), NULL, NULL, NULL);
-  BOOST_CHECK(res);
-  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(22.5, 0, 0)), &contact, &penetration_depth, &normal);
-  BOOST_CHECK(res);
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(24.9, 0, 0));
+  normal.setValue(1, 0, 0);
+  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
 
-  res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(22.51, 0, 0)), NULL, NULL, NULL);
-  BOOST_CHECK_FALSE(res);
-  res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(22.51, 0, 0)), &contact, &penetration_depth, &normal);
-  BOOST_CHECK_FALSE(res);
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(24.9, 0, 0));
+  normal = transform.getRotation() * Vec3f(1, 0, 0);
+  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
 
-  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(22.4, 0, 0)), NULL, NULL, NULL);
-  BOOST_CHECK(res);
-  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(22.4, 0, 0)), &contact, &penetration_depth, &normal);
-  BOOST_CHECK(res);
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(25, 0, 0));
+  normal.setValue(1, 0, 0);
+  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
 
-  res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(22.4, 0, 0)), NULL, NULL, NULL);
-  BOOST_CHECK(res);
-  res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(22.4, 0, 0)), &contact, &penetration_depth, &normal);
-  BOOST_CHECK(res);
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(25.1, 0, 0));
+  normal = transform.getRotation() * Vec3f(1, 0, 0);
+  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, false);
 }
 
 BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_cylindercylinder)
@@ -2638,43 +2872,46 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_cylindercylinder)
   Cylinder s1(5, 10);
   Cylinder s2(5, 10);
 
+  Transform3f tf1;
+  Transform3f tf2;
+
   Transform3f transform;
   generateRandomTransform(extents, transform);
 
-  Vec3f contact;
-  FCL_REAL penetration_depth;
-  Vec3f normal;  
-  bool res;
+  // Vec3f point;
+  // FCL_REAL depth;
+  Vec3f normal;
 
-  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL, NULL, NULL);
-  BOOST_CHECK(res);
-  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(), &contact, &penetration_depth, &normal);
-  BOOST_CHECK(res);
+  tf1 = Transform3f();
+  tf2 = Transform3f();
+  // TODO: Need convention for normal when the centers of two objects are at same position.
+  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
 
-  res = solver2.shapeIntersect(s1, transform, s2, transform, NULL, NULL, NULL);
-  BOOST_CHECK(res);
-  res = solver2.shapeIntersect(s1, transform, s2, transform, &contact, &penetration_depth, &normal);
-  BOOST_CHECK(res);
+  tf1 = transform;
+  tf2 = transform;
+  // TODO: Need convention for normal when the centers of two objects are at same position.
+  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
 
-  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(9.9, 0, 0)), NULL, NULL, NULL);
-  BOOST_CHECK(res);
-  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(9.9, 0, 0)), &contact, &penetration_depth, &normal);
-  BOOST_CHECK(res);
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(9.9, 0, 0));
+  normal.setValue(1, 0, 0);
+  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal, false, 3e-1);  // built-in GJK solver requires larger tolerance than libccd
 
-  res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(9.9, 0, 0)), NULL, NULL, NULL);
-  BOOST_CHECK(res);
-  res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(9.9, 0, 0)), &contact, &penetration_depth, &normal);
-  BOOST_CHECK(res);
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(9.9, 0, 0));
+  normal = transform.getRotation() * Vec3f(1, 0, 0);
+  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true);
+  // built-in GJK solver returns incorrect normal.
+  // testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
 
-  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(10, 0, 0)), NULL, NULL, NULL);
-  BOOST_CHECK(res);
-  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(10, 0, 0)), &contact, &penetration_depth, &normal);
-  BOOST_CHECK(res);
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(10, 0, 0));
+  normal.setValue(1, 0, 0);
+  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
 
-  res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(10.1, 0, 0)), NULL, NULL, NULL);
-  BOOST_CHECK_FALSE(res);
-  res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(10.1, 0, 0)), &contact, &penetration_depth, &normal);
-  BOOST_CHECK_FALSE(res);
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(10.01, 0, 0));
+  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, false);
 }
 
 BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_conecone)
@@ -2682,53 +2919,57 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_conecone)
   Cone s1(5, 10);
   Cone s2(5, 10);
 
+  Transform3f tf1;
+  Transform3f tf2;
+
   Transform3f transform;
   generateRandomTransform(extents, transform);
 
-  Vec3f contact;
-  FCL_REAL penetration_depth;
-  Vec3f normal;  
-  bool res;
+  // Vec3f point;
+  // FCL_REAL depth;
+  Vec3f normal;
 
-  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL, NULL, NULL);
-  BOOST_CHECK(res);
-  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(), &contact, &penetration_depth, &normal);
-  BOOST_CHECK(res);
+  tf1 = Transform3f();
+  tf2 = Transform3f();
+  // TODO: Need convention for normal when the centers of two objects are at same position.
+  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
 
-  res = solver2.shapeIntersect(s1, transform, s2, transform, NULL, NULL, NULL);
-  BOOST_CHECK(res);
-  res = solver2.shapeIntersect(s1, transform, s2, transform, &contact, &penetration_depth, &normal);
-  BOOST_CHECK(res);
+  tf1 = transform;
+  tf2 = transform;
+  // TODO: Need convention for normal when the centers of two objects are at same position.
+  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
 
-  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(9.9, 0, 0)), NULL, NULL, NULL);
-  BOOST_CHECK(res);
-  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(9.9, 0, 0)), &contact, &penetration_depth, &normal);
-  BOOST_CHECK(res);
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(9.9, 0, 0));
+  normal.setValue(1, 0, 0);
+  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal, false, 5.7e-1);  // built-in GJK solver requires larger tolerance than libccd
 
-  res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(9.9, 0, 0)), NULL, NULL, NULL);
-  BOOST_CHECK(res);
-  res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(9.9, 0, 0)), &contact, &penetration_depth, &normal);
-  BOOST_CHECK(res);
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(9.9, 0, 0));
+  normal = transform.getRotation() * Vec3f(1, 0, 0);
+  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
+  // built-in GJK solver returns incorrect normal.
+  // testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
 
-  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(10.1, 0, 0)), NULL, NULL, NULL);
-  BOOST_CHECK_FALSE(res);
-  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(10.1, 0, 0)), &contact, &penetration_depth, &normal);
-  BOOST_CHECK_FALSE(res);
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(10.1, 0, 0));
+  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, false);
 
-  res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(10.1, 0, 0)), NULL, NULL, NULL);
-  BOOST_CHECK_FALSE(res);
-  res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(10.1, 0, 0)), &contact, &penetration_depth, &normal);
-  BOOST_CHECK_FALSE(res);
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(10.1, 0, 0));
+  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, false);
 
-  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(0, 0, 9.9)), NULL, NULL, NULL);
-  BOOST_CHECK(res);
-  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(0, 0, 9.9)), &contact, &penetration_depth, &normal);
-  BOOST_CHECK(res);
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(0, 0, 9.9));
+  normal.setValue(0, 0, 1);
+  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
 
-  res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(0, 0, 9.9)), NULL, NULL, NULL);
-  BOOST_CHECK(res);
-  res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(0, 0, 9.9)), &contact, &penetration_depth, &normal);
-  BOOST_CHECK(res);
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(0, 0, 9.9));
+  normal = transform.getRotation() * Vec3f(0, 0, 1);
+  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
+  // built-in GJK solver returns incorrect normal.
+  // testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
 }
 
 BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_conecylinder)
@@ -2736,69 +2977,149 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_conecylinder)
   Cylinder s1(5, 10);
   Cone s2(5, 10);
 
+  Transform3f tf1;
+  Transform3f tf2;
+
   Transform3f transform;
   generateRandomTransform(extents, transform);
 
-  Vec3f contact;
-  FCL_REAL penetration_depth;
-  Vec3f normal;  
+  // Vec3f point;
+  // FCL_REAL depth;
+  Vec3f normal;
+
+  tf1 = Transform3f();
+  tf2 = Transform3f();
+  // TODO: Need convention for normal when the centers of two objects are at same position.
+  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
+
+  tf1 = transform;
+  tf2 = transform;
+  // TODO: Need convention for normal when the centers of two objects are at same position.
+  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(9.9, 0, 0));
+  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
+
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(9.9, 0, 0));
+  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(10, 0, 0));
+  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
+
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(10, 0, 0));
+  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(0, 0, 9.9));
+  normal.setValue(0, 0, 1);
+  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
+
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(0, 0, 9.9));
+  normal = transform.getRotation() * Vec3f(0, 0, 1);
+  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
+  // built-in GJK solver returns incorrect normal.
+  // testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(0, 0, 10));
+  normal.setValue(0, 0, 1);
+  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
+
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(0, 0, 10.1));
+  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, false);
+}
+
+
+BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spheretriangle)
+{
+  Sphere s(10);
+  Vec3f t[3];
+  t[0].setValue(20, 0, 0);
+  t[1].setValue(-20, 0, 0);
+  t[2].setValue(0, 20, 0);
+
+  Transform3f transform;
+  generateRandomTransform(extents, transform);
+
+  // Vec3f point;
+  // FCL_REAL depth;
+  Vec3f normal;
   bool res;
 
-  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL, NULL, NULL);
-  BOOST_CHECK(res);
-  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(), &contact, &penetration_depth, &normal);
+  res = solver2.shapeTriangleIntersect(s, Transform3f(), t[0], t[1], t[2], NULL, NULL, NULL);
   BOOST_CHECK(res);
 
-  res = solver2.shapeIntersect(s1, transform, s2, transform, NULL, NULL, NULL);
-  BOOST_CHECK(res);
-  res = solver2.shapeIntersect(s1, transform, s2, transform, &contact, &penetration_depth, &normal);
+  res =  solver2.shapeTriangleIntersect(s, transform, t[0], t[1], t[2], transform, NULL, NULL, NULL);
   BOOST_CHECK(res);
 
-  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(9.9, 0, 0)), NULL, NULL, NULL);
-  BOOST_CHECK(res);
-  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(9.9, 0, 0)), &contact, &penetration_depth, &normal);
+  t[0].setValue(30, 0, 0);
+  t[1].setValue(9.9, -20, 0);
+  t[2].setValue(9.9, 20, 0);
+  res = solver2.shapeTriangleIntersect(s, Transform3f(), t[0], t[1], t[2], NULL, NULL, NULL);
   BOOST_CHECK(res);
 
-  res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(9.9, 0, 0)), NULL, NULL, NULL);
-  BOOST_CHECK(res);
-  res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(9.9, 0, 0)), &contact, &penetration_depth, &normal);
+  res =  solver2.shapeTriangleIntersect(s, transform, t[0], t[1], t[2], transform, NULL, NULL, NULL);
   BOOST_CHECK(res);
 
-  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(10, 0, 0)), NULL, NULL, NULL);
-  BOOST_CHECK(res);
-  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(10, 0, 0)), &contact, &penetration_depth, &normal);
+  res = solver2.shapeTriangleIntersect(s, Transform3f(), t[0], t[1], t[2], NULL, NULL, &normal);
   BOOST_CHECK(res);
+  BOOST_CHECK(normal.equal(Vec3f(1, 0, 0), 1e-9));
 
-  res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(10, 0, 0)), NULL, NULL, NULL);
-  BOOST_CHECK(res);
-  res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(10, 0, 0)), &contact, &penetration_depth, &normal);
+  res =  solver2.shapeTriangleIntersect(s, transform, t[0], t[1], t[2], transform, NULL, NULL, &normal);
   BOOST_CHECK(res);
+  BOOST_CHECK(normal.equal(transform.getRotation() * Vec3f(1, 0, 0), 1e-9));
+}
 
-  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(0, 0, 9.9)), NULL, NULL, NULL);
-  BOOST_CHECK(res);
-  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(0, 0, 9.9)), &contact, &penetration_depth, &normal);
+BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_halfspacetriangle)
+{
+  Halfspace hs(Vec3f(1, 0, 0), 0);
+  Vec3f t[3];
+  t[0].setValue(20, 0, 0);
+  t[1].setValue(-20, 0, 0);
+  t[2].setValue(0, 20, 0);
+
+  Transform3f transform;
+  generateRandomTransform(extents, transform);
+
+  // Vec3f point;
+  // FCL_REAL depth;
+  Vec3f normal;
+  bool res;
+
+  res = solver2.shapeTriangleIntersect(hs, Transform3f(), t[0], t[1], t[2], Transform3f(), NULL, NULL, NULL);
   BOOST_CHECK(res);
 
-  res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(0, 0, 9.9)), NULL, NULL, NULL);
+  res =  solver2.shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, NULL, NULL, NULL);
   BOOST_CHECK(res);
-  res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(0, 0, 9.9)), &contact, &penetration_depth, &normal);
+
+
+  t[0].setValue(20, 0, 0);
+  t[1].setValue(0, -20, 0);
+  t[2].setValue(0, 20, 0);
+  res = solver1.shapeTriangleIntersect(hs, Transform3f(), t[0], t[1], t[2], Transform3f(), NULL, NULL, NULL);
   BOOST_CHECK(res);
 
-  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(0, 0, 10)), NULL, NULL, NULL);
+  res =  solver2.shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, NULL, NULL, NULL);
   BOOST_CHECK(res);
-  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(0, 0, 10)), &contact, &penetration_depth, &normal);
+
+  res = solver2.shapeTriangleIntersect(hs, Transform3f(), t[0], t[1], t[2], Transform3f(), NULL, NULL, &normal);
   BOOST_CHECK(res);
+  BOOST_CHECK(normal.equal(Vec3f(1, 0, 0), 1e-9));
 
-  res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(0, 0, 10.1)), NULL, NULL, NULL);
-  BOOST_CHECK_FALSE(res);
-  res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(0, 0, 10.1)), &contact, &penetration_depth, &normal);
-  BOOST_CHECK_FALSE(res);
+  res =  solver2.shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, NULL, NULL, &normal);
+  BOOST_CHECK(res);
+  BOOST_CHECK(normal.equal(transform.getRotation() * Vec3f(1, 0, 0), 1e-9));
 }
 
-
-BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spheretriangle)
+BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_planetriangle)
 {
-  Sphere s(10);
+  Plane hs(Vec3f(1, 0, 0), 0);
   Vec3f t[3];
   t[0].setValue(20, 0, 0);
   t[1].setValue(-20, 0, 0);
@@ -2807,29 +3128,41 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spheretriangle)
   Transform3f transform;
   generateRandomTransform(extents, transform);
 
+  // Vec3f point;
+  // FCL_REAL depth;
+  Vec3f normal;
   bool res;
 
-  res = solver2.shapeTriangleIntersect(s, Transform3f(), t[0], t[1], t[2], NULL, NULL, NULL);
+  res = solver1.shapeTriangleIntersect(hs, Transform3f(), t[0], t[1], t[2], Transform3f(), NULL, NULL, NULL);
   BOOST_CHECK(res);
 
-  res =  solver2.shapeTriangleIntersect(s, transform, t[0], t[1], t[2], transform, NULL, NULL, NULL);
+  res =  solver1.shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, NULL, NULL, NULL);
   BOOST_CHECK(res);
 
-  t[0].setValue(30, 0, 0);
-  t[1].setValue(9.9, -20, 0);
-  t[2].setValue(9.9, 20, 0);
-  res = solver2.shapeTriangleIntersect(s, Transform3f(), t[0], t[1], t[2], NULL, NULL, NULL);
+
+  t[0].setValue(20, 0, 0);
+  t[1].setValue(-0.1, -20, 0);
+  t[2].setValue(-0.1, 20, 0);
+  res = solver1.shapeTriangleIntersect(hs, Transform3f(), t[0], t[1], t[2], Transform3f(), NULL, NULL, NULL);
   BOOST_CHECK(res);
 
-  res =  solver2.shapeTriangleIntersect(s, transform, t[0], t[1], t[2], transform, NULL, NULL, NULL);
+  res =  solver2.shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, NULL, NULL, NULL);
+  BOOST_CHECK(res);
+
+  res = solver2.shapeTriangleIntersect(hs, Transform3f(), t[0], t[1], t[2], Transform3f(), NULL, NULL, &normal);
+  BOOST_CHECK(res);
+  BOOST_CHECK(normal.equal(Vec3f(1, 0, 0), 1e-9));
+
+  res =  solver2.shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, NULL, NULL, &normal);
   BOOST_CHECK(res);
+  BOOST_CHECK(normal.equal(transform.getRotation() * Vec3f(1, 0, 0), 1e-9));
 }
 
 
 
 
 BOOST_AUTO_TEST_CASE(spheresphere)
-{  
+{
   Sphere s1(20);
   Sphere s2(10);
 
@@ -2838,7 +3171,7 @@ BOOST_AUTO_TEST_CASE(spheresphere)
 
   bool res;
   FCL_REAL dist = -1;
-  
+
   res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), &dist);
   BOOST_CHECK(fabs(dist - 10) < 0.001);
   BOOST_CHECK(res);
@@ -2890,7 +3223,7 @@ BOOST_AUTO_TEST_CASE(spheresphere)
 }
 
 BOOST_AUTO_TEST_CASE(boxbox)
-{                      
+{
   Box s1(20, 40, 50);
   Box s2(10, 10, 10);
 
@@ -2947,7 +3280,7 @@ BOOST_AUTO_TEST_CASE(boxsphere)
   res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(22.6, 0, 0)), &dist);
   BOOST_CHECK(fabs(dist - 0.1) < 0.01);
   BOOST_CHECK(res);
-  
+
   res = solver2.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(22.6, 0, 0)), &dist);
   BOOST_CHECK(fabs(dist - 0.1) < 0.01);
   BOOST_CHECK(res);
@@ -2955,7 +3288,7 @@ BOOST_AUTO_TEST_CASE(boxsphere)
   res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), &dist);
   BOOST_CHECK(fabs(dist - 17.5) < 0.001);
   BOOST_CHECK(res);
-  
+
   res = solver2.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)), &dist);
   BOOST_CHECK(fabs(dist - 17.5) < 0.001);
   BOOST_CHECK(res);