diff --git a/include/fcl/math/transform.h b/include/fcl/math/transform.h
index 9e45f8b15e09970d7fd5dce25c5172f679423e87..84fd8c4efd26ed85f4124fe467bd0c8d80dfe3bf 100644
--- a/include/fcl/math/transform.h
+++ b/include/fcl/math/transform.h
@@ -171,6 +171,12 @@ Quaternion3f conj(const Quaternion3f& q);
 /// @brief inverse of quaternion
 Quaternion3f inverse(const Quaternion3f& q);
 
+static inline std::ostream& operator << (std::ostream& o, const Quaternion3f& q)
+{
+  o << "(" << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << ")";
+  return o;
+}
+
 
 /// @brief Simple transform class used locally by InterpMotion
 class Transform3f
diff --git a/include/fcl/shape/geometric_shapes_utility.h b/include/fcl/shape/geometric_shapes_utility.h
index 3497239fe0eed86eef9162a0bcde01a07fa8a85f..a3be41a98b816da8c7070039c26857355550e9df 100644
--- a/include/fcl/shape/geometric_shapes_utility.h
+++ b/include/fcl/shape/geometric_shapes_utility.h
@@ -119,8 +119,25 @@ template<>
 void computeBV<OBB, Halfspace>(const Halfspace& s, const Transform3f& tf, OBB& bv);
 
 template<>
-void computeBV<OBB, Plane>(const Plane& s, const Transform3f& tf, OBB& bv);
+void computeBV<RSS, Halfspace>(const Halfspace& s, const Transform3f& tf, RSS& bv);
+
+template<>
+void computeBV<OBBRSS, Halfspace>(const Halfspace& s, const Transform3f& tf, OBBRSS& bv);
+
+template<>
+void computeBV<kIOS, Halfspace>(const Halfspace& s, const Transform3f& tf, kIOS& bv);
+
+template<>
+void computeBV<KDOP<16>, Halfspace>(const Halfspace& s, const Transform3f& tf, KDOP<16>& bv);
 
+template<>
+void computeBV<KDOP<18>, Halfspace>(const Halfspace& s, const Transform3f& tf, KDOP<18>& bv);
+
+template<>
+void computeBV<KDOP<24>, Halfspace>(const Halfspace& s, const Transform3f& tf, KDOP<24>& bv);
+
+template<>
+void computeBV<OBB, Plane>(const Plane& s, const Transform3f& tf, OBB& bv);
 
 template<>
 void computeBV<RSS, Plane>(const Plane& s, const Transform3f& tf, RSS& bv);
diff --git a/src/ccd/conservative_advancement.cpp b/src/ccd/conservative_advancement.cpp
index 4cf9e39e84d362276585af0811f4fe4463310ea1..41b672790013161e3fafeac3c0f5c0a597296460 100644
--- a/src/ccd/conservative_advancement.cpp
+++ b/src/ccd/conservative_advancement.cpp
@@ -724,6 +724,7 @@ ConservativeAdvancementFunctionMatrix<NarrowPhaseSolver>::ConservativeAdvancemen
   conservative_advancement_matrix[GEOM_BOX][GEOM_CYLINDER] = &ShapeConservativeAdvancement<Box, Cylinder, NarrowPhaseSolver>;
   conservative_advancement_matrix[GEOM_BOX][GEOM_CONVEX] = &ShapeConservativeAdvancement<Box, Convex, NarrowPhaseSolver>;
   conservative_advancement_matrix[GEOM_BOX][GEOM_PLANE] = &ShapeConservativeAdvancement<Box, Plane, NarrowPhaseSolver>;
+  conservative_advancement_matrix[GEOM_BOX][GEOM_HALFSPACE] = &ShapeConservativeAdvancement<Box, Halfspace, NarrowPhaseSolver>;
     
   conservative_advancement_matrix[GEOM_SPHERE][GEOM_BOX] = &ShapeConservativeAdvancement<Sphere, Box, NarrowPhaseSolver>;
   conservative_advancement_matrix[GEOM_SPHERE][GEOM_SPHERE] = &ShapeConservativeAdvancement<Sphere, Sphere, NarrowPhaseSolver>;
@@ -732,6 +733,7 @@ ConservativeAdvancementFunctionMatrix<NarrowPhaseSolver>::ConservativeAdvancemen
   conservative_advancement_matrix[GEOM_SPHERE][GEOM_CYLINDER] = &ShapeConservativeAdvancement<Sphere, Cylinder, NarrowPhaseSolver>;
   conservative_advancement_matrix[GEOM_SPHERE][GEOM_CONVEX] = &ShapeConservativeAdvancement<Sphere, Convex, NarrowPhaseSolver>;
   conservative_advancement_matrix[GEOM_SPHERE][GEOM_PLANE] = &ShapeConservativeAdvancement<Sphere, Plane, NarrowPhaseSolver>;
+  conservative_advancement_matrix[GEOM_SPHERE][GEOM_HALFSPACE] = &ShapeConservativeAdvancement<Sphere, Halfspace, NarrowPhaseSolver>;
 
   conservative_advancement_matrix[GEOM_CAPSULE][GEOM_BOX] = &ShapeConservativeAdvancement<Capsule, Box, NarrowPhaseSolver>;
   conservative_advancement_matrix[GEOM_CAPSULE][GEOM_SPHERE] = &ShapeConservativeAdvancement<Capsule, Sphere, NarrowPhaseSolver>;
@@ -740,6 +742,7 @@ ConservativeAdvancementFunctionMatrix<NarrowPhaseSolver>::ConservativeAdvancemen
   conservative_advancement_matrix[GEOM_CAPSULE][GEOM_CYLINDER] = &ShapeConservativeAdvancement<Capsule, Cylinder, NarrowPhaseSolver>;
   conservative_advancement_matrix[GEOM_CAPSULE][GEOM_CONVEX] = &ShapeConservativeAdvancement<Capsule, Convex, NarrowPhaseSolver>;
   conservative_advancement_matrix[GEOM_CAPSULE][GEOM_PLANE] = &ShapeConservativeAdvancement<Capsule, Plane, NarrowPhaseSolver>;
+  conservative_advancement_matrix[GEOM_CAPSULE][GEOM_HALFSPACE] = &ShapeConservativeAdvancement<Capsule, Halfspace, NarrowPhaseSolver>;
 
   conservative_advancement_matrix[GEOM_CONE][GEOM_BOX] = &ShapeConservativeAdvancement<Cone, Box, NarrowPhaseSolver>;
   conservative_advancement_matrix[GEOM_CONE][GEOM_SPHERE] = &ShapeConservativeAdvancement<Cone, Sphere, NarrowPhaseSolver>;
@@ -748,6 +751,7 @@ ConservativeAdvancementFunctionMatrix<NarrowPhaseSolver>::ConservativeAdvancemen
   conservative_advancement_matrix[GEOM_CONE][GEOM_CYLINDER] = &ShapeConservativeAdvancement<Cone, Cylinder, NarrowPhaseSolver>;
   conservative_advancement_matrix[GEOM_CONE][GEOM_CONVEX] = &ShapeConservativeAdvancement<Cone, Convex, NarrowPhaseSolver>;
   conservative_advancement_matrix[GEOM_CONE][GEOM_PLANE] = &ShapeConservativeAdvancement<Cone, Plane, NarrowPhaseSolver>;
+  conservative_advancement_matrix[GEOM_CONE][GEOM_HALFSPACE] = &ShapeConservativeAdvancement<Cone, Halfspace, NarrowPhaseSolver>;
 
   conservative_advancement_matrix[GEOM_CYLINDER][GEOM_BOX] = &ShapeConservativeAdvancement<Cylinder, Box, NarrowPhaseSolver>;
   conservative_advancement_matrix[GEOM_CYLINDER][GEOM_SPHERE] = &ShapeConservativeAdvancement<Cylinder, Sphere, NarrowPhaseSolver>;
@@ -756,6 +760,7 @@ ConservativeAdvancementFunctionMatrix<NarrowPhaseSolver>::ConservativeAdvancemen
   conservative_advancement_matrix[GEOM_CYLINDER][GEOM_CYLINDER] = &ShapeConservativeAdvancement<Cylinder, Cylinder, NarrowPhaseSolver>;
   conservative_advancement_matrix[GEOM_CYLINDER][GEOM_CONVEX] = &ShapeConservativeAdvancement<Cylinder, Convex, NarrowPhaseSolver>;
   conservative_advancement_matrix[GEOM_CYLINDER][GEOM_PLANE] = &ShapeConservativeAdvancement<Cylinder, Plane, NarrowPhaseSolver>;
+  conservative_advancement_matrix[GEOM_CYLINDER][GEOM_HALFSPACE] = &ShapeConservativeAdvancement<Cylinder, Halfspace, NarrowPhaseSolver>;
 
   conservative_advancement_matrix[GEOM_CONVEX][GEOM_BOX] = &ShapeConservativeAdvancement<Convex, Box, NarrowPhaseSolver>;
   conservative_advancement_matrix[GEOM_CONVEX][GEOM_SPHERE] = &ShapeConservativeAdvancement<Convex, Sphere, NarrowPhaseSolver>;
@@ -764,6 +769,7 @@ ConservativeAdvancementFunctionMatrix<NarrowPhaseSolver>::ConservativeAdvancemen
   conservative_advancement_matrix[GEOM_CONVEX][GEOM_CYLINDER] = &ShapeConservativeAdvancement<Convex, Cylinder, NarrowPhaseSolver>;
   conservative_advancement_matrix[GEOM_CONVEX][GEOM_CONVEX] = &ShapeConservativeAdvancement<Convex, Convex, NarrowPhaseSolver>;
   conservative_advancement_matrix[GEOM_CONVEX][GEOM_PLANE] = &ShapeConservativeAdvancement<Convex, Plane, NarrowPhaseSolver>;
+  conservative_advancement_matrix[GEOM_CONVEX][GEOM_HALFSPACE] = &ShapeConservativeAdvancement<Convex, Halfspace, NarrowPhaseSolver>;
 
   conservative_advancement_matrix[GEOM_PLANE][GEOM_BOX] = &ShapeConservativeAdvancement<Plane, Box, NarrowPhaseSolver>;
   conservative_advancement_matrix[GEOM_PLANE][GEOM_SPHERE] = &ShapeConservativeAdvancement<Plane, Sphere, NarrowPhaseSolver>;
@@ -772,6 +778,16 @@ ConservativeAdvancementFunctionMatrix<NarrowPhaseSolver>::ConservativeAdvancemen
   conservative_advancement_matrix[GEOM_PLANE][GEOM_CYLINDER] = &ShapeConservativeAdvancement<Plane, Cylinder, NarrowPhaseSolver>;
   conservative_advancement_matrix[GEOM_PLANE][GEOM_CONVEX] = &ShapeConservativeAdvancement<Plane, Convex, NarrowPhaseSolver>;
   conservative_advancement_matrix[GEOM_PLANE][GEOM_PLANE] = &ShapeConservativeAdvancement<Plane, Plane, NarrowPhaseSolver>;
+  conservative_advancement_matrix[GEOM_PLANE][GEOM_HALFSPACE] = &ShapeConservativeAdvancement<Plane, Halfspace, NarrowPhaseSolver>;
+
+  conservative_advancement_matrix[GEOM_HALFSPACE][GEOM_BOX] = &ShapeConservativeAdvancement<Halfspace, Box, NarrowPhaseSolver>;
+  conservative_advancement_matrix[GEOM_HALFSPACE][GEOM_SPHERE] = &ShapeConservativeAdvancement<Halfspace, Sphere, NarrowPhaseSolver>;
+  conservative_advancement_matrix[GEOM_HALFSPACE][GEOM_CAPSULE] = &ShapeConservativeAdvancement<Halfspace, Capsule, NarrowPhaseSolver>;
+  conservative_advancement_matrix[GEOM_HALFSPACE][GEOM_CONE] = &ShapeConservativeAdvancement<Halfspace, Cone, NarrowPhaseSolver>;
+  conservative_advancement_matrix[GEOM_HALFSPACE][GEOM_CYLINDER] = &ShapeConservativeAdvancement<Halfspace, Cylinder, NarrowPhaseSolver>;
+  conservative_advancement_matrix[GEOM_HALFSPACE][GEOM_CONVEX] = &ShapeConservativeAdvancement<Halfspace, Convex, NarrowPhaseSolver>;
+  conservative_advancement_matrix[GEOM_HALFSPACE][GEOM_PLANE] = &ShapeConservativeAdvancement<Halfspace, Plane, NarrowPhaseSolver>;
+  conservative_advancement_matrix[GEOM_HALFSPACE][GEOM_HALFSPACE] = &ShapeConservativeAdvancement<Halfspace, Halfspace, NarrowPhaseSolver>;
 
   conservative_advancement_matrix[BV_AABB][GEOM_BOX] = &BVHShapeConservativeAdvancement<AABB, Box, NarrowPhaseSolver>;
   conservative_advancement_matrix[BV_AABB][GEOM_SPHERE] = &BVHShapeConservativeAdvancement<AABB, Sphere, NarrowPhaseSolver>;
@@ -780,6 +796,7 @@ ConservativeAdvancementFunctionMatrix<NarrowPhaseSolver>::ConservativeAdvancemen
   conservative_advancement_matrix[BV_AABB][GEOM_CYLINDER] = &BVHShapeConservativeAdvancement<AABB, Cylinder, NarrowPhaseSolver>;
   conservative_advancement_matrix[BV_AABB][GEOM_CONVEX] = &BVHShapeConservativeAdvancement<AABB, Convex, NarrowPhaseSolver>;
   conservative_advancement_matrix[BV_AABB][GEOM_PLANE] = &BVHShapeConservativeAdvancement<AABB, Plane, NarrowPhaseSolver>;
+  conservative_advancement_matrix[BV_AABB][GEOM_HALFSPACE] = &BVHShapeConservativeAdvancement<AABB, Halfspace, NarrowPhaseSolver>;
   
   conservative_advancement_matrix[BV_OBB][GEOM_BOX] = &BVHShapeConservativeAdvancement<OBB, Box, NarrowPhaseSolver>;
   conservative_advancement_matrix[BV_OBB][GEOM_SPHERE] = &BVHShapeConservativeAdvancement<OBB, Sphere, NarrowPhaseSolver>;
@@ -788,6 +805,7 @@ ConservativeAdvancementFunctionMatrix<NarrowPhaseSolver>::ConservativeAdvancemen
   conservative_advancement_matrix[BV_OBB][GEOM_CYLINDER] = &BVHShapeConservativeAdvancement<OBB, Cylinder, NarrowPhaseSolver>;
   conservative_advancement_matrix[BV_OBB][GEOM_CONVEX] = &BVHShapeConservativeAdvancement<OBB, Convex, NarrowPhaseSolver>;
   conservative_advancement_matrix[BV_OBB][GEOM_PLANE] = &BVHShapeConservativeAdvancement<OBB, Plane, NarrowPhaseSolver>;
+  conservative_advancement_matrix[BV_OBB][GEOM_HALFSPACE] = &BVHShapeConservativeAdvancement<OBB, Halfspace, NarrowPhaseSolver>;
 
   conservative_advancement_matrix[BV_OBBRSS][GEOM_BOX] = &BVHShapeConservativeAdvancement<OBBRSS, Box, NarrowPhaseSolver>;
   conservative_advancement_matrix[BV_OBBRSS][GEOM_SPHERE] = &BVHShapeConservativeAdvancement<OBBRSS, Sphere, NarrowPhaseSolver>;
@@ -796,6 +814,7 @@ ConservativeAdvancementFunctionMatrix<NarrowPhaseSolver>::ConservativeAdvancemen
   conservative_advancement_matrix[BV_OBBRSS][GEOM_CYLINDER] = &BVHShapeConservativeAdvancement<OBBRSS, Cylinder, NarrowPhaseSolver>;
   conservative_advancement_matrix[BV_OBBRSS][GEOM_CONVEX] = &BVHShapeConservativeAdvancement<OBBRSS, Convex, NarrowPhaseSolver>;
   conservative_advancement_matrix[BV_OBBRSS][GEOM_PLANE] = &BVHShapeConservativeAdvancement<OBBRSS, Plane, NarrowPhaseSolver>;
+  conservative_advancement_matrix[BV_OBBRSS][GEOM_HALFSPACE] = &BVHShapeConservativeAdvancement<OBBRSS, Halfspace, NarrowPhaseSolver>;
 
   conservative_advancement_matrix[BV_RSS][GEOM_BOX] = &BVHShapeConservativeAdvancement<RSS, Box, NarrowPhaseSolver>;
   conservative_advancement_matrix[BV_RSS][GEOM_SPHERE] = &BVHShapeConservativeAdvancement<RSS, Sphere, NarrowPhaseSolver>;
@@ -804,6 +823,7 @@ ConservativeAdvancementFunctionMatrix<NarrowPhaseSolver>::ConservativeAdvancemen
   conservative_advancement_matrix[BV_RSS][GEOM_CYLINDER] = &BVHShapeConservativeAdvancement<RSS, Cylinder, NarrowPhaseSolver>;
   conservative_advancement_matrix[BV_RSS][GEOM_CONVEX] = &BVHShapeConservativeAdvancement<RSS, Convex, NarrowPhaseSolver>;
   conservative_advancement_matrix[BV_RSS][GEOM_PLANE] = &BVHShapeConservativeAdvancement<RSS, Plane, NarrowPhaseSolver>;
+  conservative_advancement_matrix[BV_RSS][GEOM_HALFSPACE] = &BVHShapeConservativeAdvancement<RSS, Halfspace, NarrowPhaseSolver>;
 
   conservative_advancement_matrix[BV_KDOP16][GEOM_BOX] = &BVHShapeConservativeAdvancement<KDOP<16>, Box, NarrowPhaseSolver>;
   conservative_advancement_matrix[BV_KDOP16][GEOM_SPHERE] = &BVHShapeConservativeAdvancement<KDOP<16>, Sphere, NarrowPhaseSolver>;
@@ -812,6 +832,7 @@ ConservativeAdvancementFunctionMatrix<NarrowPhaseSolver>::ConservativeAdvancemen
   conservative_advancement_matrix[BV_KDOP16][GEOM_CYLINDER] = &BVHShapeConservativeAdvancement<KDOP<16>, Cylinder, NarrowPhaseSolver>;
   conservative_advancement_matrix[BV_KDOP16][GEOM_CONVEX] = &BVHShapeConservativeAdvancement<KDOP<16>, Convex, NarrowPhaseSolver>;
   conservative_advancement_matrix[BV_KDOP16][GEOM_PLANE] = &BVHShapeConservativeAdvancement<KDOP<16>, Plane, NarrowPhaseSolver>;
+  conservative_advancement_matrix[BV_KDOP16][GEOM_HALFSPACE] = &BVHShapeConservativeAdvancement<KDOP<16>, Halfspace, NarrowPhaseSolver>;
 
   conservative_advancement_matrix[BV_KDOP18][GEOM_BOX] = &BVHShapeConservativeAdvancement<KDOP<18>, Box, NarrowPhaseSolver>;
   conservative_advancement_matrix[BV_KDOP18][GEOM_SPHERE] = &BVHShapeConservativeAdvancement<KDOP<18>, Sphere, NarrowPhaseSolver>;
@@ -820,6 +841,7 @@ ConservativeAdvancementFunctionMatrix<NarrowPhaseSolver>::ConservativeAdvancemen
   conservative_advancement_matrix[BV_KDOP18][GEOM_CYLINDER] = &BVHShapeConservativeAdvancement<KDOP<18>, Cylinder, NarrowPhaseSolver>;
   conservative_advancement_matrix[BV_KDOP18][GEOM_CONVEX] = &BVHShapeConservativeAdvancement<KDOP<18>, Convex, NarrowPhaseSolver>;
   conservative_advancement_matrix[BV_KDOP18][GEOM_PLANE] = &BVHShapeConservativeAdvancement<KDOP<18>, Plane, NarrowPhaseSolver>;
+  conservative_advancement_matrix[BV_KDOP18][GEOM_HALFSPACE] = &BVHShapeConservativeAdvancement<KDOP<18>, Halfspace, NarrowPhaseSolver>;
 
   conservative_advancement_matrix[BV_KDOP24][GEOM_BOX] = &BVHShapeConservativeAdvancement<KDOP<24>, Box, NarrowPhaseSolver>;
   conservative_advancement_matrix[BV_KDOP24][GEOM_SPHERE] = &BVHShapeConservativeAdvancement<KDOP<24>, Sphere, NarrowPhaseSolver>;
@@ -828,6 +850,7 @@ ConservativeAdvancementFunctionMatrix<NarrowPhaseSolver>::ConservativeAdvancemen
   conservative_advancement_matrix[BV_KDOP24][GEOM_CYLINDER] = &BVHShapeConservativeAdvancement<KDOP<24>, Cylinder, NarrowPhaseSolver>;
   conservative_advancement_matrix[BV_KDOP24][GEOM_CONVEX] = &BVHShapeConservativeAdvancement<KDOP<24>, Convex, NarrowPhaseSolver>;
   conservative_advancement_matrix[BV_KDOP24][GEOM_PLANE] = &BVHShapeConservativeAdvancement<KDOP<24>, Plane, NarrowPhaseSolver>;
+  conservative_advancement_matrix[BV_KDOP24][GEOM_HALFSPACE] = &BVHShapeConservativeAdvancement<KDOP<24>, Halfspace, NarrowPhaseSolver>;
 
   conservative_advancement_matrix[BV_kIOS][GEOM_BOX] = &BVHShapeConservativeAdvancement<kIOS, Box, NarrowPhaseSolver>;
   conservative_advancement_matrix[BV_kIOS][GEOM_SPHERE] = &BVHShapeConservativeAdvancement<kIOS, Sphere, NarrowPhaseSolver>;
@@ -836,6 +859,7 @@ ConservativeAdvancementFunctionMatrix<NarrowPhaseSolver>::ConservativeAdvancemen
   conservative_advancement_matrix[BV_kIOS][GEOM_CYLINDER] = &BVHShapeConservativeAdvancement<kIOS, Cylinder, NarrowPhaseSolver>;
   conservative_advancement_matrix[BV_kIOS][GEOM_CONVEX] = &BVHShapeConservativeAdvancement<kIOS, Convex, NarrowPhaseSolver>;
   conservative_advancement_matrix[BV_kIOS][GEOM_PLANE] = &BVHShapeConservativeAdvancement<kIOS, Plane, NarrowPhaseSolver>;
+  conservative_advancement_matrix[BV_kIOS][GEOM_HALFSPACE] = &BVHShapeConservativeAdvancement<kIOS, Halfspace, NarrowPhaseSolver>;
 
 
   conservative_advancement_matrix[GEOM_BOX][BV_AABB] = &ShapeBVHConservativeAdvancement<Box, AABB, NarrowPhaseSolver>;
@@ -845,6 +869,7 @@ ConservativeAdvancementFunctionMatrix<NarrowPhaseSolver>::ConservativeAdvancemen
   conservative_advancement_matrix[GEOM_CYLINDER][BV_AABB] = &ShapeBVHConservativeAdvancement<Cylinder, AABB, NarrowPhaseSolver>;
   conservative_advancement_matrix[GEOM_CONVEX][BV_AABB] = &ShapeBVHConservativeAdvancement<Convex, AABB, NarrowPhaseSolver>;
   conservative_advancement_matrix[GEOM_PLANE][BV_AABB] = &ShapeBVHConservativeAdvancement<Plane, AABB, NarrowPhaseSolver>;
+  conservative_advancement_matrix[GEOM_HALFSPACE][BV_AABB] = &ShapeBVHConservativeAdvancement<Halfspace, AABB, NarrowPhaseSolver>;
   
   conservative_advancement_matrix[GEOM_BOX][BV_OBB] = &ShapeBVHConservativeAdvancement<Box, OBB, NarrowPhaseSolver>;
   conservative_advancement_matrix[GEOM_SPHERE][BV_OBB] = &ShapeBVHConservativeAdvancement<Sphere, OBB, NarrowPhaseSolver>;
@@ -853,6 +878,7 @@ ConservativeAdvancementFunctionMatrix<NarrowPhaseSolver>::ConservativeAdvancemen
   conservative_advancement_matrix[GEOM_CYLINDER][BV_OBB] = &ShapeBVHConservativeAdvancement<Cylinder, OBB, NarrowPhaseSolver>;
   conservative_advancement_matrix[GEOM_CONVEX][BV_OBB] = &ShapeBVHConservativeAdvancement<Convex, OBB, NarrowPhaseSolver>;
   conservative_advancement_matrix[GEOM_PLANE][BV_OBB] = &ShapeBVHConservativeAdvancement<Plane, OBB, NarrowPhaseSolver>;
+  conservative_advancement_matrix[GEOM_HALFSPACE][BV_OBB] = &ShapeBVHConservativeAdvancement<Halfspace, OBB, NarrowPhaseSolver>;
 
   conservative_advancement_matrix[GEOM_BOX][BV_RSS] = &ShapeBVHConservativeAdvancement<Box, RSS, NarrowPhaseSolver>;
   conservative_advancement_matrix[GEOM_SPHERE][BV_RSS] = &ShapeBVHConservativeAdvancement<Sphere, RSS, NarrowPhaseSolver>;
@@ -861,6 +887,7 @@ ConservativeAdvancementFunctionMatrix<NarrowPhaseSolver>::ConservativeAdvancemen
   conservative_advancement_matrix[GEOM_CYLINDER][BV_RSS] = &ShapeBVHConservativeAdvancement<Cylinder, RSS, NarrowPhaseSolver>;
   conservative_advancement_matrix[GEOM_CONVEX][BV_RSS] = &ShapeBVHConservativeAdvancement<Convex, RSS, NarrowPhaseSolver>;
   conservative_advancement_matrix[GEOM_PLANE][BV_RSS] = &ShapeBVHConservativeAdvancement<Plane, RSS, NarrowPhaseSolver>;
+  conservative_advancement_matrix[GEOM_HALFSPACE][BV_RSS] = &ShapeBVHConservativeAdvancement<Halfspace, RSS, NarrowPhaseSolver>;
 
   conservative_advancement_matrix[GEOM_BOX][BV_OBBRSS] = &ShapeBVHConservativeAdvancement<Box, OBBRSS, NarrowPhaseSolver>;
   conservative_advancement_matrix[GEOM_SPHERE][BV_OBBRSS] = &ShapeBVHConservativeAdvancement<Sphere, OBBRSS, NarrowPhaseSolver>;
@@ -869,6 +896,7 @@ ConservativeAdvancementFunctionMatrix<NarrowPhaseSolver>::ConservativeAdvancemen
   conservative_advancement_matrix[GEOM_CYLINDER][BV_OBBRSS] = &ShapeBVHConservativeAdvancement<Cylinder, OBBRSS, NarrowPhaseSolver>;
   conservative_advancement_matrix[GEOM_CONVEX][BV_OBBRSS] = &ShapeBVHConservativeAdvancement<Convex, OBBRSS, NarrowPhaseSolver>;
   conservative_advancement_matrix[GEOM_PLANE][BV_OBBRSS] = &ShapeBVHConservativeAdvancement<Plane, OBBRSS, NarrowPhaseSolver>;
+  conservative_advancement_matrix[GEOM_HALFSPACE][BV_OBBRSS] = &ShapeBVHConservativeAdvancement<Halfspace, OBBRSS, NarrowPhaseSolver>;
 
   conservative_advancement_matrix[GEOM_BOX][BV_KDOP16] = &ShapeBVHConservativeAdvancement<Box, KDOP<16>, NarrowPhaseSolver>;
   conservative_advancement_matrix[GEOM_SPHERE][BV_KDOP16] = &ShapeBVHConservativeAdvancement<Sphere, KDOP<16>, NarrowPhaseSolver>;
@@ -877,6 +905,7 @@ ConservativeAdvancementFunctionMatrix<NarrowPhaseSolver>::ConservativeAdvancemen
   conservative_advancement_matrix[GEOM_CYLINDER][BV_KDOP16] = &ShapeBVHConservativeAdvancement<Cylinder, KDOP<16>, NarrowPhaseSolver>;
   conservative_advancement_matrix[GEOM_CONVEX][BV_KDOP16] = &ShapeBVHConservativeAdvancement<Convex, KDOP<16>, NarrowPhaseSolver>;
   conservative_advancement_matrix[GEOM_PLANE][BV_KDOP16] = &ShapeBVHConservativeAdvancement<Plane, KDOP<16>, NarrowPhaseSolver>;
+  conservative_advancement_matrix[GEOM_HALFSPACE][BV_KDOP16] = &ShapeBVHConservativeAdvancement<Halfspace, KDOP<16>, NarrowPhaseSolver>;
 
   conservative_advancement_matrix[GEOM_BOX][BV_KDOP18] = &ShapeBVHConservativeAdvancement<Box, KDOP<18>, NarrowPhaseSolver>;
   conservative_advancement_matrix[GEOM_SPHERE][BV_KDOP18] = &ShapeBVHConservativeAdvancement<Sphere, KDOP<18>, NarrowPhaseSolver>;
@@ -885,6 +914,7 @@ ConservativeAdvancementFunctionMatrix<NarrowPhaseSolver>::ConservativeAdvancemen
   conservative_advancement_matrix[GEOM_CYLINDER][BV_KDOP18] = &ShapeBVHConservativeAdvancement<Cylinder, KDOP<18>, NarrowPhaseSolver>;
   conservative_advancement_matrix[GEOM_CONVEX][BV_KDOP18] = &ShapeBVHConservativeAdvancement<Convex, KDOP<18>, NarrowPhaseSolver>;
   conservative_advancement_matrix[GEOM_PLANE][BV_KDOP18] = &ShapeBVHConservativeAdvancement<Plane, KDOP<18>, NarrowPhaseSolver>;
+  conservative_advancement_matrix[GEOM_HALFSPACE][BV_KDOP18] = &ShapeBVHConservativeAdvancement<Halfspace, KDOP<18>, NarrowPhaseSolver>;
 
   conservative_advancement_matrix[GEOM_BOX][BV_KDOP24] = &ShapeBVHConservativeAdvancement<Box, KDOP<24>, NarrowPhaseSolver>;
   conservative_advancement_matrix[GEOM_SPHERE][BV_KDOP24] = &ShapeBVHConservativeAdvancement<Sphere, KDOP<24>, NarrowPhaseSolver>;
@@ -893,6 +923,7 @@ ConservativeAdvancementFunctionMatrix<NarrowPhaseSolver>::ConservativeAdvancemen
   conservative_advancement_matrix[GEOM_CYLINDER][BV_KDOP24] = &ShapeBVHConservativeAdvancement<Cylinder, KDOP<24>, NarrowPhaseSolver>;
   conservative_advancement_matrix[GEOM_CONVEX][BV_KDOP24] = &ShapeBVHConservativeAdvancement<Convex, KDOP<24>, NarrowPhaseSolver>;
   conservative_advancement_matrix[GEOM_PLANE][BV_KDOP24] = &ShapeBVHConservativeAdvancement<Plane, KDOP<24>, NarrowPhaseSolver>;
+  conservative_advancement_matrix[GEOM_HALFSPACE][BV_KDOP24] = &ShapeBVHConservativeAdvancement<Halfspace, KDOP<24>, NarrowPhaseSolver>;
 
   conservative_advancement_matrix[GEOM_BOX][BV_kIOS] = &ShapeBVHConservativeAdvancement<Box, kIOS, NarrowPhaseSolver>;
   conservative_advancement_matrix[GEOM_SPHERE][BV_kIOS] = &ShapeBVHConservativeAdvancement<Sphere, kIOS, NarrowPhaseSolver>;
@@ -901,6 +932,7 @@ ConservativeAdvancementFunctionMatrix<NarrowPhaseSolver>::ConservativeAdvancemen
   conservative_advancement_matrix[GEOM_CYLINDER][BV_kIOS] = &ShapeBVHConservativeAdvancement<Cylinder, kIOS, NarrowPhaseSolver>;
   conservative_advancement_matrix[GEOM_CONVEX][BV_kIOS] = &ShapeBVHConservativeAdvancement<Convex, kIOS, NarrowPhaseSolver>;
   conservative_advancement_matrix[GEOM_PLANE][BV_kIOS] = &ShapeBVHConservativeAdvancement<Plane, kIOS, NarrowPhaseSolver>;
+  conservative_advancement_matrix[GEOM_HALFSPACE][BV_kIOS] = &ShapeBVHConservativeAdvancement<Halfspace, kIOS, NarrowPhaseSolver>;
 
   conservative_advancement_matrix[BV_AABB][BV_AABB] = &BVHConservativeAdvancement<AABB, NarrowPhaseSolver>;
   conservative_advancement_matrix[BV_OBB][BV_OBB] = &BVHConservativeAdvancement<OBB, NarrowPhaseSolver>;
diff --git a/src/collision_func_matrix.cpp b/src/collision_func_matrix.cpp
index 0b3c2db99c551b472410e1cdfc895cf3771b8208..9a2e19dfa781c472943f2f374a05dafe42f0f763 100644
--- a/src/collision_func_matrix.cpp
+++ b/src/collision_func_matrix.cpp
@@ -459,6 +459,7 @@ CollisionFunctionMatrix<NarrowPhaseSolver>::CollisionFunctionMatrix()
   collision_matrix[GEOM_BOX][GEOM_CYLINDER] = &ShapeShapeCollide<Box, Cylinder, NarrowPhaseSolver>;
   collision_matrix[GEOM_BOX][GEOM_CONVEX] = &ShapeShapeCollide<Box, Convex, NarrowPhaseSolver>;
   collision_matrix[GEOM_BOX][GEOM_PLANE] = &ShapeShapeCollide<Box, Plane, NarrowPhaseSolver>;
+  collision_matrix[GEOM_BOX][GEOM_HALFSPACE] = &ShapeShapeCollide<Box, Halfspace, NarrowPhaseSolver>;
 
   collision_matrix[GEOM_SPHERE][GEOM_BOX] = &ShapeShapeCollide<Sphere, Box, NarrowPhaseSolver>;
   collision_matrix[GEOM_SPHERE][GEOM_SPHERE] = &ShapeShapeCollide<Sphere, Sphere, NarrowPhaseSolver>;
@@ -467,6 +468,7 @@ CollisionFunctionMatrix<NarrowPhaseSolver>::CollisionFunctionMatrix()
   collision_matrix[GEOM_SPHERE][GEOM_CYLINDER] = &ShapeShapeCollide<Sphere, Cylinder, NarrowPhaseSolver>;
   collision_matrix[GEOM_SPHERE][GEOM_CONVEX] = &ShapeShapeCollide<Sphere, Convex, NarrowPhaseSolver>;
   collision_matrix[GEOM_SPHERE][GEOM_PLANE] = &ShapeShapeCollide<Sphere, Plane, NarrowPhaseSolver>;
+  collision_matrix[GEOM_SPHERE][GEOM_HALFSPACE] = &ShapeShapeCollide<Sphere, Halfspace, NarrowPhaseSolver>;
 
   collision_matrix[GEOM_CAPSULE][GEOM_BOX] = &ShapeShapeCollide<Capsule, Box, NarrowPhaseSolver>;
   collision_matrix[GEOM_CAPSULE][GEOM_SPHERE] = &ShapeShapeCollide<Capsule, Sphere, NarrowPhaseSolver>;
@@ -475,6 +477,7 @@ CollisionFunctionMatrix<NarrowPhaseSolver>::CollisionFunctionMatrix()
   collision_matrix[GEOM_CAPSULE][GEOM_CYLINDER] = &ShapeShapeCollide<Capsule, Cylinder, NarrowPhaseSolver>;
   collision_matrix[GEOM_CAPSULE][GEOM_CONVEX] = &ShapeShapeCollide<Capsule, Convex, NarrowPhaseSolver>;
   collision_matrix[GEOM_CAPSULE][GEOM_PLANE] = &ShapeShapeCollide<Capsule, Plane, NarrowPhaseSolver>;
+  collision_matrix[GEOM_CAPSULE][GEOM_HALFSPACE] = &ShapeShapeCollide<Capsule, Halfspace, NarrowPhaseSolver>;
 
   collision_matrix[GEOM_CONE][GEOM_BOX] = &ShapeShapeCollide<Cone, Box, NarrowPhaseSolver>;
   collision_matrix[GEOM_CONE][GEOM_SPHERE] = &ShapeShapeCollide<Cone, Sphere, NarrowPhaseSolver>;
@@ -483,6 +486,7 @@ CollisionFunctionMatrix<NarrowPhaseSolver>::CollisionFunctionMatrix()
   collision_matrix[GEOM_CONE][GEOM_CYLINDER] = &ShapeShapeCollide<Cone, Cylinder, NarrowPhaseSolver>;
   collision_matrix[GEOM_CONE][GEOM_CONVEX] = &ShapeShapeCollide<Cone, Convex, NarrowPhaseSolver>;
   collision_matrix[GEOM_CONE][GEOM_PLANE] = &ShapeShapeCollide<Cone, Plane, NarrowPhaseSolver>;
+  collision_matrix[GEOM_CONE][GEOM_HALFSPACE] = &ShapeShapeCollide<Cone, Halfspace, NarrowPhaseSolver>;
 
   collision_matrix[GEOM_CYLINDER][GEOM_BOX] = &ShapeShapeCollide<Cylinder, Box, NarrowPhaseSolver>;
   collision_matrix[GEOM_CYLINDER][GEOM_SPHERE] = &ShapeShapeCollide<Cylinder, Sphere, NarrowPhaseSolver>;
@@ -491,6 +495,7 @@ CollisionFunctionMatrix<NarrowPhaseSolver>::CollisionFunctionMatrix()
   collision_matrix[GEOM_CYLINDER][GEOM_CYLINDER] = &ShapeShapeCollide<Cylinder, Cylinder, NarrowPhaseSolver>;
   collision_matrix[GEOM_CYLINDER][GEOM_CONVEX] = &ShapeShapeCollide<Cylinder, Convex, NarrowPhaseSolver>;
   collision_matrix[GEOM_CYLINDER][GEOM_PLANE] = &ShapeShapeCollide<Cylinder, Plane, NarrowPhaseSolver>;
+  collision_matrix[GEOM_CYLINDER][GEOM_HALFSPACE] = &ShapeShapeCollide<Cylinder, Halfspace, NarrowPhaseSolver>;
 
   collision_matrix[GEOM_CONVEX][GEOM_BOX] = &ShapeShapeCollide<Convex, Box, NarrowPhaseSolver>;
   collision_matrix[GEOM_CONVEX][GEOM_SPHERE] = &ShapeShapeCollide<Convex, Sphere, NarrowPhaseSolver>;
@@ -499,6 +504,7 @@ CollisionFunctionMatrix<NarrowPhaseSolver>::CollisionFunctionMatrix()
   collision_matrix[GEOM_CONVEX][GEOM_CYLINDER] = &ShapeShapeCollide<Convex, Cylinder, NarrowPhaseSolver>;
   collision_matrix[GEOM_CONVEX][GEOM_CONVEX] = &ShapeShapeCollide<Convex, Convex, NarrowPhaseSolver>;
   collision_matrix[GEOM_CONVEX][GEOM_PLANE] = &ShapeShapeCollide<Convex, Plane, NarrowPhaseSolver>;
+  collision_matrix[GEOM_CONVEX][GEOM_HALFSPACE] = &ShapeShapeCollide<Convex, Halfspace, NarrowPhaseSolver>;
 
   collision_matrix[GEOM_PLANE][GEOM_BOX] = &ShapeShapeCollide<Plane, Box, NarrowPhaseSolver>;
   collision_matrix[GEOM_PLANE][GEOM_SPHERE] = &ShapeShapeCollide<Plane, Sphere, NarrowPhaseSolver>;
@@ -507,6 +513,16 @@ CollisionFunctionMatrix<NarrowPhaseSolver>::CollisionFunctionMatrix()
   collision_matrix[GEOM_PLANE][GEOM_CYLINDER] = &ShapeShapeCollide<Plane, Cylinder, NarrowPhaseSolver>;
   collision_matrix[GEOM_PLANE][GEOM_CONVEX] = &ShapeShapeCollide<Plane, Convex, NarrowPhaseSolver>;
   collision_matrix[GEOM_PLANE][GEOM_PLANE] = &ShapeShapeCollide<Plane, Plane, NarrowPhaseSolver>;
+  collision_matrix[GEOM_PLANE][GEOM_HALFSPACE] = &ShapeShapeCollide<Plane, Halfspace, NarrowPhaseSolver>;
+
+  collision_matrix[GEOM_HALFSPACE][GEOM_BOX] = &ShapeShapeCollide<Halfspace, Box, NarrowPhaseSolver>;
+  collision_matrix[GEOM_HALFSPACE][GEOM_SPHERE] = &ShapeShapeCollide<Halfspace, Sphere, NarrowPhaseSolver>;
+  collision_matrix[GEOM_HALFSPACE][GEOM_CAPSULE] = &ShapeShapeCollide<Halfspace, Capsule, NarrowPhaseSolver>;
+  collision_matrix[GEOM_HALFSPACE][GEOM_CONE] = &ShapeShapeCollide<Halfspace, Cone, NarrowPhaseSolver>;
+  collision_matrix[GEOM_HALFSPACE][GEOM_CYLINDER] = &ShapeShapeCollide<Halfspace, Cylinder, NarrowPhaseSolver>;
+  collision_matrix[GEOM_HALFSPACE][GEOM_CONVEX] = &ShapeShapeCollide<Halfspace, Convex, NarrowPhaseSolver>;
+  collision_matrix[GEOM_HALFSPACE][GEOM_PLANE] = &ShapeShapeCollide<Halfspace, Plane, NarrowPhaseSolver>;
+  collision_matrix[GEOM_HALFSPACE][GEOM_HALFSPACE] = &ShapeShapeCollide<Halfspace, Halfspace, NarrowPhaseSolver>;
 
   collision_matrix[BV_AABB][GEOM_BOX] = &BVHShapeCollider<AABB, Box, NarrowPhaseSolver>::collide;
   collision_matrix[BV_AABB][GEOM_SPHERE] = &BVHShapeCollider<AABB, Sphere, NarrowPhaseSolver>::collide;
@@ -515,6 +531,7 @@ CollisionFunctionMatrix<NarrowPhaseSolver>::CollisionFunctionMatrix()
   collision_matrix[BV_AABB][GEOM_CYLINDER] = &BVHShapeCollider<AABB, Cylinder, NarrowPhaseSolver>::collide;
   collision_matrix[BV_AABB][GEOM_CONVEX] = &BVHShapeCollider<AABB, Convex, NarrowPhaseSolver>::collide;
   collision_matrix[BV_AABB][GEOM_PLANE] = &BVHShapeCollider<AABB, Plane, NarrowPhaseSolver>::collide;
+  collision_matrix[BV_AABB][GEOM_HALFSPACE] = &BVHShapeCollider<AABB, Halfspace, NarrowPhaseSolver>::collide;
 
   collision_matrix[BV_OBB][GEOM_BOX] = &BVHShapeCollider<OBB, Box, NarrowPhaseSolver>::collide;
   collision_matrix[BV_OBB][GEOM_SPHERE] = &BVHShapeCollider<OBB, Sphere, NarrowPhaseSolver>::collide;
@@ -523,6 +540,7 @@ CollisionFunctionMatrix<NarrowPhaseSolver>::CollisionFunctionMatrix()
   collision_matrix[BV_OBB][GEOM_CYLINDER] = &BVHShapeCollider<OBB, Cylinder, NarrowPhaseSolver>::collide;
   collision_matrix[BV_OBB][GEOM_CONVEX] = &BVHShapeCollider<OBB, Convex, NarrowPhaseSolver>::collide;
   collision_matrix[BV_OBB][GEOM_PLANE] = &BVHShapeCollider<OBB, Plane, NarrowPhaseSolver>::collide;
+  collision_matrix[BV_OBB][GEOM_HALFSPACE] = &BVHShapeCollider<OBB, Halfspace, NarrowPhaseSolver>::collide;
 
   collision_matrix[BV_RSS][GEOM_BOX] = &BVHShapeCollider<RSS, Box, NarrowPhaseSolver>::collide;
   collision_matrix[BV_RSS][GEOM_SPHERE] = &BVHShapeCollider<RSS, Sphere, NarrowPhaseSolver>::collide;
@@ -531,6 +549,7 @@ CollisionFunctionMatrix<NarrowPhaseSolver>::CollisionFunctionMatrix()
   collision_matrix[BV_RSS][GEOM_CYLINDER] = &BVHShapeCollider<RSS, Cylinder, NarrowPhaseSolver>::collide;
   collision_matrix[BV_RSS][GEOM_CONVEX] = &BVHShapeCollider<RSS, Convex, NarrowPhaseSolver>::collide;
   collision_matrix[BV_RSS][GEOM_PLANE] = &BVHShapeCollider<RSS, Plane, NarrowPhaseSolver>::collide;
+  collision_matrix[BV_RSS][GEOM_HALFSPACE] = &BVHShapeCollider<RSS, Halfspace, NarrowPhaseSolver>::collide;
 
   collision_matrix[BV_KDOP16][GEOM_BOX] = &BVHShapeCollider<KDOP<16>, Box, NarrowPhaseSolver>::collide;
   collision_matrix[BV_KDOP16][GEOM_SPHERE] = &BVHShapeCollider<KDOP<16>, Sphere, NarrowPhaseSolver>::collide;
@@ -539,6 +558,7 @@ CollisionFunctionMatrix<NarrowPhaseSolver>::CollisionFunctionMatrix()
   collision_matrix[BV_KDOP16][GEOM_CYLINDER] = &BVHShapeCollider<KDOP<16>, Cylinder, NarrowPhaseSolver>::collide;
   collision_matrix[BV_KDOP16][GEOM_CONVEX] = &BVHShapeCollider<KDOP<16>, Convex, NarrowPhaseSolver>::collide;
   collision_matrix[BV_KDOP16][GEOM_PLANE] = &BVHShapeCollider<KDOP<16>, Plane, NarrowPhaseSolver>::collide;
+  collision_matrix[BV_KDOP16][GEOM_HALFSPACE] = &BVHShapeCollider<KDOP<16>, Halfspace, NarrowPhaseSolver>::collide;
 
   collision_matrix[BV_KDOP18][GEOM_BOX] = &BVHShapeCollider<KDOP<18>, Box, NarrowPhaseSolver>::collide;
   collision_matrix[BV_KDOP18][GEOM_SPHERE] = &BVHShapeCollider<KDOP<18>, Sphere, NarrowPhaseSolver>::collide;
@@ -547,6 +567,7 @@ CollisionFunctionMatrix<NarrowPhaseSolver>::CollisionFunctionMatrix()
   collision_matrix[BV_KDOP18][GEOM_CYLINDER] = &BVHShapeCollider<KDOP<18>, Cylinder, NarrowPhaseSolver>::collide;
   collision_matrix[BV_KDOP18][GEOM_CONVEX] = &BVHShapeCollider<KDOP<18>, Convex, NarrowPhaseSolver>::collide;
   collision_matrix[BV_KDOP18][GEOM_PLANE] = &BVHShapeCollider<KDOP<18>, Plane, NarrowPhaseSolver>::collide;
+  collision_matrix[BV_KDOP18][GEOM_HALFSPACE] = &BVHShapeCollider<KDOP<18>, Halfspace, NarrowPhaseSolver>::collide;
 
   collision_matrix[BV_KDOP24][GEOM_BOX] = &BVHShapeCollider<KDOP<24>, Box, NarrowPhaseSolver>::collide;
   collision_matrix[BV_KDOP24][GEOM_SPHERE] = &BVHShapeCollider<KDOP<24>, Sphere, NarrowPhaseSolver>::collide;
@@ -555,6 +576,7 @@ CollisionFunctionMatrix<NarrowPhaseSolver>::CollisionFunctionMatrix()
   collision_matrix[BV_KDOP24][GEOM_CYLINDER] = &BVHShapeCollider<KDOP<24>, Cylinder, NarrowPhaseSolver>::collide;
   collision_matrix[BV_KDOP24][GEOM_CONVEX] = &BVHShapeCollider<KDOP<24>, Convex, NarrowPhaseSolver>::collide;
   collision_matrix[BV_KDOP24][GEOM_PLANE] = &BVHShapeCollider<KDOP<24>, Plane, NarrowPhaseSolver>::collide;
+  collision_matrix[BV_KDOP24][GEOM_HALFSPACE] = &BVHShapeCollider<KDOP<24>, Halfspace, NarrowPhaseSolver>::collide;
 
   collision_matrix[BV_kIOS][GEOM_BOX] = &BVHShapeCollider<kIOS, Box, NarrowPhaseSolver>::collide;
   collision_matrix[BV_kIOS][GEOM_SPHERE] = &BVHShapeCollider<kIOS, Sphere, NarrowPhaseSolver>::collide;
@@ -563,6 +585,7 @@ CollisionFunctionMatrix<NarrowPhaseSolver>::CollisionFunctionMatrix()
   collision_matrix[BV_kIOS][GEOM_CYLINDER] = &BVHShapeCollider<kIOS, Cylinder, NarrowPhaseSolver>::collide;
   collision_matrix[BV_kIOS][GEOM_CONVEX] = &BVHShapeCollider<kIOS, Convex, NarrowPhaseSolver>::collide;
   collision_matrix[BV_kIOS][GEOM_PLANE] = &BVHShapeCollider<kIOS, Plane, NarrowPhaseSolver>::collide;
+  collision_matrix[BV_kIOS][GEOM_HALFSPACE] = &BVHShapeCollider<kIOS, Halfspace, NarrowPhaseSolver>::collide;
 
   collision_matrix[BV_OBBRSS][GEOM_BOX] = &BVHShapeCollider<OBBRSS, Box, NarrowPhaseSolver>::collide;
   collision_matrix[BV_OBBRSS][GEOM_SPHERE] = &BVHShapeCollider<OBBRSS, Sphere, NarrowPhaseSolver>::collide;
@@ -571,6 +594,7 @@ CollisionFunctionMatrix<NarrowPhaseSolver>::CollisionFunctionMatrix()
   collision_matrix[BV_OBBRSS][GEOM_CYLINDER] = &BVHShapeCollider<OBBRSS, Cylinder, NarrowPhaseSolver>::collide;
   collision_matrix[BV_OBBRSS][GEOM_CONVEX] = &BVHShapeCollider<OBBRSS, Convex, NarrowPhaseSolver>::collide;
   collision_matrix[BV_OBBRSS][GEOM_PLANE] = &BVHShapeCollider<OBBRSS, Plane, NarrowPhaseSolver>::collide;
+  collision_matrix[BV_OBBRSS][GEOM_HALFSPACE] = &BVHShapeCollider<OBBRSS, Halfspace, NarrowPhaseSolver>::collide;
 
   collision_matrix[BV_AABB][BV_AABB] = &BVHCollide<AABB, NarrowPhaseSolver>;
   collision_matrix[BV_OBB][BV_OBB] = &BVHCollide<OBB, NarrowPhaseSolver>;
@@ -589,6 +613,7 @@ CollisionFunctionMatrix<NarrowPhaseSolver>::CollisionFunctionMatrix()
   collision_matrix[GEOM_OCTREE][GEOM_CYLINDER] = &OcTreeShapeCollide<Cylinder, NarrowPhaseSolver>;
   collision_matrix[GEOM_OCTREE][GEOM_CONVEX] = &OcTreeShapeCollide<Convex, NarrowPhaseSolver>;
   collision_matrix[GEOM_OCTREE][GEOM_PLANE] = &OcTreeShapeCollide<Plane, NarrowPhaseSolver>;
+  collision_matrix[GEOM_OCTREE][GEOM_HALFSPACE] = &OcTreeShapeCollide<Halfspace, NarrowPhaseSolver>;
 
   collision_matrix[GEOM_BOX][GEOM_OCTREE] = &ShapeOcTreeCollide<Box, NarrowPhaseSolver>;
   collision_matrix[GEOM_SPHERE][GEOM_OCTREE] = &ShapeOcTreeCollide<Sphere, NarrowPhaseSolver>;
@@ -597,6 +622,7 @@ CollisionFunctionMatrix<NarrowPhaseSolver>::CollisionFunctionMatrix()
   collision_matrix[GEOM_CYLINDER][GEOM_OCTREE] = &ShapeOcTreeCollide<Cylinder, NarrowPhaseSolver>;
   collision_matrix[GEOM_CONVEX][GEOM_OCTREE] = &ShapeOcTreeCollide<Convex, NarrowPhaseSolver>;
   collision_matrix[GEOM_PLANE][GEOM_OCTREE] = &ShapeOcTreeCollide<Plane, NarrowPhaseSolver>;
+  collision_matrix[GEOM_HALFSPACE][GEOM_OCTREE] = &ShapeOcTreeCollide<Halfspace, NarrowPhaseSolver>;
 
   collision_matrix[GEOM_OCTREE][GEOM_OCTREE] = &OcTreeCollide<NarrowPhaseSolver>;
 
diff --git a/src/distance_func_matrix.cpp b/src/distance_func_matrix.cpp
index fca95def495f453d25090e4d85465ea1eb38b439..20568f00489b9444a967233825d9f9d303e8ea52 100644
--- a/src/distance_func_matrix.cpp
+++ b/src/distance_func_matrix.cpp
@@ -301,6 +301,7 @@ DistanceFunctionMatrix<NarrowPhaseSolver>::DistanceFunctionMatrix()
   distance_matrix[GEOM_BOX][GEOM_CYLINDER] = &ShapeShapeDistance<Box, Cylinder, NarrowPhaseSolver>;
   distance_matrix[GEOM_BOX][GEOM_CONVEX] = &ShapeShapeDistance<Box, Convex, NarrowPhaseSolver>;
   distance_matrix[GEOM_BOX][GEOM_PLANE] = &ShapeShapeDistance<Box, Plane, NarrowPhaseSolver>;
+  distance_matrix[GEOM_BOX][GEOM_HALFSPACE] = &ShapeShapeDistance<Box, Halfspace, NarrowPhaseSolver>;
 
   distance_matrix[GEOM_SPHERE][GEOM_BOX] = &ShapeShapeDistance<Sphere, Box, NarrowPhaseSolver>;
   distance_matrix[GEOM_SPHERE][GEOM_SPHERE] = &ShapeShapeDistance<Sphere, Sphere, NarrowPhaseSolver>;
@@ -309,6 +310,7 @@ DistanceFunctionMatrix<NarrowPhaseSolver>::DistanceFunctionMatrix()
   distance_matrix[GEOM_SPHERE][GEOM_CYLINDER] = &ShapeShapeDistance<Sphere, Cylinder, NarrowPhaseSolver>;
   distance_matrix[GEOM_SPHERE][GEOM_CONVEX] = &ShapeShapeDistance<Sphere, Convex, NarrowPhaseSolver>;
   distance_matrix[GEOM_SPHERE][GEOM_PLANE] = &ShapeShapeDistance<Sphere, Plane, NarrowPhaseSolver>;
+  distance_matrix[GEOM_SPHERE][GEOM_HALFSPACE] = &ShapeShapeDistance<Sphere, Halfspace, NarrowPhaseSolver>;
 
   distance_matrix[GEOM_CAPSULE][GEOM_BOX] = &ShapeShapeDistance<Capsule, Box, NarrowPhaseSolver>;
   distance_matrix[GEOM_CAPSULE][GEOM_SPHERE] = &ShapeShapeDistance<Capsule, Sphere, NarrowPhaseSolver>;
@@ -317,6 +319,7 @@ DistanceFunctionMatrix<NarrowPhaseSolver>::DistanceFunctionMatrix()
   distance_matrix[GEOM_CAPSULE][GEOM_CYLINDER] = &ShapeShapeDistance<Capsule, Cylinder, NarrowPhaseSolver>;
   distance_matrix[GEOM_CAPSULE][GEOM_CONVEX] = &ShapeShapeDistance<Capsule, Convex, NarrowPhaseSolver>;
   distance_matrix[GEOM_CAPSULE][GEOM_PLANE] = &ShapeShapeDistance<Capsule, Plane, NarrowPhaseSolver>;
+  distance_matrix[GEOM_CAPSULE][GEOM_HALFSPACE] = &ShapeShapeDistance<Capsule, Halfspace, NarrowPhaseSolver>;
 
   distance_matrix[GEOM_CONE][GEOM_BOX] = &ShapeShapeDistance<Cone, Box, NarrowPhaseSolver>;
   distance_matrix[GEOM_CONE][GEOM_SPHERE] = &ShapeShapeDistance<Cone, Sphere, NarrowPhaseSolver>;
@@ -325,6 +328,7 @@ DistanceFunctionMatrix<NarrowPhaseSolver>::DistanceFunctionMatrix()
   distance_matrix[GEOM_CONE][GEOM_CYLINDER] = &ShapeShapeDistance<Cone, Cylinder, NarrowPhaseSolver>;
   distance_matrix[GEOM_CONE][GEOM_CONVEX] = &ShapeShapeDistance<Cone, Convex, NarrowPhaseSolver>;
   distance_matrix[GEOM_CONE][GEOM_PLANE] = &ShapeShapeDistance<Cone, Plane, NarrowPhaseSolver>;
+  distance_matrix[GEOM_CONE][GEOM_HALFSPACE] = &ShapeShapeDistance<Cone, Halfspace, NarrowPhaseSolver>;
 
   distance_matrix[GEOM_CYLINDER][GEOM_BOX] = &ShapeShapeDistance<Cylinder, Box, NarrowPhaseSolver>;
   distance_matrix[GEOM_CYLINDER][GEOM_SPHERE] = &ShapeShapeDistance<Cylinder, Sphere, NarrowPhaseSolver>;
@@ -333,6 +337,7 @@ DistanceFunctionMatrix<NarrowPhaseSolver>::DistanceFunctionMatrix()
   distance_matrix[GEOM_CYLINDER][GEOM_CYLINDER] = &ShapeShapeDistance<Cylinder, Cylinder, NarrowPhaseSolver>;
   distance_matrix[GEOM_CYLINDER][GEOM_CONVEX] = &ShapeShapeDistance<Cylinder, Convex, NarrowPhaseSolver>;
   distance_matrix[GEOM_CYLINDER][GEOM_PLANE] = &ShapeShapeDistance<Cylinder, Plane, NarrowPhaseSolver>;
+  distance_matrix[GEOM_CYLINDER][GEOM_HALFSPACE] = &ShapeShapeDistance<Cylinder, Halfspace, NarrowPhaseSolver>;
 
   distance_matrix[GEOM_CONVEX][GEOM_BOX] = &ShapeShapeDistance<Convex, Box, NarrowPhaseSolver>;
   distance_matrix[GEOM_CONVEX][GEOM_SPHERE] = &ShapeShapeDistance<Convex, Sphere, NarrowPhaseSolver>;
@@ -341,6 +346,7 @@ DistanceFunctionMatrix<NarrowPhaseSolver>::DistanceFunctionMatrix()
   distance_matrix[GEOM_CONVEX][GEOM_CYLINDER] = &ShapeShapeDistance<Convex, Cylinder, NarrowPhaseSolver>;
   distance_matrix[GEOM_CONVEX][GEOM_CONVEX] = &ShapeShapeDistance<Convex, Convex, NarrowPhaseSolver>;
   distance_matrix[GEOM_CONVEX][GEOM_PLANE] = &ShapeShapeDistance<Convex, Plane, NarrowPhaseSolver>;
+  distance_matrix[GEOM_CONVEX][GEOM_HALFSPACE] = &ShapeShapeDistance<Convex, Halfspace, NarrowPhaseSolver>;
 
   distance_matrix[GEOM_PLANE][GEOM_BOX] = &ShapeShapeDistance<Plane, Box, NarrowPhaseSolver>;
   distance_matrix[GEOM_PLANE][GEOM_SPHERE] = &ShapeShapeDistance<Plane, Sphere, NarrowPhaseSolver>;
@@ -349,6 +355,16 @@ DistanceFunctionMatrix<NarrowPhaseSolver>::DistanceFunctionMatrix()
   distance_matrix[GEOM_PLANE][GEOM_CYLINDER] = &ShapeShapeDistance<Plane, Cylinder, NarrowPhaseSolver>;
   distance_matrix[GEOM_PLANE][GEOM_CONVEX] = &ShapeShapeDistance<Plane, Convex, NarrowPhaseSolver>;
   distance_matrix[GEOM_PLANE][GEOM_PLANE] = &ShapeShapeDistance<Plane, Plane, NarrowPhaseSolver>;
+  distance_matrix[GEOM_PLANE][GEOM_HALFSPACE] = &ShapeShapeDistance<Plane, Halfspace, NarrowPhaseSolver>;
+
+  distance_matrix[GEOM_HALFSPACE][GEOM_BOX] = &ShapeShapeDistance<Halfspace, Box, NarrowPhaseSolver>;
+  distance_matrix[GEOM_HALFSPACE][GEOM_SPHERE] = &ShapeShapeDistance<Halfspace, Sphere, NarrowPhaseSolver>;
+  distance_matrix[GEOM_HALFSPACE][GEOM_CAPSULE] = &ShapeShapeDistance<Halfspace, Capsule, NarrowPhaseSolver>;
+  distance_matrix[GEOM_HALFSPACE][GEOM_CONE] = &ShapeShapeDistance<Halfspace, Cone, NarrowPhaseSolver>;
+  distance_matrix[GEOM_HALFSPACE][GEOM_CYLINDER] = &ShapeShapeDistance<Halfspace, Cylinder, NarrowPhaseSolver>;
+  distance_matrix[GEOM_HALFSPACE][GEOM_CONVEX] = &ShapeShapeDistance<Halfspace, Convex, NarrowPhaseSolver>;
+  distance_matrix[GEOM_HALFSPACE][GEOM_PLANE] = &ShapeShapeDistance<Halfspace, Plane, NarrowPhaseSolver>;
+  distance_matrix[GEOM_HALFSPACE][GEOM_HALFSPACE] = &ShapeShapeDistance<Halfspace, Halfspace, NarrowPhaseSolver>;
 
   /* AABB distance not implemented */
   /*
@@ -359,6 +375,7 @@ DistanceFunctionMatrix<NarrowPhaseSolver>::DistanceFunctionMatrix()
   distance_matrix[BV_AABB][GEOM_CYLINDER] = &BVHShapeDistancer<AABB, Cylinder, NarrowPhaseSolver>::distance;
   distance_matrix[BV_AABB][GEOM_CONVEX] = &BVHShapeDistancer<AABB, Convex, NarrowPhaseSolver>::distance;
   distance_matrix[BV_AABB][GEOM_PLANE] = &BVHShapeDistancer<AABB, Plane, NarrowPhaseSolver>::distance;
+  distance_matrix[BV_AABB][GEOM_HALFSPACE] = &BVHShapeDistancer<AABB, Halfspace, NarrowPhaseSolver>::distance;
 
   distance_matrix[BV_OBB][GEOM_BOX] = &BVHShapeDistancer<OBB, Box, NarrowPhaseSolver>::distance;
   distance_matrix[BV_OBB][GEOM_SPHERE] = &BVHShapeDistancer<OBB, Sphere, NarrowPhaseSolver>::distance;
@@ -367,6 +384,7 @@ DistanceFunctionMatrix<NarrowPhaseSolver>::DistanceFunctionMatrix()
   distance_matrix[BV_OBB][GEOM_CYLINDER] = &BVHShapeDistancer<OBB, Cylinder, NarrowPhaseSolver>::distance;
   distance_matrix[BV_OBB][GEOM_CONVEX] = &BVHShapeDistancer<OBB, Convex, NarrowPhaseSolver>::distance;
   distance_matrix[BV_OBB][GEOM_PLANE] = &BVHShapeDistancer<OBB, Plane, NarrowPhaseSolver>::distance;
+  distance_matrix[BV_OBB][GEOM_HALFSPACE] = &BVHShapeDistancer<OBB, Halfspace, NarrowPhaseSolver>::distance;
   */
 
   distance_matrix[BV_RSS][GEOM_BOX] = &BVHShapeDistancer<RSS, Box, NarrowPhaseSolver>::distance;
@@ -376,6 +394,7 @@ DistanceFunctionMatrix<NarrowPhaseSolver>::DistanceFunctionMatrix()
   distance_matrix[BV_RSS][GEOM_CYLINDER] = &BVHShapeDistancer<RSS, Cylinder, NarrowPhaseSolver>::distance;
   distance_matrix[BV_RSS][GEOM_CONVEX] = &BVHShapeDistancer<RSS, Convex, NarrowPhaseSolver>::distance;
   distance_matrix[BV_RSS][GEOM_PLANE] = &BVHShapeDistancer<RSS, Plane, NarrowPhaseSolver>::distance;
+  distance_matrix[BV_RSS][GEOM_HALFSPACE] = &BVHShapeDistancer<RSS, Halfspace, NarrowPhaseSolver>::distance;
 
   /* KDOP distance not implemented */
   /*
@@ -386,6 +405,7 @@ DistanceFunctionMatrix<NarrowPhaseSolver>::DistanceFunctionMatrix()
   distance_matrix[BV_KDOP16][GEOM_CYLINDER] = &BVHShapeDistancer<KDOP<16>, Cylinder, NarrowPhaseSolver>::distance;
   distance_matrix[BV_KDOP16][GEOM_CONVEX] = &BVHShapeDistancer<KDOP<16>, Convex, NarrowPhaseSolver>::distance;
   distance_matrix[BV_KDOP16][GEOM_PLANE] = &BVHShapeDistancer<KDOP<16>, Plane, NarrowPhaseSolver>::distance;
+  distance_matrix[BV_KDOP16][GEOM_HALFSPACE] = &BVHShapeDistancer<KDOP<16>, Halfspace, NarrowPhaseSolver>::distance;
 
   distance_matrix[BV_KDOP18][GEOM_BOX] = &BVHShapeDistancer<KDOP<18>, Box, NarrowPhaseSolver>::distance;
   distance_matrix[BV_KDOP18][GEOM_SPHERE] = &BVHShapeDistancer<KDOP<18>, Sphere, NarrowPhaseSolver>::distance;
@@ -394,6 +414,7 @@ DistanceFunctionMatrix<NarrowPhaseSolver>::DistanceFunctionMatrix()
   distance_matrix[BV_KDOP18][GEOM_CYLINDER] = &BVHShapeDistancer<KDOP<18>, Cylinder, NarrowPhaseSolver>::distance;
   distance_matrix[BV_KDOP18][GEOM_CONVEX] = &BVHShapeDistancer<KDOP<18>, Convex, NarrowPhaseSolver>::distance;
   distance_matrix[BV_KDOP18][GEOM_PLANE] = &BVHShapeDistancer<KDOP<18>, Plane, NarrowPhaseSolver>::distance;
+  distance_matrix[BV_KDOP18][GEOM_HALFSPACE] = &BVHShapeDistancer<KDOP<18>, Halfspace, NarrowPhaseSolver>::distance;
 
   distance_matrix[BV_KDOP24][GEOM_BOX] = &BVHShapeDistancer<KDOP<24>, Box, NarrowPhaseSolver>::distance;
   distance_matrix[BV_KDOP24][GEOM_SPHERE] = &BVHShapeDistancer<KDOP<24>, Sphere, NarrowPhaseSolver>::distance;
@@ -402,6 +423,7 @@ DistanceFunctionMatrix<NarrowPhaseSolver>::DistanceFunctionMatrix()
   distance_matrix[BV_KDOP24][GEOM_CYLINDER] = &BVHShapeDistancer<KDOP<24>, Cylinder, NarrowPhaseSolver>::distance;
   distance_matrix[BV_KDOP24][GEOM_CONVEX] = &BVHShapeDistancer<KDOP<24>, Convex, NarrowPhaseSolver>::distance;
   distance_matrix[BV_KDOP24][GEOM_PLANE] = &BVHShapeDistancer<KDOP<24>, Plane, NarrowPhaseSolver>::distance;
+  distance_matrix[BV_KDOP24][GEOM_HALFSPACE] = &BVHShapeDistancer<KDOP<24>, Halfspace, NarrowPhaseSolver>::distance;
   */
 
   distance_matrix[BV_kIOS][GEOM_BOX] = &BVHShapeDistancer<kIOS, Box, NarrowPhaseSolver>::distance;
@@ -411,6 +433,7 @@ DistanceFunctionMatrix<NarrowPhaseSolver>::DistanceFunctionMatrix()
   distance_matrix[BV_kIOS][GEOM_CYLINDER] = &BVHShapeDistancer<kIOS, Cylinder, NarrowPhaseSolver>::distance;
   distance_matrix[BV_kIOS][GEOM_CONVEX] = &BVHShapeDistancer<kIOS, Convex, NarrowPhaseSolver>::distance;
   distance_matrix[BV_kIOS][GEOM_PLANE] = &BVHShapeDistancer<kIOS, Plane, NarrowPhaseSolver>::distance;
+  distance_matrix[BV_kIOS][GEOM_HALFSPACE] = &BVHShapeDistancer<kIOS, Halfspace, NarrowPhaseSolver>::distance;
 
   distance_matrix[BV_OBBRSS][GEOM_BOX] = &BVHShapeDistancer<OBBRSS, Box, NarrowPhaseSolver>::distance;
   distance_matrix[BV_OBBRSS][GEOM_SPHERE] = &BVHShapeDistancer<OBBRSS, Sphere, NarrowPhaseSolver>::distance;
@@ -419,6 +442,7 @@ DistanceFunctionMatrix<NarrowPhaseSolver>::DistanceFunctionMatrix()
   distance_matrix[BV_OBBRSS][GEOM_CYLINDER] = &BVHShapeDistancer<OBBRSS, Cylinder, NarrowPhaseSolver>::distance;
   distance_matrix[BV_OBBRSS][GEOM_CONVEX] = &BVHShapeDistancer<OBBRSS, Convex, NarrowPhaseSolver>::distance;
   distance_matrix[BV_OBBRSS][GEOM_PLANE] = &BVHShapeDistancer<OBBRSS, Plane, NarrowPhaseSolver>::distance;
+  distance_matrix[BV_OBBRSS][GEOM_HALFSPACE] = &BVHShapeDistancer<OBBRSS, Halfspace, NarrowPhaseSolver>::distance;
 
   distance_matrix[BV_AABB][BV_AABB] = &BVHDistance<AABB, NarrowPhaseSolver>;
   distance_matrix[BV_RSS][BV_RSS] = &BVHDistance<RSS, NarrowPhaseSolver>;
@@ -433,6 +457,7 @@ DistanceFunctionMatrix<NarrowPhaseSolver>::DistanceFunctionMatrix()
   distance_matrix[GEOM_OCTREE][GEOM_CYLINDER] = &OcTreeShapeDistance<Cylinder, NarrowPhaseSolver>;
   distance_matrix[GEOM_OCTREE][GEOM_CONVEX] = &OcTreeShapeDistance<Convex, NarrowPhaseSolver>;
   distance_matrix[GEOM_OCTREE][GEOM_PLANE] = &OcTreeShapeDistance<Plane, NarrowPhaseSolver>;
+  distance_matrix[GEOM_OCTREE][GEOM_HALFSPACE] = &OcTreeShapeDistance<Halfspace, NarrowPhaseSolver>;
 
   distance_matrix[GEOM_BOX][GEOM_OCTREE] = &ShapeOcTreeDistance<Box, NarrowPhaseSolver>;
   distance_matrix[GEOM_SPHERE][GEOM_OCTREE] = &ShapeOcTreeDistance<Sphere, NarrowPhaseSolver>;
@@ -441,6 +466,7 @@ DistanceFunctionMatrix<NarrowPhaseSolver>::DistanceFunctionMatrix()
   distance_matrix[GEOM_CYLINDER][GEOM_OCTREE] = &ShapeOcTreeDistance<Cylinder, NarrowPhaseSolver>;
   distance_matrix[GEOM_CONVEX][GEOM_OCTREE] = &ShapeOcTreeDistance<Convex, NarrowPhaseSolver>;
   distance_matrix[GEOM_PLANE][GEOM_OCTREE] = &ShapeOcTreeDistance<Plane, NarrowPhaseSolver>;
+  distance_matrix[GEOM_HALFSPACE][GEOM_OCTREE] = &ShapeOcTreeDistance<Halfspace, NarrowPhaseSolver>;
 
   distance_matrix[GEOM_OCTREE][GEOM_OCTREE] = &OcTreeDistance<NarrowPhaseSolver>;
 
diff --git a/test/test_fcl_geometric_shapes.cpp b/test/test_fcl_geometric_shapes.cpp
index 624fff11c15550f9641bae4c7f197985c17167b7..a89a55d38651f397b53414bf02fce86ec15ed37e 100644
--- a/test/test_fcl_geometric_shapes.cpp
+++ b/test/test_fcl_geometric_shapes.cpp
@@ -113,6 +113,177 @@ BOOST_AUTO_TEST_CASE(gjkcache)
   }
 }
 
+template <typename S1, typename S2>
+void printComparisonError(const std::string& comparison_type,
+                          const S1& s1, const Transform3f& tf1,
+                          const S2& s2, const Transform3f& tf2,
+                          GJKSolverType solver_type,
+                          const Vec3f& contact_or_normal,
+                          const Vec3f& expected_contact_or_normal,
+                          bool check_opposite_normal,
+                          FCL_REAL tol)
+{
+  std::cout << "Disagreement between " << comparison_type
+            << " and expected_" << comparison_type << " for "
+            << getNodeTypeName(s1.getNodeType()) << " and "
+            << getNodeTypeName(s2.getNodeType()) << " with '"
+            << getGJKSolverName(solver_type) << "' solver." << std::endl
+            << "tf1.quaternion: " << tf1.getQuatRotation() << std::endl
+            << "tf1.translation: " << tf1.getTranslation() << std::endl
+            << "tf2.quaternion: " << tf2.getQuatRotation() << std::endl
+            << "tf2.translation: " << tf2.getTranslation() << std::endl
+            << comparison_type << ": " << contact_or_normal << std::endl
+            << "expected_" << comparison_type << ": " << expected_contact_or_normal;
+
+  if (check_opposite_normal)
+    std::cout << " or " << -expected_contact_or_normal;
+
+  std::cout << std::endl
+            << "difference: " << (contact_or_normal - expected_contact_or_normal).norm() << std::endl
+            << "tolerance: " << tol << std::endl;
+}
+
+template <typename S1, typename S2>
+void printComparisonError(const std::string& comparison_type,
+                          const S1& s1, const Transform3f& tf1,
+                          const S2& s2, const Transform3f& tf2,
+                          GJKSolverType solver_type,
+                          FCL_REAL depth,
+                          FCL_REAL expected_depth,
+                          FCL_REAL tol)
+{
+  std::cout << "Disagreement between " << comparison_type
+            << " and expected_" << comparison_type << " for "
+            << getNodeTypeName(s1.getNodeType()) << " and "
+            << getNodeTypeName(s2.getNodeType()) << " with '"
+            << getGJKSolverName(solver_type) << "' solver." << std::endl
+            << "tf1.quaternion: " << tf1.getQuatRotation() << std::endl
+            << "tf1.translation: " << tf1.getTranslation() << std::endl
+            << "tf2.quaternion: " << tf2.getQuatRotation() << std::endl
+            << "tf2.translation: " << tf2.getTranslation() << std::endl
+            << "depth: " << depth << std::endl
+            << "expected_depth: " << expected_depth << std::endl
+            << "difference: " << std::fabs(depth - expected_depth) << std::endl
+            << "tolerance: " << tol << std::endl;
+}
+
+template <typename S1, typename S2>
+void compareContact(const S1& s1, const Transform3f& tf1,
+                    const S2& s2, const Transform3f& tf2,
+                    GJKSolverType solver_type,
+                    const Vec3f& contact, Vec3f* expected_point,
+                    FCL_REAL depth, FCL_REAL* expected_depth,
+                    const Vec3f& normal, Vec3f* expected_normal, bool check_opposite_normal,
+                    FCL_REAL tol)
+{
+  if (expected_point)
+  {
+    bool contact_equal = contact.equal(*expected_point, tol);
+    BOOST_CHECK(contact_equal);
+    if (!contact_equal)
+      printComparisonError("contact", s1, tf1, s2, tf2, solver_type, contact, *expected_point, false, tol);
+  }
+
+  if (expected_depth)
+  {
+    bool depth_equal = std::fabs(depth - *expected_depth) < tol;
+    BOOST_CHECK(depth_equal);
+    if (!depth_equal)
+      printComparisonError("depth", s1, tf1, s2, tf2, solver_type, depth, *expected_depth, tol);
+  }
+
+  if (expected_normal)
+  {
+    bool normal_equal = normal.equal(*expected_normal, tol);
+
+    if (!normal_equal && check_opposite_normal)
+      normal_equal = normal.equal(-(*expected_normal), tol);
+
+    BOOST_CHECK(normal_equal);
+    if (!normal_equal)
+      printComparisonError("normal", s1, tf1, s2, tf2, solver_type, normal, *expected_normal, check_opposite_normal, tol);
+  }
+}
+
+template <typename S1, typename S2>
+void testShapeInersection(const S1& s1, const Transform3f& tf1,
+                          const S2& s2, const Transform3f& tf2,
+                          GJKSolverType solver_type,
+                          bool expected_res,
+                          Vec3f* expected_point = NULL,
+                          FCL_REAL* expected_depth = NULL,
+                          Vec3f* expected_normal = NULL,
+                          bool check_opposite_normal = false,
+                          FCL_REAL tol = 1e-9)
+{
+  CollisionRequest request;
+  request.gjk_solver_type = solver_type;
+  CollisionResult result;
+
+  Vec3f contact;
+  FCL_REAL depth;
+  Vec3f normal;  // normal direction should be from object 1 to object 2
+  bool res;
+
+  if (solver_type == GST_LIBCCD)
+  {
+    res = solver1.shapeIntersect(s1, tf1, s2, tf2, NULL, NULL, NULL);
+  }
+  else if (solver_type == GST_INDEP)
+  {
+    res = solver2.shapeIntersect(s1, tf1, s2, tf2, NULL, NULL, NULL);
+  }
+  else
+  {
+    std::cerr << "Invalid GJK solver. Test aborted." << std::endl;
+    return;
+  }
+  BOOST_CHECK_EQUAL(res, expected_res);
+
+  if (solver_type == GST_LIBCCD)
+  {
+    res = solver1.shapeIntersect(s1, tf1, s2, tf2, &contact, &depth, &normal);
+  }
+  else if (solver_type == GST_INDEP)
+  {
+    res = solver2.shapeIntersect(s1, tf1, s2, tf2, &contact, &depth, &normal);
+  }
+  else
+  {
+    std::cerr << "Invalid GJK solver. Test aborted." << std::endl;
+    return;
+  }
+  BOOST_CHECK_EQUAL(res, expected_res);
+  if (expected_res)
+    compareContact(s1, tf1, s2, tf2, solver_type, contact, expected_point, depth, expected_depth, normal, expected_normal, check_opposite_normal, tol);
+
+  if (s1.getNodeType() == GEOM_HALFSPACE)
+  {
+    std::cout << "Abort test since Halfspace is not allowed to be placed in the first collision geometry in the pair. "
+              << "Please see pull request #53." << std::endl;
+    return;
+  }
+
+  request.enable_contact = false;
+  result.clear();
+  res = (collide(&s1, tf1, &s2, tf2, request, result) > 0);
+  BOOST_CHECK_EQUAL(res, expected_res);
+
+  request.enable_contact = true;
+  result.clear();
+  res = (collide(&s1, tf1, &s2, tf2, request, result) > 0);
+  BOOST_CHECK_EQUAL(res, expected_res);
+  if (expected_res)
+  {
+    BOOST_CHECK_EQUAL(result.numContacts(), 1);
+    if (result.numContacts() == 1)
+    {
+      Contact contact = result.getContact(0);
+      compareContact(s1, tf1, s2, tf2, solver_type, contact.pos, expected_point, contact.penetration_depth, expected_depth, contact.normal, expected_normal, check_opposite_normal, tol);
+    }
+  }
+}
+
 BOOST_AUTO_TEST_CASE(shapeIntersection_spheresphere)
 {
   Sphere s1(20);
@@ -529,67 +700,79 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacesphere)
   Sphere s(10);
   Halfspace 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)));
-  BOOST_CHECK(contact.equal(Vec3f(-5, 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))));
-  BOOST_CHECK(contact.equal(transform.transform(Vec3f(-5, 0, 0))));
-
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(5, 0, 0)), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 15) < 0.001);
-  BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)));
-  BOOST_CHECK(contact.equal(Vec3f(-2.5, 0, 0)));
-
-  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(5, 0, 0)), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 15) < 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(-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(-7.5, 0, 0)));
 
-  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(-7.5, 0, 0))));
-
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-10.1, 0, 0)), &contact, &depth, &normal);
-  BOOST_CHECK_FALSE(res);
-
-  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-10.1, 0, 0)), &contact, &depth, &normal);
-  BOOST_CHECK_FALSE(res);
-
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(10.1, 0, 0)), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 20.1) < 0.001);
-  BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)));
-  BOOST_CHECK(contact.equal(Vec3f(0.05, 0, 0)));
-
-  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(10.1, 0, 0)), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 20.1) < 0.001);
-  BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
-  BOOST_CHECK(contact.equal(transform.transform(Vec3f(0.05, 0, 0))));
+  tf1 = Transform3f();
+  tf2 = Transform3f();
+  contact.setValue(-5, 0, 0);
+  depth = 10;
+  normal.setValue(-1, 0, 0);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = transform;
+  tf2 = transform;
+  contact = transform.transform(Vec3f(-5, 0, 0));
+  depth = 10;
+  normal = transform.getQuatRotation().transform(Vec3f(-1, 0, 0));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(5, 0, 0));
+  contact.setValue(-2.5, 0, 0);
+  depth = 15;
+  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(-2.5, 0, 0));
+  depth = 15;
+  normal = transform.getQuatRotation().transform(Vec3f(-1, 0, 0));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(-5, 0, 0));
+  contact.setValue(-7.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(-7.5, 0, 0));
+  depth = 5;
+  normal = transform.getQuatRotation().transform(Vec3f(-1, 0, 0));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(-10.1, 0, 0));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
+
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(-10.1, 0, 0));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(10.1, 0, 0));
+  contact.setValue(0.05, 0, 0);
+  depth = 20.1;
+  normal.setValue(-1, 0, 0);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(10.1, 0, 0));
+  contact = transform.transform(Vec3f(0.05, 0, 0));
+  depth = 20.1;
+  normal = transform.getQuatRotation().transform(Vec3f(-1, 0, 0));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
 }
 
 BOOST_AUTO_TEST_CASE(shapeIntersection_planesphere)
@@ -660,70 +843,83 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacebox)
   Box s(5, 10, 20);
   Halfspace 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)));
-  BOOST_CHECK(contact.equal(Vec3f(-1.25, 0, 0)));
-
-  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))));
-  BOOST_CHECK(contact.equal(transform.transform(Vec3f(-1.25, 0, 0))));
-
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(1.25, 0, 0)), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 3.75) < 0.001);
-  BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)));
-  BOOST_CHECK(contact.equal(Vec3f(-0.625, 0, 0)));
-
-  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(1.25, 0, 0)), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 3.75) < 0.001);
-  BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
-  BOOST_CHECK(contact.equal(transform.transform(Vec3f(-0.625, 0, 0))));
-
-  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.875, 0, 0)));
-
-  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.875, 0, 0))));
-
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(2.51, 0, 0)), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 5.01) < 0.001);
-  BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)));
-  BOOST_CHECK(contact.equal(Vec3f(0.005, 0, 0)));
-
-  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(2.51, 0, 0)), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 5.01) < 0.001);
-  BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
-  BOOST_CHECK(contact.equal(transform.transform(Vec3f(0.005, 0, 0))));
-
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-2.51, 0, 0)), &contact, &depth, &normal);
-  BOOST_CHECK_FALSE(res);
-
-  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-2.51, 0, 0)), &contact, &depth, &normal);
-  BOOST_CHECK_FALSE(res);
 
-  res = solver1.shapeIntersect(s, Transform3f(transform.getQuatRotation()), hs, Transform3f(), &contact, &depth, &normal);
-  BOOST_CHECK(res);
+  tf1 = Transform3f();
+  tf2 = Transform3f();
+  contact.setValue(-1.25, 0, 0);
+  depth = 2.5;
+  normal.setValue(-1, 0, 0);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = transform;
+  tf2 = transform;
+  contact = transform.transform(Vec3f(-1.25, 0, 0));
+  depth = 2.5;
+  normal = transform.getQuatRotation().transform(Vec3f(-1, 0, 0));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(1.25, 0, 0));
+  contact.setValue(-0.625, 0, 0);
+  depth = 3.75;
+  normal.setValue(-1, 0, 0);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(1.25, 0, 0));
+  contact = transform.transform(Vec3f(-0.625, 0, 0));
+  depth = 3.75;
+  normal = transform.getQuatRotation().transform(Vec3f(-1, 0, 0));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(-1.25, 0, 0));
+  contact.setValue(-1.875, 0, 0);
+  depth = 1.25;
+  normal.setValue(-1, 0, 0);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(-1.25, 0, 0));
+  contact = transform.transform(Vec3f(-1.875, 0, 0));
+  depth = 1.25;
+  normal = transform.getQuatRotation().transform(Vec3f(-1, 0, 0));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(2.51, 0, 0));
+  contact.setValue(0.005, 0, 0);
+  depth = 5.01;
+  normal.setValue(-1, 0, 0);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(2.51, 0, 0));
+  contact = transform.transform(Vec3f(0.005, 0, 0));
+  depth = 5.01;
+  normal = transform.getQuatRotation().transform(Vec3f(-1, 0, 0));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(-2.51, 0, 0));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
+
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(-2.51, 0, 0));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
+
+  tf1 = Transform3f(transform.getQuatRotation());
+  tf2 = Transform3f();
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true);
 }
 
 BOOST_AUTO_TEST_CASE(shapeIntersection_planebox)
@@ -796,6 +992,224 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecapsule)
   Capsule s(5, 10);
   Halfspace hs(Vec3f(1, 0, 0), 0);
 
+  Transform3f tf1;
+  Transform3f tf2;
+
+  Transform3f transform;
+  generateRandomTransform(extents, transform);
+
+  Vec3f contact;
+  FCL_REAL depth;
+  Vec3f normal;
+
+  tf1 = Transform3f();
+  tf2 = Transform3f();
+  contact.setValue(-2.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;
+  contact = transform.transform(Vec3f(-2.5, 0, 0));
+  depth = 5;
+  normal = transform.getQuatRotation().transform(Vec3f(-1, 0, 0));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(2.5, 0, 0));
+  contact.setValue(-1.25, 0, 0);
+  depth = 7.5;
+  normal.setValue(-1, 0, 0);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = transform;
+  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));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(-2.5, 0, 0));
+  contact.setValue(-3.75, 0, 0);
+  depth = 2.5;
+  normal.setValue(-1, 0, 0);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = transform;
+  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));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(5.1, 0, 0));
+  contact.setValue(0.05, 0, 0);
+  depth = 10.1;
+  normal.setValue(-1, 0, 0);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = transform;
+  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));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  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 = Halfspace(Vec3f(0, 1, 0), 0);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f();
+  contact.setValue(0, -2.5, 0);
+  depth = 5;
+  normal.setValue(0, -1, 0);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = transform;
+  tf2 = transform;
+  contact = transform.transform(Vec3f(0, -2.5, 0));
+  depth = 5;
+  normal = transform.getQuatRotation().transform(Vec3f(0, -1, 0));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(0, 2.5, 0));
+  contact.setValue(0, -1.25, 0);
+  depth = 7.5;
+  normal.setValue(0, -1, 0);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = transform;
+  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));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(0, -2.5, 0));
+  contact.setValue(0, -3.75, 0);
+  depth = 2.5;
+  normal.setValue(0, -1, 0);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = transform;
+  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));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(0, 5.1, 0));
+  contact.setValue(0, 0.05, 0);
+  depth = 10.1;
+  normal.setValue(0, -1, 0);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = transform;
+  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));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(0, -5.1, 0));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
+
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(0, -5.1, 0));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
+
+
+
+
+  hs = Halfspace(Vec3f(0, 0, 1), 0);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f();
+  contact.setValue(0, 0, -5);
+  depth = 10;
+  normal.setValue(0, 0, -1);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = transform;
+  tf2 = transform;
+  contact = transform.transform(Vec3f(0, 0, -5));
+  depth = 10;
+  normal = transform.getQuatRotation().transform(Vec3f(0, 0, -1));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(0, 0, 2.5));
+  contact.setValue(0, 0, -3.75);
+  depth = 12.5;
+  normal.setValue(0, 0, -1);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = transform;
+  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));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(0, 0, -2.5));
+  contact.setValue(0, 0, -6.25);
+  depth = 7.5;
+  normal.setValue(0, 0, -1);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = transform;
+  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));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(0, 0, 10.1));
+  contact.setValue(0, 0, 0.05);
+  depth = 20.1;
+  normal.setValue(0, 0, -1);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = transform;
+  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));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(0, 0, -10.1));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
+
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(0, 0, -10.1));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
+}
+
+BOOST_AUTO_TEST_CASE(shapeIntersection_planecapsule)
+{
+  Capsule s(5, 10);
+  Plane hs(Vec3f(1, 0, 0), 0);
+
   Transform3f transform;
   generateRandomTransform(extents, transform);
 
@@ -807,50 +1221,44 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecapsule)
   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)));
-  BOOST_CHECK(contact.equal(Vec3f(-2.5, 0, 0)));
+  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 - 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))));
+  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))));
 
   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(2.5, 0, 0)), &contact, &depth, &normal);
   BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 7.5) < 0.001);
-  BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)));
-  BOOST_CHECK(contact.equal(Vec3f(-1.25, 0, 0)));
+  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)));
 
   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(2.5, 0, 0)), &contact, &depth, &normal);
   BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 7.5) < 0.001);
-  BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
-  BOOST_CHECK(contact.equal(transform.transform(Vec3f(-1.25, 0, 0))));
+  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(-3.75, 0, 0)));
+  BOOST_CHECK(contact.equal(Vec3f(-2.5, 0, 0)));
 
   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(-3.75, 0, 0))));
+  BOOST_CHECK(contact.equal(transform.transform(Vec3f(-2.5, 0, 0))));
 
   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(5.1, 0, 0)), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 10.1) < 0.001);
-  BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)));
-  BOOST_CHECK(contact.equal(Vec3f(0.05, 0, 0)));
+  BOOST_CHECK_FALSE(res);
 
   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(5.1, 0, 0)), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 10.1) < 0.001);
-  BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
-  BOOST_CHECK(contact.equal(transform.transform(Vec3f(0.05, 0, 0))));
+  BOOST_CHECK_FALSE(res);
 
   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-5.1, 0, 0)), &contact, &depth, &normal);
   BOOST_CHECK_FALSE(res);
@@ -861,55 +1269,49 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecapsule)
 
 
 
-  hs = Halfspace(Vec3f(0, 1, 0), 0);
+  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)));
-  BOOST_CHECK(contact.equal(Vec3f(0, -2.5, 0)));
+  BOOST_CHECK(normal.equal(Vec3f(0, -1, 0)) || normal.equal(Vec3f(0, 1, 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 - 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))));
+  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))));
 
   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 2.5, 0)), &contact, &depth, &normal);
   BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 7.5) < 0.001);
-  BOOST_CHECK(normal.equal(Vec3f(0, -1, 0)));
-  BOOST_CHECK(contact.equal(Vec3f(0, -1.25, 0)));
+  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)));
 
   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 2.5, 0)), &contact, &depth, &normal);
   BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 7.5) < 0.001);
-  BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0))));
-  BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, -1.25, 0))));
+  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))));
 
   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, -3.75, 0)));
+  BOOST_CHECK(contact.equal(Vec3f(0, -2.5, 0)));
 
   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, -3.75, 0))));
+  BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, -2.5, 0))));
 
   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 5.1, 0)), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 10.1) < 0.001);
-  BOOST_CHECK(normal.equal(Vec3f(0, -1, 0)));
-  BOOST_CHECK(contact.equal(Vec3f(0, 0.05, 0)));
+  BOOST_CHECK_FALSE(res);
 
   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 5.1, 0)), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 10.1) < 0.001);
-  BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0))));
-  BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0.05, 0))));
+  BOOST_CHECK_FALSE(res);
 
   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, -5.1, 0)), &contact, &depth, &normal);
   BOOST_CHECK_FALSE(res);
@@ -920,55 +1322,49 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecapsule)
 
 
 
-  hs = Halfspace(Vec3f(0, 0, 1), 0);
+  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)));
-  BOOST_CHECK(contact.equal(Vec3f(0, 0, -5)));
+  BOOST_CHECK(normal.equal(Vec3f(0, 0, -1)) || normal.equal(Vec3f(0, 0, 1)));
+  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(0, 0, -1))));
-  BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, -5))));
+  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))));
 
   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, 2.5)), &contact, &depth, &normal);
   BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 12.5) < 0.001);
-  BOOST_CHECK(normal.equal(Vec3f(0, 0, -1)));
-  BOOST_CHECK(contact.equal(Vec3f(0, 0, -3.75)));
+  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)));
 
   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, 2.5)), &contact, &depth, &normal);
   BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 12.5) < 0.001);
-  BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1))));
-  BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, -3.75))));
+  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))));
 
   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, -6.25)));
+  BOOST_CHECK(contact.equal(Vec3f(0, 0, -2.5)));
 
   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, -6.25))));
+  BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, -2.5))));
 
   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, 10.1)), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 20.1) < 0.001);
-  BOOST_CHECK(normal.equal(Vec3f(0, 0, -1)));
-  BOOST_CHECK(contact.equal(Vec3f(0, 0, 0.05)));
+  BOOST_CHECK_FALSE(res);
 
   res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, 10.1)), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 20.1) < 0.001);
-  BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1))));
-  BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, 0.05))));
+  BOOST_CHECK_FALSE(res);
 
   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, -10.1)), &contact, &depth, &normal);
   BOOST_CHECK_FALSE(res);
@@ -977,358 +1373,222 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecapsule)
   BOOST_CHECK_FALSE(res);
 }
 
-BOOST_AUTO_TEST_CASE(shapeIntersection_planecapsule)
-{
-  Capsule s(5, 10);
-  Plane hs(Vec3f(1, 0, 0), 0);
-
-  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)));
-
-  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))));
-
-  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)));
-
-  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)));
-
-  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(5.1, 0, 0)), &contact, &depth, &normal);
-  BOOST_CHECK_FALSE(res);
-
-  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(5.1, 0, 0)), &contact, &depth, &normal);
-  BOOST_CHECK_FALSE(res);
-
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-5.1, 0, 0)), &contact, &depth, &normal);
-  BOOST_CHECK_FALSE(res);
-
-  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-5.1, 0, 0)), &contact, &depth, &normal);
-  BOOST_CHECK_FALSE(res);
-
-
-
-
-  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)));
-
-  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))));
-
-  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)));
-
-  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))));
-
-  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)));
-
-  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))));
-
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 5.1, 0)), &contact, &depth, &normal);
-  BOOST_CHECK_FALSE(res);
-
-  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 5.1, 0)), &contact, &depth, &normal);
-  BOOST_CHECK_FALSE(res);
-
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, -5.1, 0)), &contact, &depth, &normal);
-  BOOST_CHECK_FALSE(res);
-
-  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, -5.1, 0)), &contact, &depth, &normal);
-  BOOST_CHECK_FALSE(res);
-
-
-
-
-  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)));
-
-  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))));
-
-  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)));
-
-  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))));
-
-  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)));
-
-  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))));
-
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, 10.1)), &contact, &depth, &normal);
-  BOOST_CHECK_FALSE(res);
-
-  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, 10.1)), &contact, &depth, &normal);
-  BOOST_CHECK_FALSE(res);
-
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, -10.1)), &contact, &depth, &normal);
-  BOOST_CHECK_FALSE(res);
-
-  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, -10.1)), &contact, &depth, &normal);
-  BOOST_CHECK_FALSE(res);
-}
-
-BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecylinder)
+BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecylinder)
 {
   Cylinder s(5, 10);
   Halfspace 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)));
-  BOOST_CHECK(contact.equal(Vec3f(-2.5, 0, 0)));
-
-  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))));
-  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 - 7.5) < 0.001);
-  BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)));
-  BOOST_CHECK(contact.equal(Vec3f(-1.25, 0, 0)));
-
-  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(2.5, 0, 0)), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 7.5) < 0.001);
-  BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
-  BOOST_CHECK(contact.equal(transform.transform(Vec3f(-1.25, 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(-3.75, 0, 0)));
-
-  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(-3.75, 0, 0))));
-
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(5.1, 0, 0)), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 10.1) < 0.001);
-  BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)));
-  BOOST_CHECK(contact.equal(Vec3f(0.05, 0, 0)));
-
-  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(5.1, 0, 0)), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 10.1) < 0.001);
-  BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
-  BOOST_CHECK(contact.equal(transform.transform(Vec3f(0.05, 0, 0))));
-
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-5.1, 0, 0)), &contact, &depth, &normal);
-  BOOST_CHECK_FALSE(res);
-
-  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-5.1, 0, 0)), &contact, &depth, &normal);
-  BOOST_CHECK_FALSE(res);
+  tf1 = Transform3f();
+  tf2 = Transform3f();
+  contact.setValue(-2.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;
+  contact = transform.transform(Vec3f(-2.5, 0, 0));
+  depth = 5;
+  normal = transform.getQuatRotation().transform(Vec3f(-1, 0, 0));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(2.5, 0, 0));
+  contact.setValue(-1.25, 0, 0);
+  depth = 7.5;
+  normal.setValue(-1, 0, 0);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = transform;
+  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));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(-2.5, 0, 0));
+  contact.setValue(-3.75, 0, 0);
+  depth = 2.5;
+  normal.setValue(-1, 0, 0);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = transform;
+  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));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(5.1, 0, 0));
+  contact.setValue(0.05, 0, 0);
+  depth = 10.1;
+  normal.setValue(-1, 0, 0);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = transform;
+  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));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  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 = Halfspace(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)));
-  BOOST_CHECK(contact.equal(Vec3f(0, -2.5, 0)));
-
-  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))));
-  BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, -2.5, 0))));
-
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 2.5, 0)), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 7.5) < 0.001);
-  BOOST_CHECK(normal.equal(Vec3f(0, -1, 0)));
-  BOOST_CHECK(contact.equal(Vec3f(0, -1.25, 0)));
-
-  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 2.5, 0)), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 7.5) < 0.001);
-  BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0))));
-  BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, -1.25, 0))));
-
-  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, -3.75, 0)));
-
-  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, -3.75, 0))));
-
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 5.1, 0)), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 10.1) < 0.001);
-  BOOST_CHECK(normal.equal(Vec3f(0, -1, 0)));
-  BOOST_CHECK(contact.equal(Vec3f(0, 0.05, 0)));
-
-  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 5.1, 0)), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 10.1) < 0.001);
-  BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0))));
-  BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0.05, 0))));
-
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, -5.1, 0)), &contact, &depth, &normal);
-  BOOST_CHECK_FALSE(res);
-
-  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, -5.1, 0)), &contact, &depth, &normal);
-  BOOST_CHECK_FALSE(res);
+  tf1 = Transform3f();
+  tf2 = Transform3f();
+  contact.setValue(0, -2.5, 0);
+  depth = 5;
+  normal.setValue(0, -1, 0);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = transform;
+  tf2 = transform;
+  contact = transform.transform(Vec3f(0, -2.5, 0));
+  depth = 5;
+  normal = transform.getQuatRotation().transform(Vec3f(0, -1, 0));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(0, 2.5, 0));
+  contact.setValue(0, -1.25, 0);
+  depth = 7.5;
+  normal.setValue(0, -1, 0);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = transform;
+  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));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(0, -2.5, 0));
+  contact.setValue(0, -3.75, 0);
+  depth = 2.5;
+  normal.setValue(0, -1, 0);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = transform;
+  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));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(0, 5.1, 0));
+  contact.setValue(0, 0.05, 0);
+  depth = 10.1;
+  normal.setValue(0, -1, 0);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = transform;
+  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));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(0, -5.1, 0));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
+
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(0, -5.1, 0));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
 
 
 
 
   hs = Halfspace(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)));
-  BOOST_CHECK(contact.equal(Vec3f(0, 0, -2.5)));
-
-  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))));
-  BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, -2.5))));
-
-  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, -1.25)));
-
-  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, -1.25))));
-
-  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, -3.75)));
-
-  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, -3.75))));
-
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, 5.1)), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 10.1) < 0.001);
-  BOOST_CHECK(normal.equal(Vec3f(0, 0, -1)));
-  BOOST_CHECK(contact.equal(Vec3f(0, 0, 0.05)));
-
-  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, 5.1)), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 10.1) < 0.001);
-  BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1))));
-  BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, 0.05))));
-
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, -5.1)), &contact, &depth, &normal);
-  BOOST_CHECK_FALSE(res);
-
-  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, -5.1)), &contact, &depth, &normal);
-  BOOST_CHECK_FALSE(res);
+  tf1 = Transform3f();
+  tf2 = Transform3f();
+  contact.setValue(0, 0, -2.5);
+  depth = 5;
+  normal.setValue(0, 0, -1);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = transform;
+  tf2 = transform;
+  contact = transform.transform(Vec3f(0, 0, -2.5));
+  depth = 5;
+  normal = transform.getQuatRotation().transform(Vec3f(0, 0, -1));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(0, 0, 2.5));
+  contact.setValue(0, 0, -1.25);
+  depth = 7.5;
+  normal.setValue(0, 0, -1);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = transform;
+  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));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(0, 0, -2.5));
+  contact.setValue(0, 0, -3.75);
+  depth = 2.5;
+  normal.setValue(0, 0, -1);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = transform;
+  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));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(0, 0, 5.1));
+  contact.setValue(0, 0, 0.05);
+  depth = 10.1;
+  normal.setValue(0, 0, -1);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = transform;
+  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));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(0, 0, -5.1));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
+
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(0, 0, -5.1));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
 }
 
 BOOST_AUTO_TEST_CASE(shapeIntersection_planecylinder)
@@ -1505,6 +1765,9 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecone)
   Cone s(5, 10);
   Halfspace hs(Vec3f(1, 0, 0), 0);
 
+  Transform3f tf1;
+  Transform3f tf2;
+
   Transform3f transform;
   generateRandomTransform(extents, transform);
 
@@ -1513,177 +1776,207 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecone)
   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)));
-  BOOST_CHECK(contact.equal(Vec3f(-2.5, 0, -5)));
-
-  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))));
-  BOOST_CHECK(contact.equal(transform.transform(Vec3f(-2.5, 0, -5))));
-
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(2.5, 0, 0)), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 7.5) < 0.001);
-  BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)));
-  BOOST_CHECK(contact.equal(Vec3f(-1.25, 0, -5)));
-
-  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(2.5, 0, 0)), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 7.5) < 0.001);
-  BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
-  BOOST_CHECK(contact.equal(transform.transform(Vec3f(-1.25, 0, -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(-3.75, 0, -5)));
-
-  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(-3.75, 0, -5))));
-
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(5.1, 0, 0)), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 10.1) < 0.001);
-  BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)));
-  BOOST_CHECK(contact.equal(Vec3f(0.05, 0, -5)));
-
-  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(5.1, 0, 0)), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 10.1) < 0.001);
-  BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
-  BOOST_CHECK(contact.equal(transform.transform(Vec3f(0.05, 0, -5))));
-
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-5.1, 0, 0)), &contact, &depth, &normal);
-  BOOST_CHECK_FALSE(res);
-
-  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-5.1, 0, 0)), &contact, &depth, &normal);
-  BOOST_CHECK_FALSE(res);
+  tf1 = Transform3f();
+  tf2 = Transform3f();
+  contact.setValue(-2.5, 0, -5);
+  depth = 5;
+  normal.setValue(-1, 0, 0);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = transform;
+  tf2 = transform;
+  contact = transform.transform(Vec3f(-2.5, 0, -5));
+  depth = 5;
+  normal = transform.getQuatRotation().transform(Vec3f(-1, 0, 0));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(2.5, 0, 0));
+  contact.setValue(-1.25, 0, -5);
+  depth = 7.5;
+  normal.setValue(-1, 0, 0);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = transform;
+  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));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(-2.5, 0, 0));
+  contact.setValue(-3.75, 0, -5);
+  depth = 2.5;
+  normal.setValue(-1, 0, 0);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = transform;
+  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));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(5.1, 0, 0));
+  contact.setValue(0.05, 0, -5);
+  depth = 10.1;
+  normal.setValue(-1, 0, 0);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = transform;
+  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));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  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 = Halfspace(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)));
-  BOOST_CHECK(contact.equal(Vec3f(0, -2.5, -5)));
-
-  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))));
-  BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, -2.5, -5))));
-
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 2.5, 0)), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 7.5) < 0.001);
-  BOOST_CHECK(normal.equal(Vec3f(0, -1, 0)));
-  BOOST_CHECK(contact.equal(Vec3f(0, -1.25, -5)));
-
-  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 2.5, 0)), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 7.5) < 0.001);
-  BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0))));
-  BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, -1.25, -5))));
-
-  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, -3.75, -5)));
-
-  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, -3.75, -5))));
-
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 5.1, 0)), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 10.1) < 0.001);
-  BOOST_CHECK(normal.equal(Vec3f(0, -1, 0)));
-  BOOST_CHECK(contact.equal(Vec3f(0, 0.05, -5)));
-
-  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 5.1, 0)), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 10.1) < 0.001);
-  BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0))));
-  BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0.05, -5))));
-
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, -5.1, 0)), &contact, &depth, &normal);
-  BOOST_CHECK_FALSE(res);
-
-  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, -5.1, 0)), &contact, &depth, &normal);
-  BOOST_CHECK_FALSE(res);
+  tf1 = Transform3f();
+  tf2 = Transform3f();
+  contact.setValue(0, -2.5, -5);
+  depth = 5;
+  normal.setValue(0, -1, 0);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = transform;
+  tf2 = transform;
+  contact = transform.transform(Vec3f(0, -2.5, -5));
+  depth = 5;
+  normal = transform.getQuatRotation().transform(Vec3f(0, -1, 0));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(0, 2.5, 0));
+  contact.setValue(0, -1.25, -5);
+  depth = 7.5;
+  normal.setValue(0, -1, 0);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = transform;
+  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));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(0, -2.5, 0));
+  contact.setValue(0, -3.75, -5);
+  depth = 2.5;
+  normal.setValue(0, -1, 0);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = transform;
+  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));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(0, 5.1, 0));
+  contact.setValue(0, 0.05, -5);
+  depth = 10.1;
+  normal.setValue(0, -1, 0);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = transform;
+  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));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(0, -5.1, 0));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
+
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(0, -5.1, 0));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
 
 
 
 
   hs = Halfspace(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)));
-  BOOST_CHECK(contact.equal(Vec3f(0, 0, -2.5)));
-
-  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))));
-  BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, -2.5))));
-
-  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, -1.25)));
-
-  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, -1.25))));
-
-  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, -3.75)));
-
-  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, -3.75))));
-
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, 5.1)), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 10.1) < 0.001);
-  BOOST_CHECK(normal.equal(Vec3f(0, 0, -1)));
-  BOOST_CHECK(contact.equal(Vec3f(0, 0, 0.05)));
-
-  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, 5.1)), &contact, &depth, &normal);
-  BOOST_CHECK(res);
-  BOOST_CHECK(std::abs(depth - 10.1) < 0.001);
-  BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1))));
-  BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, 0.05))));
-
-  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, -5.1)), &contact, &depth, &normal);
-  BOOST_CHECK_FALSE(res);
-
-  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, -5.1)), &contact, &depth, &normal);
-  BOOST_CHECK_FALSE(res);
+  tf1 = Transform3f();
+  tf2 = Transform3f();
+  contact.setValue(0, 0, -2.5);
+  depth = 5;
+  normal.setValue(0, 0, -1);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = transform;
+  tf2 = transform;
+  contact = transform.transform(Vec3f(0, 0, -2.5));
+  depth = 5;
+  normal = transform.getQuatRotation().transform(Vec3f(0, 0, -1));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(0, 0, 2.5));
+  contact.setValue(0, 0, -1.25);
+  depth = 7.5;
+  normal.setValue(0, 0, -1);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = transform;
+  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));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(0, 0, -2.5));
+  contact.setValue(0, 0, -3.75);
+  depth = 2.5;
+  normal.setValue(0, 0, -1);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = transform;
+  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));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(0, 0, 5.1));
+  contact.setValue(0, 0, 0.05);
+  depth = 10.1;
+  normal.setValue(0, 0, -1);
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = transform;
+  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));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(0, 0, -5.1));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
+
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(0, 0, -5.1));
+  testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false);
 }
 
 BOOST_AUTO_TEST_CASE(shapeIntersection_planecone)
diff --git a/test/test_fcl_utility.cpp b/test/test_fcl_utility.cpp
index e3bda2d8870ebb4be1550eaec6b228864ebcc291..76f8e952f29abf4c6538fbbf500c50b27afb458c 100644
--- a/test/test_fcl_utility.cpp
+++ b/test/test_fcl_utility.cpp
@@ -400,4 +400,58 @@ bool defaultContinuousDistanceFunction(ContinuousCollisionObject* o1, Continuous
   return true;
 }
 
+std::string getNodeTypeName(NODE_TYPE node_type)
+{
+  if (node_type == BV_UNKNOWN)
+    return std::string("BV_UNKNOWN");
+  else if (node_type == BV_AABB)
+    return std::string("BV_AABB");
+  else if (node_type == BV_OBB)
+    return std::string("BV_OBB");
+  else if (node_type == BV_RSS)
+    return std::string("BV_RSS");
+  else if (node_type == BV_kIOS)
+    return std::string("BV_kIOS");
+  else if (node_type == BV_OBBRSS)
+    return std::string("BV_OBBRSS");
+  else if (node_type == BV_KDOP16)
+    return std::string("BV_KDOP16");
+  else if (node_type == BV_KDOP18)
+    return std::string("BV_KDOP18");
+  else if (node_type == BV_KDOP24)
+    return std::string("BV_KDOP24");
+  else if (node_type == GEOM_BOX)
+    return std::string("GEOM_BOX");
+  else if (node_type == GEOM_SPHERE)
+    return std::string("GEOM_SPHERE");
+  else if (node_type == GEOM_CAPSULE)
+    return std::string("GEOM_CAPSULE");
+  else if (node_type == GEOM_CONE)
+    return std::string("GEOM_CONE");
+  else if (node_type == GEOM_CYLINDER)
+    return std::string("GEOM_CYLINDER");
+  else if (node_type == GEOM_CONVEX)
+    return std::string("GEOM_CONVEX");
+  else if (node_type == GEOM_PLANE)
+    return std::string("GEOM_PLANE");
+  else if (node_type == GEOM_HALFSPACE)
+    return std::string("GEOM_HALFSPACE");
+  else if (node_type == GEOM_TRIANGLE)
+    return std::string("GEOM_TRIANGLE");
+  else if (node_type == GEOM_OCTREE)
+    return std::string("GEOM_OCTREE");
+  else
+    return std::string("invalid");
+}
+
+std::string getGJKSolverName(GJKSolverType solver_type)
+{
+  if (solver_type == GST_LIBCCD)
+    return std::string("libccd");
+  else if (solver_type == GST_INDEP)
+    return std::string("built-in");
+  else
+    return std::string("invalid");
+}
+
 }
diff --git a/test/test_fcl_utility.h b/test/test_fcl_utility.h
index 45d58936011be0c515ad9d610ec92cb1767a50e3..401e16eb438a4d6b7c2c83f0bd176bb183f67f78 100644
--- a/test/test_fcl_utility.h
+++ b/test/test_fcl_utility.h
@@ -182,6 +182,10 @@ bool defaultContinuousCollisionFunction(ContinuousCollisionObject* o1, Continuou
 bool defaultContinuousDistanceFunction(ContinuousCollisionObject* o1, ContinuousCollisionObject* o2, void* cdata_, FCL_REAL& dist);
 
 
+std::string getNodeTypeName(NODE_TYPE node_type);
+
+std::string getGJKSolverName(GJKSolverType solver_type);
+
 }
 
 #endif