diff --git a/test/test_fcl_geometric_shapes.cpp b/test/test_fcl_geometric_shapes.cpp
index 02f749292c638320b2ac486faba39ae992d8eae0..0abfc32d4df15fc780b4a313b6f21d8d89556010 100644
--- a/test/test_fcl_geometric_shapes.cpp
+++ b/test/test_fcl_geometric_shapes.cpp
@@ -371,52 +371,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)
@@ -424,51 +421,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)
@@ -476,50 +523,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)
@@ -527,62 +567,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)
@@ -590,74 +621,61 @@ 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;
 
-  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, false, 0.061);
 
-  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, false, 0.46);
 
-  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);
+  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);
 
-  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);
 
-  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);
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(0, 0, 10.01));
+  testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
 
-  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);
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(0, 0, 10.01));
+  testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
 }
 
 BOOST_AUTO_TEST_CASE(shapeIntersection_spheretriangle)