diff --git a/CMakeModules/FCLVersion.cmake b/CMakeModules/FCLVersion.cmake
index bb5a8d41c..778de83f9 100644
--- a/CMakeModules/FCLVersion.cmake
+++ b/CMakeModules/FCLVersion.cmake
@@ -1,7 +1,7 @@
 # set the version in a way CMake can use
 set(FCL_MAJOR_VERSION 0)
 set(FCL_MINOR_VERSION 3)
-set(FCL_PATCH_VERSION 2)
+set(FCL_PATCH_VERSION 3)
 set(FCL_VERSION "${FCL_MAJOR_VERSION}.${FCL_MINOR_VERSION}.${FCL_PATCH_VERSION}")
 
 # increment this when we have ABI changes
diff --git a/include/fcl/narrowphase/narrowphase.h b/include/fcl/narrowphase/narrowphase.h
index 5abf8acf6..b4dc1c136 100644
--- a/include/fcl/narrowphase/narrowphase.h
+++ b/include/fcl/narrowphase/narrowphase.h
@@ -249,6 +249,11 @@ bool GJKSolver_libccd::shapeIntersect<Sphere, Capsule>(const Sphere& s1, const T
                                                        const Capsule& s2, const Transform3f& tf2,
                                                        Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
 
+template<>
+bool GJKSolver_libccd::shapeIntersect<Capsule, Sphere>(const Capsule &s1, const Transform3f& tf1,
+                                                       const Sphere &s2, const Transform3f& tf2,
+                                                       Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+
 /// @brief Fast implementation for sphere-sphere collision
 template<>
 bool GJKSolver_libccd::shapeIntersect<Sphere, Sphere>(const Sphere& s1, const Transform3f& tf1,
@@ -266,26 +271,51 @@ bool GJKSolver_libccd::shapeIntersect<Sphere, Halfspace>(const Sphere& s1, const
                                                          const Halfspace& s2, const Transform3f& tf2,
                                                          Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
 
+template<>
+bool GJKSolver_libccd::shapeIntersect<Halfspace, Sphere>(const Halfspace& s1, const Transform3f& tf1,
+                                                         const Sphere& s2, const Transform3f& tf2,
+                                                         Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+
 template<>
 bool GJKSolver_libccd::shapeIntersect<Box, Halfspace>(const Box& s1, const Transform3f& tf1,
                                                       const Halfspace& s2, const Transform3f& tf2,
                                                       Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
 
+template<>
+bool GJKSolver_libccd::shapeIntersect<Halfspace, Box>(const Halfspace& s1, const Transform3f& tf1,
+                                                      const Box& s2, const Transform3f& tf2,
+                                                      Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+
 template<>
 bool GJKSolver_libccd::shapeIntersect<Capsule, Halfspace>(const Capsule& s1, const Transform3f& tf1,
                                                           const Halfspace& s2, const Transform3f& tf2,
                                                           Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
 
+template<>
+bool GJKSolver_libccd::shapeIntersect<Halfspace, Capsule>(const Halfspace& s1, const Transform3f& tf1,
+                                                          const Capsule& s2, const Transform3f& tf2,
+                                                          Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+
 template<>
 bool GJKSolver_libccd::shapeIntersect<Cylinder, Halfspace>(const Cylinder& s1, const Transform3f& tf1,
                                                            const Halfspace& s2, const Transform3f& tf2,
                                                            Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
 
+template<>
+bool GJKSolver_libccd::shapeIntersect<Halfspace, Cylinder>(const Halfspace& s1, const Transform3f& tf1,
+                                                           const Cylinder& s2, const Transform3f& tf2,
+                                                           Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+
 template<>
 bool GJKSolver_libccd::shapeIntersect<Cone, Halfspace>(const Cone& s1, const Transform3f& tf1,
                                                        const Halfspace& s2, const Transform3f& tf2,
                                                        Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
 
+template<>
+bool GJKSolver_libccd::shapeIntersect<Halfspace, Cone>(const Halfspace& s1, const Transform3f& tf1,
+                                                       const Cone& s2, const Transform3f& tf2,
+                                                       Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+
 template<>
 bool GJKSolver_libccd::shapeIntersect<Halfspace, Halfspace>(const Halfspace& s1, const Transform3f& tf1,
                                                             const Halfspace& s2, const Transform3f& tf2,
@@ -296,35 +326,60 @@ bool GJKSolver_libccd::shapeIntersect<Plane, Halfspace>(const Plane& s1, const T
                                                         const Halfspace& s2, const Transform3f& tf2,
                                                         Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
 
+template<>
+bool GJKSolver_libccd::shapeIntersect<Halfspace, Plane>(const Halfspace& s1, const Transform3f& tf1,
+                                                        const Plane& s2, const Transform3f& tf2,
+                                                        Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+
 template<>
 bool GJKSolver_libccd::shapeIntersect<Sphere, Plane>(const Sphere& s1, const Transform3f& tf1,
                                                      const Plane& s2, const Transform3f& tf2,
                                                      Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
 
+template<>
+bool GJKSolver_libccd::shapeIntersect<Plane, Sphere>(const Plane& s1, const Transform3f& tf1,
+                                                     const Sphere& s2, const Transform3f& tf2,
+                                                     Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+
 template<>
 bool GJKSolver_libccd::shapeIntersect<Box, Plane>(const Box& s1, const Transform3f& tf1,
                                                   const Plane& s2, const Transform3f& tf2,
                                                   Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
 
+template<>
+bool GJKSolver_libccd::shapeIntersect<Plane, Box>(const Plane& s1, const Transform3f& tf1,
+                                                  const Box& s2, const Transform3f& tf2,
+                                                  Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+
 template<>
 bool GJKSolver_libccd::shapeIntersect<Capsule, Plane>(const Capsule& s1, const Transform3f& tf1,
                                                       const Plane& s2, const Transform3f& tf2,
                                                       Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
 
+template<>
+bool GJKSolver_libccd::shapeIntersect<Plane, Capsule>(const Plane& s1, const Transform3f& tf1,
+                                                      const Capsule& s2, const Transform3f& tf2,
+                                                      Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+
 template<>
 bool GJKSolver_libccd::shapeIntersect<Cylinder, Plane>(const Cylinder& s1, const Transform3f& tf1,
                                                        const Plane& s2, const Transform3f& tf2,
                                                        Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
 
+template<>
+bool GJKSolver_libccd::shapeIntersect<Plane, Cylinder>(const Plane& s1, const Transform3f& tf1,
+                                                       const Cylinder& s2, const Transform3f& tf2,
+                                                       Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+
 template<>
 bool GJKSolver_libccd::shapeIntersect<Cone, Plane>(const Cone& s1, const Transform3f& tf1,
                                                    const Plane& s2, const Transform3f& tf2,
                                                    Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
 
 template<>
-bool GJKSolver_libccd::shapeIntersect<Halfspace, Plane>(const Halfspace& s1, const Transform3f& tf1,
-                                                        const Plane& s2, const Transform3f& tf2,
-                                                        Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+bool GJKSolver_libccd::shapeIntersect<Plane, Cone>(const Plane& s1, const Transform3f& tf1,
+                                                   const Cone& s2, const Transform3f& tf2,
+                                                   Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
 
 template<>
 bool GJKSolver_libccd::shapeIntersect<Plane, Plane>(const Plane& s1, const Transform3f& tf1,
@@ -356,12 +411,23 @@ bool GJKSolver_libccd::shapeDistance<Sphere, Capsule>(const Sphere& s1, const Tr
                                                       const Capsule& s2, const Transform3f& tf2,
                                                       FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const;
 
+template<>
+bool GJKSolver_libccd::shapeDistance<Capsule, Sphere>(const Capsule& s1, const Transform3f& tf1,
+                                                      const Sphere& s2, const Transform3f& tf2,
+                                                      FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const;
+
 /// @brief Fast implementation for sphere-sphere distance
 template<>
 bool GJKSolver_libccd::shapeDistance<Sphere, Sphere>(const Sphere& s1, const Transform3f& tf1,
                                                      const Sphere& s2, const Transform3f& tf2,
                                                      FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const;
 
+// @brief Computation of the distance result for capsule capsule. Closest points are based on two line-segments.
+template<>
+bool GJKSolver_libccd::shapeDistance<Capsule, Capsule>(const Capsule& s1, const Transform3f& tf1,
+                                                       const Capsule& s2, const Transform3f& tf2,
+                                                       FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const;
+
 /// @brief Fast implementation for sphere-triangle distance
 template<>
 bool GJKSolver_libccd::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf,
@@ -373,12 +439,6 @@ template<>
 bool GJKSolver_libccd::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf1, 
                                                      const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
                                                      FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const;
-// @brief Computation of the distance result for capsule capsule. Closest points are based on two line-segments.
-template<>
-bool GJKSolver_libccd::shapeDistance<Capsule, Capsule>(const Capsule& s1, const Transform3f& tf1,
-						       const Capsule& s2, const Transform3f& tf2,
-						       FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const;
-
 
 /// @brief collision and distance solver based on GJK algorithm implemented in fcl (rewritten the code from the GJK in bullet)
 struct GJKSolver_indep
@@ -732,18 +792,24 @@ struct GJKSolver_indep
   mutable Vec3f cached_guess;
 };
 
+/// @brief Fast implementation for sphere-capsule collision
 template<>
-bool GJKSolver_indep::shapeIntersect<Sphere, Capsule>(const Sphere &s1, const Transform3f& tf1,
-                                                      const Capsule &s2, const Transform3f& tf2,
+bool GJKSolver_indep::shapeIntersect<Sphere, Capsule>(const Sphere& s1, const Transform3f& tf1,
+                                                      const Capsule& s2, const Transform3f& tf2,
                                                       Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
 
-/// @brief Fast implementation for sphere-sphere collision                                            
+template<>
+bool GJKSolver_indep::shapeIntersect<Capsule, Sphere>(const Capsule &s1, const Transform3f& tf1,
+                                                      const Sphere &s2, const Transform3f& tf2,
+                                                      Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+
+/// @brief Fast implementation for sphere-sphere collision
 template<>
 bool GJKSolver_indep::shapeIntersect<Sphere, Sphere>(const Sphere& s1, const Transform3f& tf1,
                                                      const Sphere& s2, const Transform3f& tf2,
                                                      Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
 
-/// @brief Fast implementation for box-box collision                                                 
+/// @brief Fast implementation for box-box collision
 template<>
 bool GJKSolver_indep::shapeIntersect<Box, Box>(const Box& s1, const Transform3f& tf1,
                                                const Box& s2, const Transform3f& tf2,
@@ -754,26 +820,51 @@ bool GJKSolver_indep::shapeIntersect<Sphere, Halfspace>(const Sphere& s1, const
                                                         const Halfspace& s2, const Transform3f& tf2,
                                                         Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
 
+template<>
+bool GJKSolver_indep::shapeIntersect<Halfspace, Sphere>(const Halfspace& s1, const Transform3f& tf1,
+                                                        const Sphere& s2, const Transform3f& tf2,
+                                                        Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+
 template<>
 bool GJKSolver_indep::shapeIntersect<Box, Halfspace>(const Box& s1, const Transform3f& tf1,
                                                      const Halfspace& s2, const Transform3f& tf2,
                                                      Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
 
+template<>
+bool GJKSolver_indep::shapeIntersect<Halfspace, Box>(const Halfspace& s1, const Transform3f& tf1,
+                                                     const Box& s2, const Transform3f& tf2,
+                                                     Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+
 template<>
 bool GJKSolver_indep::shapeIntersect<Capsule, Halfspace>(const Capsule& s1, const Transform3f& tf1,
                                                          const Halfspace& s2, const Transform3f& tf2,
                                                          Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
 
+template<>
+bool GJKSolver_indep::shapeIntersect<Halfspace, Capsule>(const Halfspace& s1, const Transform3f& tf1,
+                                                         const Capsule& s2, const Transform3f& tf2,
+                                                         Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+
 template<>
 bool GJKSolver_indep::shapeIntersect<Cylinder, Halfspace>(const Cylinder& s1, const Transform3f& tf1,
                                                           const Halfspace& s2, const Transform3f& tf2,
                                                           Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
 
+template<>
+bool GJKSolver_indep::shapeIntersect<Halfspace, Cylinder>(const Halfspace& s1, const Transform3f& tf1,
+                                                          const Cylinder& s2, const Transform3f& tf2,
+                                                          Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+
 template<>
 bool GJKSolver_indep::shapeIntersect<Cone, Halfspace>(const Cone& s1, const Transform3f& tf1,
                                                       const Halfspace& s2, const Transform3f& tf2,
                                                       Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
 
+template<>
+bool GJKSolver_indep::shapeIntersect<Halfspace, Cone>(const Halfspace& s1, const Transform3f& tf1,
+                                                      const Cone& s2, const Transform3f& tf2,
+                                                      Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+
 template<>
 bool GJKSolver_indep::shapeIntersect<Halfspace, Halfspace>(const Halfspace& s1, const Transform3f& tf1,
                                                            const Halfspace& s2, const Transform3f& tf2,
@@ -784,35 +875,60 @@ bool GJKSolver_indep::shapeIntersect<Plane, Halfspace>(const Plane& s1, const Tr
                                                        const Halfspace& s2, const Transform3f& tf2,
                                                        Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
 
+template<>
+bool GJKSolver_indep::shapeIntersect<Halfspace, Plane>(const Halfspace& s1, const Transform3f& tf1,
+                                                       const Plane& s2, const Transform3f& tf2,
+                                                       Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+
 template<>
 bool GJKSolver_indep::shapeIntersect<Sphere, Plane>(const Sphere& s1, const Transform3f& tf1,
                                                     const Plane& s2, const Transform3f& tf2,
                                                     Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
 
+template<>
+bool GJKSolver_indep::shapeIntersect<Plane, Sphere>(const Plane& s1, const Transform3f& tf1,
+                                                    const Sphere& s2, const Transform3f& tf2,
+                                                    Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+
 template<>
 bool GJKSolver_indep::shapeIntersect<Box, Plane>(const Box& s1, const Transform3f& tf1,
                                                  const Plane& s2, const Transform3f& tf2,
                                                  Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
 
+template<>
+bool GJKSolver_indep::shapeIntersect<Plane, Box>(const Plane& s1, const Transform3f& tf1,
+                                                 const Box& s2, const Transform3f& tf2,
+                                                 Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+
 template<>
 bool GJKSolver_indep::shapeIntersect<Capsule, Plane>(const Capsule& s1, const Transform3f& tf1,
                                                      const Plane& s2, const Transform3f& tf2,
                                                      Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
 
+template<>
+bool GJKSolver_indep::shapeIntersect<Plane, Capsule>(const Plane& s1, const Transform3f& tf1,
+                                                     const Capsule& s2, const Transform3f& tf2,
+                                                     Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+
 template<>
 bool GJKSolver_indep::shapeIntersect<Cylinder, Plane>(const Cylinder& s1, const Transform3f& tf1,
                                                       const Plane& s2, const Transform3f& tf2,
                                                       Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
 
+template<>
+bool GJKSolver_indep::shapeIntersect<Plane, Cylinder>(const Plane& s1, const Transform3f& tf1,
+                                                      const Cylinder& s2, const Transform3f& tf2,
+                                                      Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+
 template<>
 bool GJKSolver_indep::shapeIntersect<Cone, Plane>(const Cone& s1, const Transform3f& tf1,
                                                   const Plane& s2, const Transform3f& tf2,
                                                   Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
 
 template<>
-bool GJKSolver_indep::shapeIntersect<Halfspace, Plane>(const Halfspace& s1, const Transform3f& tf1,
-                                                       const Plane& s2, const Transform3f& tf2,
-                                                       Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+bool GJKSolver_indep::shapeIntersect<Plane, Cone>(const Plane& s1, const Transform3f& tf1,
+                                                  const Cone& s2, const Transform3f& tf2,
+                                                  Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
 
 template<>
 bool GJKSolver_indep::shapeIntersect<Plane, Plane>(const Plane& s1, const Transform3f& tf1,
@@ -820,15 +936,16 @@ bool GJKSolver_indep::shapeIntersect<Plane, Plane>(const Plane& s1, const Transf
                                                    Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
 
 /// @brief Fast implementation for sphere-triangle collision
-template<> 
+template<>
 bool GJKSolver_indep::shapeTriangleIntersect(const Sphere& s, const Transform3f& tf,
                                              const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
 
 /// @brief Fast implementation for sphere-triangle collision
-template<> 
+template<>
 bool GJKSolver_indep::shapeTriangleIntersect(const Sphere& s, const Transform3f& tf1,
                                              const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
 
+
 template<>
 bool GJKSolver_indep::shapeTriangleIntersect(const Halfspace& s, const Transform3f& tf1,
                                              const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
@@ -837,18 +954,16 @@ template<>
 bool GJKSolver_indep::shapeTriangleIntersect(const Plane& s, const Transform3f& tf1,
                                              const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
 
-
+/// @brief Fast implementation for sphere-capsule distance
 template<>
 bool GJKSolver_indep::shapeDistance<Sphere, Capsule>(const Sphere& s1, const Transform3f& tf1,
                                                      const Capsule& s2, const Transform3f& tf2,
                                                      FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const;
 
-// @brief Computation of the distance result for capsule capsule. Closest points are based on two line-segments.
- template<>
-   bool GJKSolver_indep::shapeDistance<Capsule, Capsule>(const Capsule& s1, const Transform3f& tf1,
-							 const Capsule& s2, const Transform3f& tf2,
-							 FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const;
-
+template<>
+bool GJKSolver_indep::shapeDistance<Capsule, Sphere>(const Capsule& s1, const Transform3f& tf1,
+                                                     const Sphere& s2, const Transform3f& tf2,
+                                                     FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const;
 
 /// @brief Fast implementation for sphere-sphere distance
 template<>
@@ -856,19 +971,24 @@ bool GJKSolver_indep::shapeDistance<Sphere, Sphere>(const Sphere& s1, const Tran
                                                     const Sphere& s2, const Transform3f& tf2,
                                                     FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const;
 
+// @brief Computation of the distance result for capsule capsule. Closest points are based on two line-segments.
+template<>
+bool GJKSolver_indep::shapeDistance<Capsule, Capsule>(const Capsule& s1, const Transform3f& tf1,
+                                                      const Capsule& s2, const Transform3f& tf2,
+                                                      FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const;
+
 /// @brief Fast implementation for sphere-triangle distance
 template<>
 bool GJKSolver_indep::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf,
-                                                    const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, 
+                                                    const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
                                                     FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const;
 
 /// @brief Fast implementation for sphere-triangle distance
-template<> 
-bool GJKSolver_indep::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf1, 
+template<>
+bool GJKSolver_indep::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf1,
                                                     const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
                                                     FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const;
 
-
 }
 
 #endif
diff --git a/src/narrowphase/narrowphase.cpp b/src/narrowphase/narrowphase.cpp
index b701a7ea9..08c9e59d1 100644
--- a/src/narrowphase/narrowphase.cpp
+++ b/src/narrowphase/narrowphase.cpp
@@ -2555,12 +2555,44 @@ bool planeIntersect(const Plane& s1, const Transform3f& tf1,
 
 } // details
 
+// Shape intersect algorithms not using libccd
+//
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+
+// |            | box | sphere | capsule | cone | cylinder | plane | half-space | triangle |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+
+// | box        |  O  |        |         |      |          |   O   |      O     |          |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+
+// | sphere     |/////|   O    |    O    |      |          |   O   |      O     |    O     |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+
+// | capsule    |/////|////////|         |      |          |   O   |      O     |          |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+
+// | cone       |/////|////////|/////////|      |          |   O   |      O     |          |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+
+// | cylinder   |/////|////////|/////////|//////|          |   O   |      O     |          |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+
+// | plane      |/////|////////|/////////|//////|//////////|   O   |      O     |    O     |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+
+// | half-space |/////|////////|/////////|//////|//////////|///////|      O     |    O     |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+
+// | triangle   |/////|////////|/////////|//////|//////////|///////|////////////|          |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+
+
 template<>
 bool GJKSolver_libccd::shapeIntersect<Sphere, Capsule>(const Sphere &s1, const Transform3f& tf1,
                                                        const Capsule &s2, const Transform3f& tf2,
                                                        Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
 {
-  return details::sphereCapsuleIntersect (s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
+  return details::sphereCapsuleIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
+}
+
+template<>
+bool GJKSolver_libccd::shapeIntersect<Capsule, Sphere>(const Capsule &s1, const Transform3f& tf1,
+                                                       const Sphere &s2, const Transform3f& tf2,
+                                                       Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+{
+  const bool res = details::sphereCapsuleIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal);
+  if (normal) (*normal) *= -1.0;
+  return res;
 }
 
 template<>
@@ -2587,6 +2619,16 @@ bool GJKSolver_libccd::shapeIntersect<Sphere, Halfspace>(const Sphere& s1, const
   return details::sphereHalfspaceIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
 }
 
+template<>
+bool GJKSolver_libccd::shapeIntersect<Halfspace, Sphere>(const Halfspace& s1, const Transform3f& tf1,
+                                                         const Sphere& s2, const Transform3f& tf2,
+                                                         Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+{
+  const bool res = details::sphereHalfspaceIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal);
+  if (normal) (*normal) *= -1.0;
+  return res;
+}
+
 template<>
 bool GJKSolver_libccd::shapeIntersect<Box, Halfspace>(const Box& s1, const Transform3f& tf1,
                                                       const Halfspace& s2, const Transform3f& tf2,
@@ -2595,6 +2637,16 @@ bool GJKSolver_libccd::shapeIntersect<Box, Halfspace>(const Box& s1, const Trans
   return details::boxHalfspaceIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
 }
 
+template<>
+bool GJKSolver_libccd::shapeIntersect<Halfspace, Box>(const Halfspace& s1, const Transform3f& tf1,
+                                                      const Box& s2, const Transform3f& tf2,
+                                                      Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+{
+  const bool res = details::boxHalfspaceIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal);
+  if (normal) (*normal) *= -1.0;
+  return res;
+}
+
 template<>
 bool GJKSolver_libccd::shapeIntersect<Capsule, Halfspace>(const Capsule& s1, const Transform3f& tf1,
                                                           const Halfspace& s2, const Transform3f& tf2,
@@ -2603,6 +2655,16 @@ bool GJKSolver_libccd::shapeIntersect<Capsule, Halfspace>(const Capsule& s1, con
   return details::capsuleHalfspaceIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
 }
 
+template<>
+bool GJKSolver_libccd::shapeIntersect<Halfspace, Capsule>(const Halfspace& s1, const Transform3f& tf1,
+                                                          const Capsule& s2, const Transform3f& tf2,
+                                                          Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+{
+  const bool res = details::capsuleHalfspaceIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal);
+  if (normal) (*normal) *= -1.0;
+  return res;
+}
+
 template<>
 bool GJKSolver_libccd::shapeIntersect<Cylinder, Halfspace>(const Cylinder& s1, const Transform3f& tf1,
                                                            const Halfspace& s2, const Transform3f& tf2,
@@ -2611,6 +2673,16 @@ bool GJKSolver_libccd::shapeIntersect<Cylinder, Halfspace>(const Cylinder& s1, c
   return details::cylinderHalfspaceIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
 }
 
+template<>
+bool GJKSolver_libccd::shapeIntersect<Halfspace, Cylinder>(const Halfspace& s1, const Transform3f& tf1,
+                                                           const Cylinder& s2, const Transform3f& tf2,
+                                                           Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+{
+  const bool res = details::cylinderHalfspaceIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal);
+  if (normal) (*normal) *= -1.0;
+  return res;
+}
+
 template<>
 bool GJKSolver_libccd::shapeIntersect<Cone, Halfspace>(const Cone& s1, const Transform3f& tf1,
                                                        const Halfspace& s2, const Transform3f& tf2,
@@ -2619,6 +2691,16 @@ bool GJKSolver_libccd::shapeIntersect<Cone, Halfspace>(const Cone& s1, const Tra
   return details::coneHalfspaceIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
 }
 
+template<>
+bool GJKSolver_libccd::shapeIntersect<Halfspace, Cone>(const Halfspace& s1, const Transform3f& tf1,
+                                                       const Cone& s2, const Transform3f& tf2,
+                                                       Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+{
+  const bool res = details::coneHalfspaceIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal);
+  if (normal) (*normal) *= -1.0;
+  return res;
+}
+
 template<>
 bool GJKSolver_libccd::shapeIntersect<Halfspace, Halfspace>(const Halfspace& s1, const Transform3f& tf1,
                                                             const Halfspace& s2, const Transform3f& tf2,
@@ -2643,6 +2725,18 @@ bool GJKSolver_libccd::shapeIntersect<Plane, Halfspace>(const Plane& s1, const T
   return details::planeHalfspaceIntersect(s1, tf1, s2, tf2, pl, p, d, depth, ret);
 }
 
+template<>
+bool GJKSolver_libccd::shapeIntersect<Halfspace, Plane>(const Halfspace& s1, const Transform3f& tf1,
+                                                        const Plane& s2, const Transform3f& tf2,
+                                                        Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+{
+  Plane pl;
+  Vec3f p, d;
+  FCL_REAL depth;
+  int ret;
+  return details::halfspacePlaneIntersect(s1, tf1, s2, tf2, pl, p, d, depth, ret);
+}
+
 template<>
 bool GJKSolver_libccd::shapeIntersect<Sphere, Plane>(const Sphere& s1, const Transform3f& tf1,
                                                      const Plane& s2, const Transform3f& tf2,
@@ -2651,6 +2745,16 @@ bool GJKSolver_libccd::shapeIntersect<Sphere, Plane>(const Sphere& s1, const Tra
   return details::spherePlaneIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
 }
 
+template<>
+bool GJKSolver_libccd::shapeIntersect<Plane, Sphere>(const Plane& s1, const Transform3f& tf1,
+                                                     const Sphere& s2, const Transform3f& tf2,
+                                                     Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+{
+  const bool res = details::spherePlaneIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal);
+  if (normal) (*normal) *= -1.0;
+  return res;
+}
+
 template<>
 bool GJKSolver_libccd::shapeIntersect<Box, Plane>(const Box& s1, const Transform3f& tf1,
                                                   const Plane& s2, const Transform3f& tf2,
@@ -2659,6 +2763,16 @@ bool GJKSolver_libccd::shapeIntersect<Box, Plane>(const Box& s1, const Transform
   return details::boxPlaneIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
 }
 
+template<>
+bool GJKSolver_libccd::shapeIntersect<Plane, Box>(const Plane& s1, const Transform3f& tf1,
+                                                  const Box& s2, const Transform3f& tf2,
+                                                  Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+{
+  const bool res = details::boxPlaneIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal);
+  if (normal) (*normal) *= -1.0;
+  return res;
+}
+
 template<>
 bool GJKSolver_libccd::shapeIntersect<Capsule, Plane>(const Capsule& s1, const Transform3f& tf1,
                                                       const Plane& s2, const Transform3f& tf2,
@@ -2667,6 +2781,16 @@ bool GJKSolver_libccd::shapeIntersect<Capsule, Plane>(const Capsule& s1, const T
   return details::capsulePlaneIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
 }
 
+template<>
+bool GJKSolver_libccd::shapeIntersect<Plane, Capsule>(const Plane& s1, const Transform3f& tf1,
+                                                      const Capsule& s2, const Transform3f& tf2,
+                                                      Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+{
+  const bool res = details::capsulePlaneIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal);
+  if (normal) (*normal) *= -1.0;
+  return res;
+}
+
 template<>
 bool GJKSolver_libccd::shapeIntersect<Cylinder, Plane>(const Cylinder& s1, const Transform3f& tf1,
                                                        const Plane& s2, const Transform3f& tf2,
@@ -2675,6 +2799,16 @@ bool GJKSolver_libccd::shapeIntersect<Cylinder, Plane>(const Cylinder& s1, const
   return details::cylinderPlaneIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
 }
 
+template<>
+bool GJKSolver_libccd::shapeIntersect<Plane, Cylinder>(const Plane& s1, const Transform3f& tf1,
+                                                       const Cylinder& s2, const Transform3f& tf2,
+                                                       Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+{
+  const bool res = details::cylinderPlaneIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal);
+  if (normal) (*normal) *= -1.0;
+  return res;
+}
+
 template<>
 bool GJKSolver_libccd::shapeIntersect<Cone, Plane>(const Cone& s1, const Transform3f& tf1,
                                                    const Plane& s2, const Transform3f& tf2,
@@ -2684,15 +2818,13 @@ bool GJKSolver_libccd::shapeIntersect<Cone, Plane>(const Cone& s1, const Transfo
 }
 
 template<>
-bool GJKSolver_libccd::shapeIntersect<Halfspace, Plane>(const Halfspace& s1, const Transform3f& tf1,
-                                                        const Plane& s2, const Transform3f& tf2,
-                                                        Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+bool GJKSolver_libccd::shapeIntersect<Plane, Cone>(const Plane& s1, const Transform3f& tf1,
+                                                   const Cone& s2, const Transform3f& tf2,
+                                                   Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
 {
-  Plane pl;
-  Vec3f p, d;
-  FCL_REAL depth;
-  int ret;
-  return details::halfspacePlaneIntersect(s1, tf1, s2, tf2, pl, p, d, depth, ret);
+  const bool res = details::conePlaneIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal);
+  if (normal) (*normal) *= -1.0;
+  return res;
 }
 
 template<>
@@ -2704,6 +2836,8 @@ bool GJKSolver_libccd::shapeIntersect<Plane, Plane>(const Plane& s1, const Trans
 }
 
 
+
+
 template<> 
 bool GJKSolver_libccd::shapeTriangleIntersect(const Sphere& s, const Transform3f& tf,
                                               const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
@@ -2718,7 +2852,6 @@ bool GJKSolver_libccd::shapeTriangleIntersect(const Sphere& s, const Transform3f
   return details::sphereTriangleIntersect(s, tf1, tf2.transform(P1), tf2.transform(P2), tf2.transform(P3), contact_points, penetration_depth, normal);
 }
 
-
 template<>
 bool GJKSolver_libccd::shapeTriangleIntersect(const Halfspace& s, const Transform3f& tf1,
                                               const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
@@ -2733,8 +2866,27 @@ bool GJKSolver_libccd::shapeTriangleIntersect(const Plane& s, const Transform3f&
   return details::planeTriangleIntersect(s, tf1, P1, P2, P3, tf2, contact_points, penetration_depth, normal);
 }
 
-
-
+// Shape distance algorithms not using libccd
+//
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+
+// |            | box | sphere | capsule | cone | cylinder | plane | half-space | triangle |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+
+// | box        |     |        |         |      |          |       |            |          |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+
+// | sphere     |/////|   O    |    O    |      |          |       |            |    O     |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+
+// | capsule    |/////|////////|    O    |      |          |       |            |          |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+
+// | cone       |/////|////////|/////////|      |          |       |            |          |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+
+// | cylinder   |/////|////////|/////////|//////|          |       |            |          |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+
+// | plane      |/////|////////|/////////|//////|//////////|       |            |          |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+
+// | half-space |/////|////////|/////////|//////|//////////|///////|            |          |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+
+// | triangle   |/////|////////|/////////|//////|//////////|///////|////////////|          |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+
 
 template<>
 bool GJKSolver_libccd::shapeDistance<Sphere, Capsule>(const Sphere& s1, const Transform3f& tf1,
@@ -2744,6 +2896,14 @@ bool GJKSolver_libccd::shapeDistance<Sphere, Capsule>(const Sphere& s1, const Tr
   return details::sphereCapsuleDistance(s1, tf1, s2, tf2, dist, p1, p2);
 }
 
+template<>
+bool GJKSolver_libccd::shapeDistance<Capsule, Sphere>(const Capsule& s1, const Transform3f& tf1,
+                                                      const Sphere& s2, const Transform3f& tf2,
+                                                      FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const
+{
+  return details::sphereCapsuleDistance(s2, tf2, s1, tf1, dist, p2, p1);
+}
+
 template<>
 bool GJKSolver_libccd::shapeDistance<Sphere, Sphere>(const Sphere& s1, const Transform3f& tf1,
                                                      const Sphere& s2, const Transform3f& tf2,
@@ -2752,32 +2912,71 @@ bool GJKSolver_libccd::shapeDistance<Sphere, Sphere>(const Sphere& s1, const Tra
   return details::sphereSphereDistance(s1, tf1, s2, tf2, dist, p1, p2);
 }
 
+template<>
+bool GJKSolver_libccd::shapeDistance<Capsule, Capsule>(const Capsule& s1, const Transform3f& tf1,
+                                                       const Capsule& s2, const Transform3f& tf2,
+                                                       FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const
+{
+  return details::capsuleCapsuleDistance(s1, tf1, s2, tf2, dist, p1, p2);
+}
+
+
+
+
 template<>
 bool GJKSolver_libccd::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf,
-                                                     const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, 
+                                                     const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
                                                      FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const
 {
   return details::sphereTriangleDistance(s, tf, P1, P2, P3, dist, p1, p2);
 }
 
-template<> 
-bool GJKSolver_libccd::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf1, 
+template<>
+bool GJKSolver_libccd::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf1,
                                                      const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
                                                      FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const
 {
   return details::sphereTriangleDistance(s, tf1, P1, P2, P3, tf2, dist, p1, p2);
 }
 
-
-
-
+// Shape intersect algorithms not using built-in GJK algorithm
+//
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+
+// |            | box | sphere | capsule | cone | cylinder | plane | half-space | triangle |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+
+// | box        |  O  |        |         |      |          |   O   |      O     |          |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+
+// | sphere     |/////|   O    |    O    |      |          |   O   |      O     |    O     |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+
+// | capsule    |/////|////////|         |      |          |   O   |      O     |          |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+
+// | cone       |/////|////////|/////////|      |          |   O   |      O     |          |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+
+// | cylinder   |/////|////////|/////////|//////|          |   O   |      O     |          |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+
+// | plane      |/////|////////|/////////|//////|//////////|   O   |      O     |    O     |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+
+// | half-space |/////|////////|/////////|//////|//////////|///////|      O     |    O     |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+
+// | triangle   |/////|////////|/////////|//////|//////////|///////|////////////|          |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+
 
 template<>
 bool GJKSolver_indep::shapeIntersect<Sphere, Capsule>(const Sphere &s1, const Transform3f& tf1,
                                                       const Capsule &s2, const Transform3f& tf2,
                                                       Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
 {
-  return details::sphereCapsuleIntersect (s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
+  return details::sphereCapsuleIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
+}
+
+template<>
+bool GJKSolver_indep::shapeIntersect<Capsule, Sphere>(const Capsule &s1, const Transform3f& tf1,
+                                                      const Sphere &s2, const Transform3f& tf2,
+                                                      Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+{
+  const bool res = details::sphereCapsuleIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal);
+  if (normal) (*normal) *= -1.0;
+  return res;
 }
 
 template<>
@@ -2804,6 +3003,16 @@ bool GJKSolver_indep::shapeIntersect<Sphere, Halfspace>(const Sphere& s1, const
   return details::sphereHalfspaceIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
 }
 
+template<>
+bool GJKSolver_indep::shapeIntersect<Halfspace, Sphere>(const Halfspace& s1, const Transform3f& tf1,
+                                                        const Sphere& s2, const Transform3f& tf2,
+                                                        Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+{
+  const bool res = details::sphereHalfspaceIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal);
+  if (normal) (*normal) *= -1.0;
+  return res;
+}
+
 template<>
 bool GJKSolver_indep::shapeIntersect<Box, Halfspace>(const Box& s1, const Transform3f& tf1,
                                                      const Halfspace& s2, const Transform3f& tf2,
@@ -2812,6 +3021,16 @@ bool GJKSolver_indep::shapeIntersect<Box, Halfspace>(const Box& s1, const Transf
   return details::boxHalfspaceIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
 }
 
+template<>
+bool GJKSolver_indep::shapeIntersect<Halfspace, Box>(const Halfspace& s1, const Transform3f& tf1,
+                                                     const Box& s2, const Transform3f& tf2,
+                                                     Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+{
+  const bool res = details::boxHalfspaceIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal);
+  if (normal) (*normal) *= -1.0;
+  return res;
+}
+
 template<>
 bool GJKSolver_indep::shapeIntersect<Capsule, Halfspace>(const Capsule& s1, const Transform3f& tf1,
                                                          const Halfspace& s2, const Transform3f& tf2,
@@ -2820,6 +3039,16 @@ bool GJKSolver_indep::shapeIntersect<Capsule, Halfspace>(const Capsule& s1, cons
   return details::capsuleHalfspaceIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
 }
 
+template<>
+bool GJKSolver_indep::shapeIntersect<Halfspace, Capsule>(const Halfspace& s1, const Transform3f& tf1,
+                                                         const Capsule& s2, const Transform3f& tf2,
+                                                         Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+{
+  const bool res = details::capsuleHalfspaceIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal);
+  if (normal) (*normal) *= -1.0;
+  return res;
+}
+
 template<>
 bool GJKSolver_indep::shapeIntersect<Cylinder, Halfspace>(const Cylinder& s1, const Transform3f& tf1,
                                                           const Halfspace& s2, const Transform3f& tf2,
@@ -2828,6 +3057,16 @@ bool GJKSolver_indep::shapeIntersect<Cylinder, Halfspace>(const Cylinder& s1, co
   return details::cylinderHalfspaceIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
 }
 
+template<>
+bool GJKSolver_indep::shapeIntersect<Halfspace, Cylinder>(const Halfspace& s1, const Transform3f& tf1,
+                                                          const Cylinder& s2, const Transform3f& tf2,
+                                                          Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+{
+  const bool res = details::cylinderHalfspaceIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal);
+  if (normal) (*normal) *= -1.0;
+  return res;
+}
+
 template<>
 bool GJKSolver_indep::shapeIntersect<Cone, Halfspace>(const Cone& s1, const Transform3f& tf1,
                                                       const Halfspace& s2, const Transform3f& tf2,
@@ -2836,6 +3075,16 @@ bool GJKSolver_indep::shapeIntersect<Cone, Halfspace>(const Cone& s1, const Tran
   return details::coneHalfspaceIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
 }
 
+template<>
+bool GJKSolver_indep::shapeIntersect<Halfspace, Cone>(const Halfspace& s1, const Transform3f& tf1,
+                                                      const Cone& s2, const Transform3f& tf2,
+                                                      Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+{
+  const bool res = details::coneHalfspaceIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal);
+  if (normal) (*normal) *= -1.0;
+  return res;
+}
+
 template<>
 bool GJKSolver_indep::shapeIntersect<Halfspace, Halfspace>(const Halfspace& s1, const Transform3f& tf1,
                                                            const Halfspace& s2, const Transform3f& tf2,
@@ -2845,7 +3094,6 @@ bool GJKSolver_indep::shapeIntersect<Halfspace, Halfspace>(const Halfspace& s1,
   Vec3f p, d;
   FCL_REAL depth;
   int ret;
-  
   return details::halfspaceIntersect(s1, tf1, s2, tf2, p, d, s, depth, ret);
 }
 
@@ -2861,6 +3109,18 @@ bool GJKSolver_indep::shapeIntersect<Plane, Halfspace>(const Plane& s1, const Tr
   return details::planeHalfspaceIntersect(s1, tf1, s2, tf2, pl, p, d, depth, ret);
 }
 
+template<>
+bool GJKSolver_indep::shapeIntersect<Halfspace, Plane>(const Halfspace& s1, const Transform3f& tf1,
+                                                       const Plane& s2, const Transform3f& tf2,
+                                                       Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+{
+  Plane pl;
+  Vec3f p, d;
+  FCL_REAL depth;
+  int ret;
+  return details::halfspacePlaneIntersect(s1, tf1, s2, tf2, pl, p, d, depth, ret);
+}
+
 template<>
 bool GJKSolver_indep::shapeIntersect<Sphere, Plane>(const Sphere& s1, const Transform3f& tf1,
                                                     const Plane& s2, const Transform3f& tf2,
@@ -2869,6 +3129,16 @@ bool GJKSolver_indep::shapeIntersect<Sphere, Plane>(const Sphere& s1, const Tran
   return details::spherePlaneIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
 }
 
+template<>
+bool GJKSolver_indep::shapeIntersect<Plane, Sphere>(const Plane& s1, const Transform3f& tf1,
+                                                    const Sphere& s2, const Transform3f& tf2,
+                                                    Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+{
+  const bool res = details::spherePlaneIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal);
+  if (normal) (*normal) *= -1.0;
+  return res;
+}
+
 template<>
 bool GJKSolver_indep::shapeIntersect<Box, Plane>(const Box& s1, const Transform3f& tf1,
                                                  const Plane& s2, const Transform3f& tf2,
@@ -2877,6 +3147,16 @@ bool GJKSolver_indep::shapeIntersect<Box, Plane>(const Box& s1, const Transform3
   return details::boxPlaneIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
 }
 
+template<>
+bool GJKSolver_indep::shapeIntersect<Plane, Box>(const Plane& s1, const Transform3f& tf1,
+                                                 const Box& s2, const Transform3f& tf2,
+                                                 Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+{
+  const bool res = details::boxPlaneIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal);
+  if (normal) (*normal) *= -1.0;
+  return res;
+}
+
 template<>
 bool GJKSolver_indep::shapeIntersect<Capsule, Plane>(const Capsule& s1, const Transform3f& tf1,
                                                      const Plane& s2, const Transform3f& tf2,
@@ -2885,6 +3165,16 @@ bool GJKSolver_indep::shapeIntersect<Capsule, Plane>(const Capsule& s1, const Tr
   return details::capsulePlaneIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
 }
 
+template<>
+bool GJKSolver_indep::shapeIntersect<Plane, Capsule>(const Plane& s1, const Transform3f& tf1,
+                                                     const Capsule& s2, const Transform3f& tf2,
+                                                     Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+{
+  const bool res = details::capsulePlaneIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal);
+  if (normal) (*normal) *= -1.0;
+  return res;
+}
+
 template<>
 bool GJKSolver_indep::shapeIntersect<Cylinder, Plane>(const Cylinder& s1, const Transform3f& tf1,
                                                       const Plane& s2, const Transform3f& tf2,
@@ -2893,6 +3183,16 @@ bool GJKSolver_indep::shapeIntersect<Cylinder, Plane>(const Cylinder& s1, const
   return details::cylinderPlaneIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
 }
 
+template<>
+bool GJKSolver_indep::shapeIntersect<Plane, Cylinder>(const Plane& s1, const Transform3f& tf1,
+                                                      const Cylinder& s2, const Transform3f& tf2,
+                                                      Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+{
+  const bool res = details::cylinderPlaneIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal);
+  if (normal) (*normal) *= -1.0;
+  return res;
+}
+
 template<>
 bool GJKSolver_indep::shapeIntersect<Cone, Plane>(const Cone& s1, const Transform3f& tf1,
                                                   const Plane& s2, const Transform3f& tf2,
@@ -2902,15 +3202,13 @@ bool GJKSolver_indep::shapeIntersect<Cone, Plane>(const Cone& s1, const Transfor
 }
 
 template<>
-bool GJKSolver_indep::shapeIntersect<Halfspace, Plane>(const Halfspace& s1, const Transform3f& tf1,
-                                                       const Plane& s2, const Transform3f& tf2,
-                                                       Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+bool GJKSolver_indep::shapeIntersect<Plane, Cone>(const Plane& s1, const Transform3f& tf1,
+                                                  const Cone& s2, const Transform3f& tf2,
+                                                  Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
 {
-  Plane pl;
-  Vec3f p, d;
-  FCL_REAL depth;
-  int ret;
-  return details::halfspacePlaneIntersect(s1, tf1, s2, tf2, pl, p, d, depth, ret);
+  const bool res = details::conePlaneIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal);
+  if (normal) (*normal) *= -1.0;
+  return res;
 }
 
 template<>
@@ -2922,14 +3220,16 @@ bool GJKSolver_indep::shapeIntersect<Plane, Plane>(const Plane& s1, const Transf
 }
 
 
-template<> 
+
+
+template<>
 bool GJKSolver_indep::shapeTriangleIntersect(const Sphere& s, const Transform3f& tf,
                                              const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
 {
   return details::sphereTriangleIntersect(s, tf, P1, P2, P3, contact_points, penetration_depth, normal);
 }
 
-template<> 
+template<>
 bool GJKSolver_indep::shapeTriangleIntersect(const Sphere& s, const Transform3f& tf1,
                                              const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
 {
@@ -2950,6 +3250,27 @@ bool GJKSolver_indep::shapeTriangleIntersect(const Plane& s, const Transform3f&
   return details::planeTriangleIntersect(s, tf1, P1, P2, P3, tf2, contact_points, penetration_depth, normal);
 }
 
+// Shape distance algorithms not using built-in GJK algorithm
+//
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+
+// |            | box | sphere | capsule | cone | cylinder | plane | half-space | triangle |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+
+// | box        |     |        |         |      |          |       |            |          |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+
+// | sphere     |/////|   O    |    O    |      |          |       |            |    O     |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+
+// | capsule    |/////|////////|    O    |      |          |       |            |          |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+
+// | cone       |/////|////////|/////////|      |          |       |            |          |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+
+// | cylinder   |/////|////////|/////////|//////|          |       |            |          |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+
+// | plane      |/////|////////|/////////|//////|//////////|       |            |          |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+
+// | half-space |/////|////////|/////////|//////|//////////|///////|            |          |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+
+// | triangle   |/////|////////|/////////|//////|//////////|///////|////////////|          |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+
 
 template<>
 bool GJKSolver_indep::shapeDistance<Sphere, Capsule>(const Sphere& s1, const Transform3f& tf1,
@@ -2959,6 +3280,14 @@ bool GJKSolver_indep::shapeDistance<Sphere, Capsule>(const Sphere& s1, const Tra
   return details::sphereCapsuleDistance(s1, tf1, s2, tf2, dist, p1, p2);
 }
 
+template<>
+bool GJKSolver_indep::shapeDistance<Capsule, Sphere>(const Capsule& s1, const Transform3f& tf1,
+                                                     const Sphere& s2, const Transform3f& tf2,
+                                                     FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const
+{
+  return details::sphereCapsuleDistance(s2, tf2, s1, tf1, dist, p2, p1);
+}
+
 template<>
 bool GJKSolver_indep::shapeDistance<Sphere, Sphere>(const Sphere& s1, const Transform3f& tf1,
                                                     const Sphere& s2, const Transform3f& tf2,
@@ -2968,36 +3297,30 @@ bool GJKSolver_indep::shapeDistance<Sphere, Sphere>(const Sphere& s1, const Tran
 }
 
 template<>
-bool GJKSolver_indep::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf,
-                                                    const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, 
-                                                    FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const
+bool GJKSolver_indep::shapeDistance<Capsule, Capsule>(const Capsule& s1, const Transform3f& tf1,
+                                                      const Capsule& s2, const Transform3f& tf2,
+                                                      FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const
 {
-  return details::sphereTriangleDistance(s, tf, P1, P2, P3, dist, p1, p2);
+  return details::capsuleCapsuleDistance(s1, tf1, s2, tf2, dist, p1, p2);
 }
 
-template<> 
-bool GJKSolver_indep::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf1, 
-                                                    const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
-                                                    FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const
-{
-  return details::sphereTriangleDistance(s, tf1, P1, P2, P3, tf2, dist, p1, p2);
-}
+
 
 
 template<>
-bool GJKSolver_indep::shapeDistance<Capsule, Capsule>(const Capsule& s1, const Transform3f& tf1,
-							const Capsule& s2, const Transform3f& tf2,
-							FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const
+bool GJKSolver_indep::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf,
+                                                    const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
+                                                    FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const
 {
-  return details::capsuleCapsuleDistance(s1, tf1, s2, tf2, dist, p1, p2);
+  return details::sphereTriangleDistance(s, tf, P1, P2, P3, dist, p1, p2);
 }
 
 template<>
-bool GJKSolver_libccd::shapeDistance<Capsule, Capsule>(const Capsule& s1, const Transform3f& tf1,
-						       const Capsule& s2, const Transform3f& tf2,
-						       FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const
+bool GJKSolver_indep::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf1,
+                                                    const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
+                                                    FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const
 {
-  return details::capsuleCapsuleDistance(s1, tf1, s2, tf2, dist, p1, p2);
+  return details::sphereTriangleDistance(s, tf1, P1, P2, P3, tf2, dist, p1, p2);
 }
 
 } // fcl
diff --git a/test/test_fcl_geometric_shapes.cpp b/test/test_fcl_geometric_shapes.cpp
index 624fff11c..8c60adfaa 100644
--- a/test/test_fcl_geometric_shapes.cpp
+++ b/test/test_fcl_geometric_shapes.cpp
@@ -64,7 +64,7 @@ BOOST_AUTO_TEST_CASE(gjkcache)
 
   TranslationMotion motion(Transform3f(Vec3f(-20.0, -20.0, -20.0)), Transform3f(Vec3f(20.0, 20.0, 20.0)));
 
-  int N = 1000;  
+  int N = 1000;
   FCL_REAL dt = 1.0 / (N - 1);
 
   /// test exploiting spatial coherence
@@ -659,7 +659,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacebox)
 {
   Box s(5, 10, 20);
   Halfspace hs(Vec3f(1, 0, 0), 0);
-  
+
   Transform3f transform;
   generateRandomTransform(extents, transform);
 
@@ -730,7 +730,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planebox)
 {
   Box s(5, 10, 20);
   Plane hs(Vec3f(1, 0, 0), 0);
-  
+
   Transform3f transform;
   generateRandomTransform(extents, transform);
 
@@ -827,7 +827,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecapsule)
   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);
@@ -1013,7 +1013,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecapsule)
   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);
@@ -1157,7 +1157,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecylinder)
   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);
@@ -1181,7 +1181,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecylinder)
   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);
@@ -1367,7 +1367,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecylinder)
   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);
@@ -1512,7 +1512,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecone)
   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);
@@ -1536,7 +1536,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecone)
   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);
@@ -1722,7 +1722,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecone)
   BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
   BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(1, 0, 0))));
   BOOST_CHECK(contact.equal(transform.transform(Vec3f(2.5, 0, -2.5))));
-  
+
   res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-2.5, 0, 0)), &contact, &depth, &normal);
   BOOST_CHECK(res);
   BOOST_CHECK(std::abs(depth - 2.5) < 0.001);
@@ -1857,7 +1857,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecone)
 
 
 BOOST_AUTO_TEST_CASE(shapeDistance_spheresphere)
-{  
+{
   Sphere s1(20);
   Sphere s2(10);
 
@@ -1919,7 +1919,7 @@ BOOST_AUTO_TEST_CASE(shapeDistance_spheresphere)
 }
 
 BOOST_AUTO_TEST_CASE(shapeDistance_boxbox)
-{                      
+{
   Box s1(20, 40, 50);
   Box s2(10, 10, 10);
   Vec3f closest_p1, closest_p2;
@@ -2006,7 +2006,7 @@ BOOST_AUTO_TEST_CASE(shapeDistance_boxsphere)
   res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(22.6, 0, 0)), &dist);
   BOOST_CHECK(fabs(dist - 0.1) < 0.001);
   BOOST_CHECK(res);
-  
+
   res = solver1.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(22.6, 0, 0)), &dist);
   BOOST_CHECK(fabs(dist - 0.1) < 0.05);
   BOOST_CHECK(res);
@@ -2014,7 +2014,7 @@ BOOST_AUTO_TEST_CASE(shapeDistance_boxsphere)
   res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), &dist);
   BOOST_CHECK(fabs(dist - 17.5) < 0.001);
   BOOST_CHECK(res);
-  
+
   res = solver1.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)), &dist);
   BOOST_CHECK(fabs(dist - 17.5) < 0.001);
   BOOST_CHECK(res);
@@ -2145,13 +2145,13 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spheresphere)
 
   Vec3f contact;
   FCL_REAL penetration_depth;
-  Vec3f normal;  
+  Vec3f normal;
   bool res;
 
   request.gjk_solver_type = GST_INDEP; // use indep GJK solver
 
   res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), NULL, NULL, NULL);
-  BOOST_CHECK_FALSE(res); 
+  BOOST_CHECK_FALSE(res);
   res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), &contact, &penetration_depth, &normal);
   BOOST_CHECK_FALSE(res);
   result.clear();
@@ -2267,7 +2267,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_boxbox)
 
   Vec3f contact;
   FCL_REAL penetration_depth;
-  Vec3f normal;  
+  Vec3f normal;
   bool res;
 
   res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL, NULL, NULL);
@@ -2296,7 +2296,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_boxbox)
   BOOST_CHECK(res);
   res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(q), &contact, &penetration_depth, &normal);
   BOOST_CHECK(res);
-  
+
   res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(q), NULL, NULL, NULL);
   BOOST_CHECK(res);
   res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(q), &contact, &penetration_depth, &normal);
@@ -2313,7 +2313,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spherebox)
 
   Vec3f contact;
   FCL_REAL penetration_depth;
-  Vec3f normal;  
+  Vec3f normal;
   bool res;
 
   res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL, NULL, NULL);
@@ -2357,7 +2357,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_cylindercylinder)
 
   Vec3f contact;
   FCL_REAL penetration_depth;
-  Vec3f normal;  
+  Vec3f normal;
   bool res;
 
   res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL, NULL, NULL);
@@ -2401,7 +2401,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_conecone)
 
   Vec3f contact;
   FCL_REAL penetration_depth;
-  Vec3f normal;  
+  Vec3f normal;
   bool res;
 
   res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL, NULL, NULL);
@@ -2455,7 +2455,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_conecylinder)
 
   Vec3f contact;
   FCL_REAL penetration_depth;
-  Vec3f normal;  
+  Vec3f normal;
   bool res;
 
   res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL, NULL, NULL);
@@ -2543,7 +2543,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spheretriangle)
 
 
 BOOST_AUTO_TEST_CASE(spheresphere)
-{  
+{
   Sphere s1(20);
   Sphere s2(10);
 
@@ -2552,7 +2552,7 @@ BOOST_AUTO_TEST_CASE(spheresphere)
 
   bool res;
   FCL_REAL dist = -1;
-  
+
   res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), &dist);
   BOOST_CHECK(fabs(dist - 10) < 0.001);
   BOOST_CHECK(res);
@@ -2604,7 +2604,7 @@ BOOST_AUTO_TEST_CASE(spheresphere)
 }
 
 BOOST_AUTO_TEST_CASE(boxbox)
-{                      
+{
   Box s1(20, 40, 50);
   Box s2(10, 10, 10);
 
@@ -2661,7 +2661,7 @@ BOOST_AUTO_TEST_CASE(boxsphere)
   res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(22.6, 0, 0)), &dist);
   BOOST_CHECK(fabs(dist - 0.1) < 0.01);
   BOOST_CHECK(res);
-  
+
   res = solver2.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(22.6, 0, 0)), &dist);
   BOOST_CHECK(fabs(dist - 0.1) < 0.01);
   BOOST_CHECK(res);
@@ -2669,7 +2669,7 @@ BOOST_AUTO_TEST_CASE(boxsphere)
   res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), &dist);
   BOOST_CHECK(fabs(dist - 17.5) < 0.001);
   BOOST_CHECK(res);
-  
+
   res = solver2.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)), &dist);
   BOOST_CHECK(fabs(dist - 17.5) < 0.001);
   BOOST_CHECK(res);
@@ -2751,4 +2751,176 @@ BOOST_AUTO_TEST_CASE(conecone)
 
 
 
+template<typename S1, typename S2>
+void testReversibleShapeIntersection(const S1& s1, const S2& s2, FCL_REAL distance)
+{
+  Transform3f tf1(Vec3f(-0.5 * distance, 0.0, 0.0));
+  Transform3f tf2(Vec3f(+0.5 * distance, 0.0, 0.0));
+
+  Vec3f contactA;
+  Vec3f contactB;
+  FCL_REAL depthA;
+  FCL_REAL depthB;
+  Vec3f normalA;
+  Vec3f normalB;
+
+  bool resA;
+  bool resB;
+
+  const double tol = 1e-6;
+
+  resA = solver1.shapeIntersect(s1, tf1, s2, tf2, &contactA, &depthA, &normalA);
+  resB = solver1.shapeIntersect(s2, tf2, s1, tf1, &contactB, &depthB, &normalB);
+
+  BOOST_CHECK(resA);
+  BOOST_CHECK(resB);
+  BOOST_CHECK(contactA.equal(contactB, tol));  // contact point should be same
+  BOOST_CHECK(normalA.equal(-normalB, tol));  // normal should be opposite
+  BOOST_CHECK_CLOSE(depthA, depthB, tol);  // penetration depth should be same
+
+  resA = solver2.shapeIntersect(s1, tf1, s2, tf2, &contactA, &depthA, &normalA);
+  resB = solver2.shapeIntersect(s2, tf2, s1, tf1, &contactB, &depthB, &normalB);
+
+  BOOST_CHECK(resA);
+  BOOST_CHECK(resB);
+  BOOST_CHECK(contactA.equal(contactB, tol));
+  BOOST_CHECK(normalA.equal(-normalB, tol));
+  BOOST_CHECK_CLOSE(depthA, depthB, tol);
+}
+
+BOOST_AUTO_TEST_CASE(reversibleShapeIntersection_allshapes)
+{
+  // This test check whether a shape intersection algorithm is called for the
+  // reverse case as well. For example, if FCL has sphere-capsule intersection
+  // algorithm, then this algorithm should be called for capsule-sphere case.
+
+  // Prepare all kinds of primitive shapes (7) -- box, sphere, capsule, cone, cylinder, plane, halfspace
+  Box box(10, 10, 10);
+  Sphere sphere(5);
+  Capsule capsule(5, 10);
+  Cone cone(5, 10);
+  Cylinder cylinder(5, 10);
+  Plane plane(Vec3f(), 0.0);
+  Halfspace halfspace(Vec3f(), 0.0);
+
+  // Use sufficiently short distance so that all the primitive shapes can intersect
+  FCL_REAL distance = 5.0;
+
+  // If new shape intersection algorithm is added for two distinct primitive
+  // shapes, uncomment associated lines. For example, box-sphere intersection
+  // algorithm is added, then uncomment box-sphere.
+
+//  testReversibleShapeIntersection(box, sphere, distance);
+//  testReversibleShapeIntersection(box, capsule, distance);
+//  testReversibleShapeIntersection(box, cone, distance);
+//  testReversibleShapeIntersection(box, cylinder, distance);
+  testReversibleShapeIntersection(box, plane, distance);
+  testReversibleShapeIntersection(box, halfspace, distance);
+
+  testReversibleShapeIntersection(sphere, capsule, distance);
+//  testReversibleShapeIntersection(sphere, cone, distance);
+//  testReversibleShapeIntersection(sphere, cylinder, distance);
+  testReversibleShapeIntersection(sphere, plane, distance);
+  testReversibleShapeIntersection(sphere, halfspace, distance);
+
+//  testReversibleShapeIntersection(capsule, cone, distance);
+//  testReversibleShapeIntersection(capsule, cylinder, distance);
+  testReversibleShapeIntersection(capsule, plane, distance);
+  testReversibleShapeIntersection(capsule, halfspace, distance);
+
+//  testReversibleShapeIntersection(cone, cylinder, distance);
+  testReversibleShapeIntersection(cone, plane, distance);
+  testReversibleShapeIntersection(cone, halfspace, distance);
+
+  testReversibleShapeIntersection(cylinder, plane, distance);
+  testReversibleShapeIntersection(cylinder, halfspace, distance);
+
+  testReversibleShapeIntersection(plane, halfspace, distance);
+}
 
+template<typename S1, typename S2>
+void testReversibleShapeDistance(const S1& s1, const S2& s2, FCL_REAL distance)
+{
+  Transform3f tf1(Vec3f(-0.5 * distance, 0.0, 0.0));
+  Transform3f tf2(Vec3f(+0.5 * distance, 0.0, 0.0));
+
+  FCL_REAL distA;
+  FCL_REAL distB;
+  Vec3f p1A;
+  Vec3f p1B;
+  Vec3f p2A;
+  Vec3f p2B;
+
+  bool resA;
+  bool resB;
+
+  const double tol = 1e-6;
+
+  resA = solver1.shapeDistance(s1, tf1, s2, tf2, &distA, &p1A, &p2A);
+  resB = solver1.shapeDistance(s2, tf2, s1, tf1, &distB, &p1B, &p2B);
+
+  BOOST_CHECK(resA);
+  BOOST_CHECK(resB);
+  BOOST_CHECK_CLOSE(distA, distB, tol);  // distances should be same
+  BOOST_CHECK(p1A.equal(p2B, tol));  // closest points should in reverse order
+  BOOST_CHECK(p2A.equal(p1B, tol));
+
+  resA = solver2.shapeDistance(s1, tf1, s2, tf2, &distA, &p1A, &p2A);
+  resB = solver2.shapeDistance(s2, tf2, s1, tf1, &distB, &p1B, &p2B);
+
+  BOOST_CHECK(resA);
+  BOOST_CHECK(resB);
+  BOOST_CHECK_CLOSE(distA, distB, tol);
+  BOOST_CHECK(p1A.equal(p2B, tol));
+  BOOST_CHECK(p2A.equal(p1B, tol));
+}
+
+BOOST_AUTO_TEST_CASE(reversibleShapeDistance_allshapes)
+{
+  // This test check whether a shape distance algorithm is called for the
+  // reverse case as well. For example, if FCL has sphere-capsule distance
+  // algorithm, then this algorithm should be called for capsule-sphere case.
+
+  // Prepare all kinds of primitive shapes (7) -- box, sphere, capsule, cone, cylinder, plane, halfspace
+  Box box(10, 10, 10);
+  Sphere sphere(5);
+  Capsule capsule(5, 10);
+  Cone cone(5, 10);
+  Cylinder cylinder(5, 10);
+  Plane plane(Vec3f(), 0.0);
+  Halfspace halfspace(Vec3f(), 0.0);
+
+  // Use sufficiently long distance so that all the primitive shapes CANNOT intersect
+  FCL_REAL distance = 15.0;
+
+  // If new shape distance algorithm is added for two distinct primitive
+  // shapes, uncomment associated lines. For example, box-sphere intersection
+  // algorithm is added, then uncomment box-sphere.
+
+//  testReversibleShapeDistance(box, sphere, distance);
+//  testReversibleShapeDistance(box, capsule, distance);
+//  testReversibleShapeDistance(box, cone, distance);
+//  testReversibleShapeDistance(box, cylinder, distance);
+//  testReversibleShapeDistance(box, plane, distance);
+//  testReversibleShapeDistance(box, halfspace, distance);
+
+  testReversibleShapeDistance(sphere, capsule, distance);
+//  testReversibleShapeDistance(sphere, cone, distance);
+//  testReversibleShapeDistance(sphere, cylinder, distance);
+//  testReversibleShapeDistance(sphere, plane, distance);
+//  testReversibleShapeDistance(sphere, halfspace, distance);
+
+//  testReversibleShapeDistance(capsule, cone, distance);
+//  testReversibleShapeDistance(capsule, cylinder, distance);
+//  testReversibleShapeDistance(capsule, plane, distance);
+//  testReversibleShapeDistance(capsule, halfspace, distance);
+
+//  testReversibleShapeDistance(cone, cylinder, distance);
+//  testReversibleShapeDistance(cone, plane, distance);
+//  testReversibleShapeDistance(cone, halfspace, distance);
+
+//  testReversibleShapeDistance(cylinder, plane, distance);
+//  testReversibleShapeDistance(cylinder, halfspace, distance);
+
+//  testReversibleShapeDistance(plane, halfspace, distance);
+}