From 295bbefc4adbea5174967b9dcc47bee149d137b6 Mon Sep 17 00:00:00 2001
From: Florent Lamiraux <florent@laas.fr>
Date: Fri, 14 Dec 2018 09:21:11 +0100
Subject: [PATCH] Take into account security margin in shape shape collision
 request

  - Collision detection calls distance computation and returns collision
    if the distance between objects is below the security margin.
  - This modification requires a modification in the structure of the software
    to make collision detection and distance computation closer.
  - GJKSolver_indep::shapeTriangleDistance and
    GJKSolver_indep::shapeTriangleIntersect have been replaced by
    GJKSolver_indep::shapeTriangleInteraction.
---
 include/hpp/fcl/narrowphase/narrowphase.h     |  812 +++++------
 .../fcl/traversal/traversal_node_bvh_shape.h  |   53 +-
 .../hpp/fcl/traversal/traversal_node_bvhs.h   |   15 +-
 .../hpp/fcl/traversal/traversal_node_octree.h |   34 +-
 .../hpp/fcl/traversal/traversal_node_shapes.h |    8 +-
 src/BVH/BVH_utility.cpp                       |   18 +-
 src/collision_func_matrix.cpp                 |   56 +-
 src/narrowphase/narrowphase.cpp               |   91 +-
 src/traversal/traversal_node_base.cpp         |   10 +-
 src/traversal/traversal_node_bvhs.cpp         |   19 +-
 test/test_fcl_geometric_shapes.cpp            | 1185 ++++++++++-------
 test/test_fcl_gjk.cpp                         |    5 +-
 12 files changed, 1200 insertions(+), 1106 deletions(-)

diff --git a/include/hpp/fcl/narrowphase/narrowphase.h b/include/hpp/fcl/narrowphase/narrowphase.h
index a35ecbc4..6d9e9de7 100644
--- a/include/hpp/fcl/narrowphase/narrowphase.h
+++ b/include/hpp/fcl/narrowphase/narrowphase.h
@@ -47,534 +47,442 @@ namespace fcl
 
 
 
-/// @brief collision and distance solver based on GJK algorithm implemented in fcl (rewritten the code from the GJK in bullet)
-struct GJKSolver_indep
-{  
-  /// @brief intersection checking between two shapes
-  template<typename S1, typename S2>
-  bool shapeIntersect(const S1& s1, const Transform3f& tf1,
-                      const S2& s2, const Transform3f& tf2,
-                      Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+  /// @brief collision and distance solver based on GJK algorithm implemented in fcl (rewritten the code from the GJK in bullet)
+  struct GJKSolver_indep
   {
-    Vec3f guess(1, 0, 0);
-    if(enable_cached_guess) guess = cached_guess;
+    /// @brief intersection checking between two shapes
+    template<typename S1, typename S2>
+      bool shapeIntersect(const S1& s1, const Transform3f& tf1,
+                          const S2& s2, const Transform3f& tf2,
+                          Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+    {
+      Vec3f guess(1, 0, 0);
+      if(enable_cached_guess) guess = cached_guess;
     
-    details::MinkowskiDiff shape;
-    shape.shapes[0] = &s1;
-    shape.shapes[1] = &s2;
-    shape.toshape1 = tf2.getRotation().transpose() * tf1.getRotation();
-    shape.toshape0 = tf1.inverseTimes(tf2);
+      details::MinkowskiDiff shape;
+      shape.shapes[0] = &s1;
+      shape.shapes[1] = &s2;
+      shape.toshape1 = tf2.getRotation().transpose() * tf1.getRotation();
+      shape.toshape0 = tf1.inverseTimes(tf2);
   
-    details::GJK gjk((unsigned int )gjk_max_iterations, gjk_tolerance);
-    details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
-    if(enable_cached_guess) cached_guess = gjk.getGuessFromSimplex();
+      details::GJK gjk((unsigned int )gjk_max_iterations, gjk_tolerance);
+      details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
+      if(enable_cached_guess) cached_guess = gjk.getGuessFromSimplex();
     
-    switch(gjk_status)
-    {
-    case details::GJK::Inside:
-      {
-        details::EPA epa(epa_max_face_num, epa_max_vertex_num, epa_max_iterations, epa_tolerance);
-        details::EPA::Status epa_status = epa.evaluate(gjk, -guess);
-        if(epa_status != details::EPA::Failed)
+      switch(gjk_status)
         {
-          Vec3f w0 (Vec3f::Zero());
-          for(size_t i = 0; i < epa.result.rank; ++i)
+        case details::GJK::Inside:
           {
-            w0 += shape.support(epa.result.vertex[i]->d, 0) *
-              epa.result.coefficient[i];
+            details::EPA epa(epa_max_face_num, epa_max_vertex_num, epa_max_iterations, epa_tolerance);
+            details::EPA::Status epa_status = epa.evaluate(gjk, -guess);
+            if(epa_status != details::EPA::Failed)
+              {
+                Vec3f w0 (Vec3f::Zero());
+                for(size_t i = 0; i < epa.result.rank; ++i)
+                  {
+                    w0 += shape.support(epa.result.vertex[i]->d, 0) *
+                      epa.result.coefficient[i];
+                  }
+                if(penetration_depth) *penetration_depth = -epa.depth;
+                if(normal) *normal = tf2.getRotation() * epa.normal;
+                if(contact_points) *contact_points = tf1.transform(w0 - epa.normal*(epa.depth *0.5));
+                return true;
+              }
+            else return false;
           }
-          if(penetration_depth) *penetration_depth = -epa.depth;
-          if(normal) *normal = tf2.getRotation() * epa.normal;
-          if(contact_points) *contact_points = tf1.transform(w0 - epa.normal*(epa.depth *0.5));
-          return true;
+          break;
+        default:
+          ;
         }
-        else return false;
-      }
-      break;
-    default:
-      ;
-    }
 
-    return false;
-  }
+      return false;
+    }
 
-  //// @brief intersection checking between one shape and a triangle with transformation
-  template<typename S>
-  bool shapeTriangleIntersect(const S& s, const Transform3f& tf1,
-                              const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
-                              const Transform3f& tf2, FCL_REAL& distance,
-                              Vec3f& p1, Vec3f& p2, Vec3f& normal) const
-  {
-    bool col;
-    TriangleP tri(P1, P2, P3);
+    //// @brief intersection checking between one shape and a triangle with transformation
+    template<typename S>
+    bool shapeTriangleInteraction
+    (const S& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2,
+     const Vec3f& P3, const Transform3f& tf2, FCL_REAL& distance,
+     Vec3f& p1, Vec3f& p2, Vec3f& normal) const
+    {
+      bool col;
+      TriangleP tri(P1, P2, P3);
 
-    Vec3f guess(1, 0, 0);
-    if(enable_cached_guess) guess = cached_guess;
+      Vec3f guess(1, 0, 0);
+      if(enable_cached_guess) guess = cached_guess;
 
-    details::MinkowskiDiff shape;
-    shape.shapes[0] = &s;
-    shape.shapes[1] = &tri;
-    shape.toshape1 = tf2.getRotation().transpose() * tf1.getRotation();
-    shape.toshape0 = tf1.inverseTimes(tf2);
+      details::MinkowskiDiff shape;
+      shape.shapes[0] = &s;
+      shape.shapes[1] = &tri;
+      shape.toshape1 = tf2.getRotation().transpose() * tf1.getRotation();
+      shape.toshape0 = tf1.inverseTimes(tf2);
   
-    details::GJK gjk((unsigned int )gjk_max_iterations, gjk_tolerance);
-    details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
-    if(enable_cached_guess) cached_guess = gjk.getGuessFromSimplex();
+      details::GJK gjk((unsigned int )gjk_max_iterations, gjk_tolerance);
+      details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
+      if(enable_cached_guess) cached_guess = gjk.getGuessFromSimplex();
 
-    switch(gjk_status)
+      switch(gjk_status)
+        {
+        case details::GJK::Inside:
+          {
+            col = true;
+            details::EPA epa(epa_max_face_num, epa_max_vertex_num, epa_max_iterations, epa_tolerance);
+            details::EPA::Status epa_status = epa.evaluate(gjk, -guess);
+            assert (epa_status != details::EPA::Failed);
+            Vec3f w0 (Vec3f::Zero());
+            for(size_t i = 0; i < epa.result.rank; ++i)
+              {
+                w0 += shape.support(epa.result.vertex[i]->d, 0) *
+                  epa.result.coefficient[i];
+              }
+            distance = -epa.depth;
+            normal = -epa.normal;
+            p1 = p2 = tf1.transform(w0 - epa.normal*(epa.depth *0.5));
+            assert (distance <= 0);
+            break;
+          }
+        case details::GJK::Valid:
+          {
+            col = false;
+            Vec3f w0 (Vec3f::Zero()), w1 (Vec3f::Zero());
+            for(size_t i = 0; i < gjk.getSimplex()->rank; ++i)
+              {
+                FCL_REAL p = gjk.getSimplex()->coefficient[i];
+                w0 += shape.support(gjk.getSimplex()->vertex[i]->d, 0) * p;
+                w1 += shape.support(-gjk.getSimplex()->vertex[i]->d, 1) * p;
+              }
+            distance = (w0 - w1).norm();
+            p1 = w0;
+            p2 = shape.toshape0.transform(w1);
+          }
+          break;
+        default:
+          ;
+        }
+      return col;
+    }
+
+    /// @brief distance computation between two shapes
+    template<typename S1, typename S2>
+      bool shapeDistance(const S1& s1, const Transform3f& tf1,
+                         const S2& s2, const Transform3f& tf2,
+                         FCL_REAL& distance, Vec3f& p1, Vec3f& p2,
+                         Vec3f& normal) const
     {
-    case details::GJK::Inside:
-      {
-        col = true;
-        details::EPA epa(epa_max_face_num, epa_max_vertex_num, epa_max_iterations, epa_tolerance);
-        details::EPA::Status epa_status = epa.evaluate(gjk, -guess);
-        assert (epa_status != details::EPA::Failed);
-        Vec3f w0 (Vec3f::Zero());
-        for(size_t i = 0; i < epa.result.rank; ++i)
+      bool compute_normal (true);
+      Vec3f guess(1, 0, 0);
+      if(enable_cached_guess) guess = cached_guess;
+
+      details::MinkowskiDiff shape;
+      shape.shapes[0] = &s1;
+      shape.shapes[1] = &s2;
+      shape.toshape1 = tf2.getRotation().transpose() * tf1.getRotation();
+      shape.toshape0 = tf1.inverseTimes(tf2);
+
+      details::GJK gjk((unsigned int) gjk_max_iterations, gjk_tolerance);
+      details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
+      if(enable_cached_guess) cached_guess = gjk.getGuessFromSimplex();
+
+      if(gjk_status == details::GJK::Valid)
         {
-          w0 += shape.support(epa.result.vertex[i]->d, 0) *
-            epa.result.coefficient[i];
+          Vec3f w0 (Vec3f::Zero()), w1 (Vec3f::Zero());
+          for(size_t i = 0; i < gjk.getSimplex()->rank; ++i)
+            {
+              FCL_REAL p = gjk.getSimplex()->coefficient[i];
+              w0 += shape.support(gjk.getSimplex()->vertex[i]->d, 0) * p;
+              w1 += shape.support(-gjk.getSimplex()->vertex[i]->d, 1) * p;
+            }
+
+          distance = (w0 - w1).norm();
+
+          p1 = tf1.transform (w0);
+          p2 = tf1.transform (w1);
+          return true;
+        }
+      else if (gjk_status == details::GJK::Inside)
+        {
+          if (compute_normal)
+            {
+              details::EPA epa(epa_max_face_num, epa_max_vertex_num,
+                               epa_max_iterations, epa_tolerance);
+              details::EPA::Status epa_status = epa.evaluate(gjk, -guess);
+              if(epa_status != details::EPA::Failed)
+                {
+                  Vec3f w0 (Vec3f::Zero());
+                  for(size_t i = 0; i < epa.result.rank; ++i)
+                    {
+                      w0 += shape.support(epa.result.vertex[i]->d, 0) *
+                        epa.result.coefficient[i];
+                    }
+                  distance = epa.depth;
+                  normal = tf2.getRotation() * epa.normal;
+                  p1 = p2 = tf1.transform(w0 - epa.normal*(epa.depth *0.5));
+                }
+            }
+          else
+            {
+              Vec3f w0 (Vec3f::Zero()), w1 (Vec3f::Zero());
+              for(size_t i = 0; i < gjk.getSimplex()->rank; ++i)
+                {
+                  FCL_REAL p = gjk.getSimplex()->coefficient[i];
+                  w0 += shape.support(gjk.getSimplex()->vertex[i]->d, 0) * p;
+                  w1 += shape.support(-gjk.getSimplex()->vertex[i]->d, 1) * p;
+                }
+              distance = 0;
+
+              p1 = tf1.transform (w0);
+              p2 = tf1.transform (w1);
+            }
+          return false;
         }
-        distance = epa.depth;
-        normal = -epa.normal;
-        p1 = p2 = tf1.transform(w0 - epa.normal*(epa.depth *0.5));
-        break;
-      }
-    case details::GJK::Valid:
-      {
-        col = false;
-        Vec3f w0 (Vec3f::Zero()), w1 (Vec3f::Zero());
-        for(size_t i = 0; i < gjk.getSimplex()->rank; ++i)
+      else
         {
-          FCL_REAL p = gjk.getSimplex()->coefficient[i];
-          w0 += shape.support(gjk.getSimplex()->vertex[i]->d, 0) * p;
-          w1 += shape.support(-gjk.getSimplex()->vertex[i]->d, 1) * p;
+          // Failure
+          abort ();
         }
-        FCL_REAL dist ((w0 - w1).norm());
-        distance = dist;
-        p1 = w0;
-        p2 = shape.toshape0.transform(w1);
-      }
-      break;
-    default:
-      ;
     }
-    return col;
-  }
-
-  /// @brief distance computation between two shapes
-  template<typename S1, typename S2>
-  bool shapeDistance(const S1& s1, const Transform3f& tf1,
-                     const S2& s2, const Transform3f& tf2,
-                     FCL_REAL* distance, Vec3f* p1, Vec3f* p2) const
-  {
-    Vec3f guess(1, 0, 0);
-    if(enable_cached_guess) guess = cached_guess;
-
-    details::MinkowskiDiff shape;
-    shape.shapes[0] = &s1;
-    shape.shapes[1] = &s2;
-    shape.toshape1 = tf2.getRotation().transpose() * tf1.getRotation();
-    shape.toshape0 = tf1.inverseTimes(tf2);
-
-    details::GJK gjk((unsigned int) gjk_max_iterations, gjk_tolerance);
-    details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
-    if(enable_cached_guess) cached_guess = gjk.getGuessFromSimplex();
 
-    if(gjk_status == details::GJK::Valid)
+    /// @brief default setting for GJK algorithm
+    GJKSolver_indep()
     {
-      Vec3f w0 (Vec3f::Zero()), w1 (Vec3f::Zero());
-      for(size_t i = 0; i < gjk.getSimplex()->rank; ++i)
-      {
-        FCL_REAL p = gjk.getSimplex()->coefficient[i];
-        w0 += shape.support(gjk.getSimplex()->vertex[i]->d, 0) * p;
-        w1 += shape.support(-gjk.getSimplex()->vertex[i]->d, 1) * p;
-      }
-
-      if(distance) *distance = (w0 - w1).norm();
-      
-      if(p1) *p1 = tf1.transform (w0);
-      if(p2) *p2 = tf1.transform (w1);
-      
-      return true;
+      gjk_max_iterations = 128;
+      gjk_tolerance = 1e-6;
+      epa_max_face_num = 128;
+      epa_max_vertex_num = 64;
+      epa_max_iterations = 255;
+      epa_tolerance = 1e-6;
+      enable_cached_guess = false;
+      cached_guess = Vec3f(1, 0, 0);
     }
-    else if (gjk_status == details::GJK::Inside)
-    {
-      Vec3f w0 (Vec3f::Zero()), w1 (Vec3f::Zero());
-      for(size_t i = 0; i < gjk.getSimplex()->rank; ++i)
-      {
-        FCL_REAL p = gjk.getSimplex()->coefficient[i];
-        w0 += shape.support(gjk.getSimplex()->vertex[i]->d, 0) * p;
-        w1 += shape.support(-gjk.getSimplex()->vertex[i]->d, 1) * p;
-      }
-      if(distance) *distance = 0;
-
-      if(p1) *p1 = tf1.transform (w0);
-      if(p2) *p2 = tf1.transform (w1);
 
-      return false;
+    void enableCachedGuess(bool if_enable) const
+    {
+      enable_cached_guess = if_enable;
     }
-    if(distance) *distance = 0;
-    return false;
-  }
-
-  template<typename S1, typename S2>
-  bool shapeDistance(const S1& s1, const Transform3f& tf1,
-                     const S2& s2, const Transform3f& tf2,
-                     FCL_REAL* distance) const
-  {
-    return shapeDistance(s1, tf1, s2, tf2, distance, NULL, NULL);
-  }
-
-  /// @brief distance computation between one shape and a triangle
-  template<typename S>
-  bool shapeTriangleDistance(const S& s, const Transform3f& tf,
-                             const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
-                             FCL_REAL* distance, Vec3f* p1, Vec3f* p2) const
-  {
-    TriangleP tri(P1, P2, P3);
-    Vec3f guess(1, 0, 0);
-    if(enable_cached_guess) guess = cached_guess;
-
-    details::MinkowskiDiff shape;
-    shape.shapes[0] = &s;
-    shape.shapes[1] = &tri;
-    shape.toshape1 = tf.getRotation();
-    shape.toshape0 = inverse(tf);
-
-    details::GJK gjk((unsigned int )gjk_max_iterations, gjk_tolerance);
-    details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
-    if(enable_cached_guess) cached_guess = gjk.getGuessFromSimplex();
-    
-    if(gjk_status == details::GJK::Valid)
+
+    void setCachedGuess(const Vec3f& guess) const
     {
-      Vec3f w0 (Vec3f::Zero()), w1 (Vec3f::Zero());
-      for(size_t i = 0; i < gjk.getSimplex()->rank; ++i)
-      {
-        FCL_REAL p = gjk.getSimplex()->coefficient[i];
-        w0 += shape.support(gjk.getSimplex()->vertex[i]->d, 0) * p;
-        w1 += shape.support(-gjk.getSimplex()->vertex[i]->d, 1) * p;
-      }
-
-      if(distance) *distance = (w0 - w1).norm();
-      if(p1) *p1 = w0;
-      if(p2) *p2 = shape.toshape0.transform(w1);
-      return true;
+      cached_guess = guess;
     }
-    else
+
+    Vec3f getCachedGuess() const
     {
-      if(distance) *distance = -1;
-      return false;
+      return cached_guess;
     }
-  }
 
-  template<typename S>
-  bool shapeTriangleDistance(const S& s, const Transform3f& tf,
-                             const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
-                             FCL_REAL* distance) const
-  {
-    return shapeTriangleDistance(s, tf, P1, P2, P3, distance, NULL, NULL);
-  }
-  
-  /// @brief distance computation between one shape and a triangle with transformation
-  template<typename S>
-  bool shapeTriangleDistance(const S& s, const Transform3f& tf1,
-                             const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
-                             FCL_REAL* distance, Vec3f* p1, Vec3f* p2) const
-  {
-    TriangleP tri(P1, P2, P3);
-    Vec3f guess(1, 0, 0);
-    if(enable_cached_guess) guess = cached_guess;
-    
-    details::MinkowskiDiff shape;
-    shape.shapes[0] = &s;
-    shape.shapes[1] = &tri;
-    shape.toshape1 = tf2.getRotation().transpose() * tf1.getRotation();
-    shape.toshape0 = tf1.inverseTimes(tf2);
+    /// @brief maximum number of simplex face used in EPA algorithm
+    unsigned int epa_max_face_num;
 
-    details::GJK gjk((unsigned int )gjk_max_iterations, gjk_tolerance);
-    details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
-    if(enable_cached_guess) cached_guess = gjk.getGuessFromSimplex();
+    /// @brief maximum number of simplex vertex used in EPA algorithm
+    unsigned int epa_max_vertex_num;
 
-    if(gjk_status == details::GJK::Valid)
-    {
-      Vec3f w0 (Vec3f::Zero()), w1 (Vec3f::Zero());
-      for(size_t i = 0; i < gjk.getSimplex()->rank; ++i)
-      {
-        FCL_REAL p = gjk.getSimplex()->coefficient[i];
-        w0 += shape.support(gjk.getSimplex()->vertex[i]->d, 0) * p;
-        w1 += shape.support(-gjk.getSimplex()->vertex[i]->d, 1) * p;
-      }
-
-      if(distance) *distance = (w0 - w1).norm();
-      if(p1) *p1 = w0;
-      if(p2) *p2 = shape.toshape0.transform(w1);
-      return true;
-    }
-    else
-    {
-      if(distance) *distance = -1;
-      return false;
-    }
-  }
+    /// @brief maximum number of iterations used for EPA iterations
+    unsigned int epa_max_iterations;
 
-  template<typename S>
-  bool shapeTriangleDistance(const S& s, const Transform3f& tf1,
-                             const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
-                             FCL_REAL* distance) const
-  {
-    return shapeTriangleDistance(s, tf1, P1, P2, P3, tf2, distance, NULL, NULL);
-  }
-  
-  /// @brief default setting for GJK algorithm
-  GJKSolver_indep()
-  {
-    gjk_max_iterations = 128;
-    gjk_tolerance = 1e-6;
-    epa_max_face_num = 128;
-    epa_max_vertex_num = 64;
-    epa_max_iterations = 255;
-    epa_tolerance = 1e-6;
-    enable_cached_guess = false;
-    cached_guess = Vec3f(1, 0, 0);
-  }
-
-  void enableCachedGuess(bool if_enable) const
-  {
-    enable_cached_guess = if_enable;
-  }
+    /// @brief the threshold used in EPA to stop iteration
+    FCL_REAL epa_tolerance;
 
-  void setCachedGuess(const Vec3f& guess) const
-  {
-    cached_guess = guess;
-  }
+    /// @brief the threshold used in GJK to stop iteration
+    FCL_REAL gjk_tolerance;
 
-  Vec3f getCachedGuess() const
-  {
-    return cached_guess;
-  }
+    /// @brief maximum number of iterations used for GJK iterations
+    FCL_REAL gjk_max_iterations;
 
-  /// @brief maximum number of simplex face used in EPA algorithm
-  unsigned int epa_max_face_num;
+    /// @brief Whether smart guess can be provided
+    mutable bool enable_cached_guess;
 
-  /// @brief maximum number of simplex vertex used in EPA algorithm
-  unsigned int epa_max_vertex_num;
+    /// @brief smart guess
+    mutable Vec3f cached_guess;
+  };
 
-  /// @brief maximum number of iterations used for EPA iterations
-  unsigned int epa_max_iterations;
+  /// @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,
+                                                          Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
 
-  /// @brief the threshold used in EPA to stop iteration
-  FCL_REAL epa_tolerance;
+  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 the threshold used in GJK to stop iteration
-  FCL_REAL gjk_tolerance;
+  /// @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 maximum number of iterations used for GJK iterations
-  FCL_REAL gjk_max_iterations;
+  /// @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,
+                                                   Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
 
-  /// @brief Whether smart guess can be provided
-  mutable bool enable_cached_guess;
+  template<>
+    bool GJKSolver_indep::shapeIntersect<Sphere, Halfspace>(const Sphere& s1, const Transform3f& tf1,
+                                                            const Halfspace& s2, const Transform3f& tf2,
+                                                            Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
 
-  /// @brief smart guess
-  mutable Vec3f cached_guess;
-};
+  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;
 
-/// @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,
-                                                      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<Capsule, Sphere>(const Capsule &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<Halfspace, Box>(const Halfspace& s1, const Transform3f& tf1,
+                                                         const Box& 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;
+  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;
 
-/// @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,
-                                               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<Sphere, Halfspace>(const Sphere& s1, const Transform3f& tf1,
-                                                        const Halfspace& s2, const Transform3f& tf2,
+  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,
+                                                               Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+
+  template<>
+    bool GJKSolver_indep::shapeIntersect<Plane, Halfspace>(const Plane& 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, 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<Halfspace, Sphere>(const Halfspace& s1, const Transform3f& tf1,
+  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, Halfspace>(const Box& s1, const Transform3f& tf1,
-                                                     const Halfspace& s2, const Transform3f& tf2,
+  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<Halfspace, Box>(const Halfspace& s1, const Transform3f& tf1,
+  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, Halfspace>(const Capsule& s1, const Transform3f& tf1,
-                                                         const Halfspace& s2, const Transform3f& tf2,
+  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<Halfspace, Capsule>(const Halfspace& s1, const Transform3f& tf1,
+  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, Halfspace>(const Cylinder& s1, const Transform3f& tf1,
-                                                          const Halfspace& s2, const Transform3f& tf2,
+  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<Halfspace, Cylinder>(const Halfspace& s1, const Transform3f& tf1,
+  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, Halfspace>(const Cone& s1, const Transform3f& tf1,
-                                                      const Halfspace& s2, const Transform3f& tf2,
+  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, Cone>(const Halfspace& s1, const Transform3f& tf1,
+  template<>
+    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<Halfspace, Halfspace>(const Halfspace& 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<Plane, Halfspace>(const Plane& 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, Plane>(const Halfspace& s1, const Transform3f& tf1,
+  template<>
+    bool GJKSolver_indep::shapeIntersect<Plane, Plane>(const Plane& 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<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,
-                                                   const Plane& s2, const Transform3f& tf2,
-                                                   Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
-
-/// @brief Fast implementation for sphere-triangle collision
-template<>
-bool GJKSolver_indep::shapeTriangleIntersect
-  (const Sphere& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2,
-   const Vec3f& P3, const Transform3f& tf2, FCL_REAL& distance,
-   Vec3f& p1, Vec3f& p2, 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, FCL_REAL& distance,
-   Vec3f& p1, Vec3f& p2, Vec3f& normal) const;
-
-template<>
-bool GJKSolver_indep::shapeTriangleIntersect
-  (const Plane& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2,
-   const Vec3f& P3, const Transform3f& tf2, FCL_REAL& distance,
-   Vec3f& p1, Vec3f& p2, 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;
-
-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<>
-bool GJKSolver_indep::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_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,
-                                                    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,
-                                                    const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
-                                                    FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const;
+  /// @brief Fast implementation for sphere-triangle collision
+  template<>
+    bool GJKSolver_indep::shapeTriangleInteraction
+    (const Sphere& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2,
+     const Vec3f& P3, const Transform3f& tf2, FCL_REAL& distance,
+     Vec3f& p1, Vec3f& p2, Vec3f& normal) const;
+
+  template<>
+    bool GJKSolver_indep::shapeTriangleInteraction
+    (const Halfspace& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2,
+     const Vec3f& P3, const Transform3f& tf2, FCL_REAL& distance,
+     Vec3f& p1, Vec3f& p2, Vec3f& normal) const;
+
+  template<>
+    bool GJKSolver_indep::shapeTriangleInteraction
+    (const Plane& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2,
+     const Vec3f& P3, const Transform3f& tf2, FCL_REAL& distance,
+     Vec3f& p1, Vec3f& p2, 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, Vec3f& normal) 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, Vec3f& normal) const;
+
+  /// @brief Fast implementation for sphere-sphere distance
+  template<>
+    bool GJKSolver_indep::shapeDistance<Sphere, Sphere>
+    (const Sphere& s1, const Transform3f& tf1,
+     const Sphere& s2, const Transform3f& tf2,
+     FCL_REAL& dist, Vec3f& p1, Vec3f& p2, Vec3f& normal) 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, Vec3f& normal) const;
 
 }
 
diff --git a/include/hpp/fcl/traversal/traversal_node_bvh_shape.h b/include/hpp/fcl/traversal/traversal_node_bvh_shape.h
index 54f9f6ba..c14a3fc5 100644
--- a/include/hpp/fcl/traversal/traversal_node_bvh_shape.h
+++ b/include/hpp/fcl/traversal/traversal_node_bvh_shape.h
@@ -210,9 +210,9 @@ public:
     Vec3f normal;
     Vec3f c1, c2;
 
-    if(nsolver->shapeTriangleIntersect(*(this->model2), this->tf2, p1, p2, p3,
-                                       Transform3f (), distance, c1, c2,
-                                       normal))
+    if(nsolver->shapeTriangleInteraction(*(this->model2), this->tf2, p1, p2, p3,
+                                         Transform3f (), distance, c1, c2,
+                                         normal))
     {
       if(this->request.num_max_contacts > this->result->numContacts())
         this->result->addContact(Contact(this->model1, this->model2,
@@ -270,8 +270,8 @@ static inline void meshShapeCollisionOrientedNodeLeafTesting
   Vec3f normal;
   Vec3f p1, p2; // closest points
 
-  if(nsolver->shapeTriangleIntersect(model2, tf2, P1, P2, P3, tf1,
-                                     distance, p1, p2, normal))
+  if(nsolver->shapeTriangleInteraction(model2, tf2, P1, P2, P3, tf1,
+                                       distance, p1, p2, normal))
   {
     if(request.num_max_contacts > result.numContacts())
       result.addContact(Contact(model1, &model2, primitive_id, Contact::NONE,
@@ -437,8 +437,9 @@ public:
     Vec3f normal;
     Vec3f p1, p2; // closest points
 
-    if(nsolver->shapeTriangleIntersect(*(this->model1), this->tf1, P1, P2, P3,
-                                       p1, p2, distance, normal))
+    if(nsolver->shapeTriangleInteraction(*(this->model1), this->tf1, P1, P2, P3,
+                                         Transform3f (), p1, p2, distance,
+                                         normal))
     {
       if(this->request.num_max_contacts > this->result->numContacts())
       {  
@@ -714,10 +715,14 @@ public:
     const Vec3f& p3 = vertices[tri_id[2]];
     
     FCL_REAL d;
-    Vec3f closest_p1, closest_p2;
-    nsolver->shapeTriangleDistance(*(this->model2), this->tf2, p1, p2, p3, &d, &closest_p2, &closest_p1);
+    Vec3f closest_p1, closest_p2, normal;
+    nsolver->shapeTriangleInteraction(*(this->model2), this->tf2, p1, p2, p3,
+                                      Transform3f (), d, closest_p2, closest_p1,
+                                      normal);
 
-    this->result->update(d, this->model1, this->model2, primitive_id, DistanceResult::NONE, closest_p1, closest_p2);
+    this->result->update(d, this->model1, this->model2, primitive_id,
+                         DistanceResult::NONE, closest_p1, closest_p2,
+                         normal);
   }
 
   /// @brief Whether the traversal process can stop early
@@ -764,10 +769,12 @@ void meshShapeDistanceOrientedNodeLeafTesting(int b1, int /* b2 */,
   const Vec3f& p3 = vertices[tri_id[2]];
     
   FCL_REAL distance;
-  Vec3f closest_p1, closest_p2;
-  nsolver->shapeTriangleDistance(model2, tf2, p1, p2, p3, tf1, &distance, &closest_p2, &closest_p1);
+  Vec3f closest_p1, closest_p2, normal;
+  nsolver->shapeTriangleInteraction(model2, tf2, p1, p2, p3, tf1, distance,
+                                    closest_p2, closest_p1, normal);
 
-  result.update(distance, model1, &model2, primitive_id, DistanceResult::NONE, closest_p1, closest_p2);
+  result.update(distance, model1, &model2, primitive_id, DistanceResult::NONE,
+                closest_p1, closest_p2, normal);
 }
 
 
@@ -786,10 +793,12 @@ static inline void distancePreprocessOrientedNode(const BVHModel<BV>* model1,
   const Vec3f& p3 = vertices[init_tri[2]];
   
   FCL_REAL distance;
-  Vec3f closest_p1, closest_p2;
-  nsolver->shapeTriangleDistance(model2, tf2, p1, p2, p3, tf1, &distance, &closest_p2, &closest_p1);
+  Vec3f closest_p1, closest_p2, normal;
+  nsolver->shapeTriangleInteraction(model2, tf2, p1, p2, p3, tf1, distance,
+                                    closest_p2, closest_p1, normal);
 
-  result.update(distance, model1, &model2, init_tri_id, DistanceResult::NONE, closest_p1, closest_p2);
+  result.update(distance, model1, &model2, init_tri_id, DistanceResult::NONE,
+                closest_p1, closest_p2, normal);
 }
 
 
@@ -929,10 +938,14 @@ public:
     const Vec3f& p3 = vertices[tri_id[2]];
     
     FCL_REAL distance;
-    Vec3f closest_p1, closest_p2;
-    nsolver->shapeTriangleDistance(*(this->model1), this->tf1, p1, p2, p3, &distance, &closest_p1, &closest_p2);
-
-    this->result->update(distance, this->model1, this->model2, DistanceResult::NONE, primitive_id, closest_p1, closest_p2);
+    Vec3f closest_p1, closest_p2, normal;
+    nsolver->shapeTriangleInteraction(*(this->model1), this->tf1, p1, p2, p3,
+                                      Transform3f (), distance, closest_p1,
+                                      closest_p2, normal);
+
+    this->result->update(distance, this->model1, this->model2,
+                         DistanceResult::NONE, primitive_id, closest_p1,
+                         closest_p2, normal);
   }
 
   /// @brief Whether the traversal process can stop early
diff --git a/include/hpp/fcl/traversal/traversal_node_bvhs.h b/include/hpp/fcl/traversal/traversal_node_bvhs.h
index 6f69693f..31ce49af 100644
--- a/include/hpp/fcl/traversal/traversal_node_bvhs.h
+++ b/include/hpp/fcl/traversal/traversal_node_bvhs.h
@@ -235,9 +235,10 @@ public:
     TriangleP tri2 (Q1, Q2, Q3);
     GJKSolver_indep solver;
     Vec3f p1, p2; // closest points if no collision contact points if collision.
+    Vec3f normal;
     FCL_REAL distance;
     solver.shapeDistance (tri1, this->tf1, tri2, this->tf2,
-                          &distance, &p1, &p2);
+                          distance, p1, p2, normal);
     FCL_REAL distToCollision = distance - this->request.security_margin;
     sqrDistLowerBound = distance * distance;
     if (distToCollision <= 0) { // collision
@@ -456,19 +457,13 @@ public:
     const Vec3f& t23 = vertices2[tri_id2[2]];
 
     // nearest point pair
-    Vec3f P1, P2;
+    Vec3f P1, P2, normal;
 
     FCL_REAL d = sqrt (TriangleDistance::sqrTriDistance
 		       (t11, t12, t13, t21, t22, t23, P1, P2));
 
-    if(this->request.enable_nearest_points)
-    {
-      this->result->update(d, this->model1, this->model2, primitive_id1, primitive_id2, P1, P2);
-    }
-    else
-    {
-      this->result->update(d, this->model1, this->model2, primitive_id1, primitive_id2);
-    }
+    this->result->update(d, this->model1, this->model2, primitive_id1,
+                         primitive_id2, P1, P2, normal);
   }
 
   /// @brief Whether the traversal process can stop early
diff --git a/include/hpp/fcl/traversal/traversal_node_octree.h b/include/hpp/fcl/traversal/traversal_node_octree.h
index 7011e49e..4ea2609a 100644
--- a/include/hpp/fcl/traversal/traversal_node_octree.h
+++ b/include/hpp/fcl/traversal/traversal_node_octree.h
@@ -248,10 +248,13 @@ private:
         constructBox(bv1, tf1, box, box_tf);
  
         FCL_REAL dist;
-        Vec3f closest_p1, closest_p2;
-        solver->shapeDistance(box, box_tf, s, tf2, &dist, &closest_p1, &closest_p2);
+        Vec3f closest_p1, closest_p2, normal;
+        solver->shapeDistance(box, box_tf, s, tf2, dist, closest_p1,
+                              closest_p2, normal);
         
-        dresult->update(dist, tree1, &s, root1 - tree1->getRoot(), DistanceResult::NONE, closest_p1, closest_p2);
+        dresult->update(dist, tree1, &s, root1 - tree1->getRoot(),
+                        DistanceResult::NONE, closest_p1, closest_p2,
+                        normal);
         
         return drequest->isSatisfied(*dresult);
       }
@@ -402,10 +405,12 @@ private:
         const Vec3f& p3 = tree2->vertices[tri_id[2]];
         
         FCL_REAL dist;
-        Vec3f closest_p1, closest_p2;
-        solver->shapeTriangleDistance(box, box_tf, p1, p2, p3, tf2, &dist, &closest_p1, &closest_p2);
+        Vec3f closest_p1, closest_p2, normal;
+        solver->shapeTriangleInteraction(box, box_tf, p1, p2, p3, tf2, dist,
+                                         closest_p1, closest_p2, normal);
 
-        dresult->update(dist, tree1, tree2, root1 - tree1->getRoot(), primitive_id);
+        dresult->update(dist, tree1, tree2, root1 - tree1->getRoot(),
+                        primitive_id, closest_p1, closest_p2, normal);
 
         return drequest->isSatisfied(*dresult);
       }
@@ -494,7 +499,7 @@ private:
           const Vec3f& p3 = tree2->vertices[tri_id[2]];
           Vec3f c1, c2, normal;
           FCL_REAL distance;
-          if(solver->shapeTriangleIntersect
+          if(solver->shapeTriangleInteraction
              (box, box_tf, p1, p2, p3, tf2, distance, c1, c2, normal))
           {
             AABB overlap_part;
@@ -542,7 +547,7 @@ private:
           {
             Vec3f c1, c2, normal;
             FCL_REAL distance;
-            if(solver->shapeTriangleIntersect
+            if(solver->shapeTriangleInteraction
                (box, box_tf, p1, p2, p3, tf2, distance, c1, c2, normal))
             {
               is_intersect = true;
@@ -556,8 +561,8 @@ private:
             FCL_REAL distance;
             Vec3f normal;
 
-            if(solver->shapeTriangleIntersect(box, box_tf, p1, p2, p3, tf2,
-                                              distance, c1, c2, normal))
+            if(solver->shapeTriangleInteraction(box, box_tf, p1, p2, p3, tf2,
+                                                distance, c1, c2, normal))
             {
               is_intersect = true;
               assert (crequest->security_margin == 0);
@@ -631,10 +636,13 @@ private:
         constructBox(bv2, tf2, box2, box2_tf);
 
         FCL_REAL dist;
-        Vec3f closest_p1, closest_p2;
-        solver->shapeDistance(box1, box1_tf, box2, box2_tf, &dist, &closest_p1, &closest_p2);
+        Vec3f closest_p1, closest_p2, normal;
+        solver->shapeDistance(box1, box1_tf, box2, box2_tf, dist, closest_p1,
+                              closest_p2, normal);
 
-        dresult->update(dist, tree1, tree2, root1 - tree1->getRoot(), root2 - tree2->getRoot(), closest_p1, closest_p2);
+        dresult->update(dist, tree1, tree2, root1 - tree1->getRoot(),
+                        root2 - tree2->getRoot(), closest_p1, closest_p2,
+                        normal);
         
         return drequest->isSatisfied(*dresult);
       }
diff --git a/include/hpp/fcl/traversal/traversal_node_shapes.h b/include/hpp/fcl/traversal/traversal_node_shapes.h
index 278eff54..f02dc9cf 100644
--- a/include/hpp/fcl/traversal/traversal_node_shapes.h
+++ b/include/hpp/fcl/traversal/traversal_node_shapes.h
@@ -135,9 +135,11 @@ public:
   void leafTesting(int, int) const
   {
     FCL_REAL distance;
-    Vec3f closest_p1, closest_p2;
-    nsolver->shapeDistance(*model1, tf1, *model2, tf2, &distance, &closest_p1, &closest_p2);
-    result->update(distance, model1, model2, DistanceResult::NONE, DistanceResult::NONE, closest_p1, closest_p2);
+    Vec3f closest_p1, closest_p2, normal;
+    nsolver->shapeDistance(*model1, tf1, *model2, tf2, distance, closest_p1,
+                           closest_p2, normal);
+    result->update(distance, model1, model2, DistanceResult::NONE,
+                   DistanceResult::NONE, closest_p1, closest_p2, normal);
   }
 
   const S1* model1;
diff --git a/src/BVH/BVH_utility.cpp b/src/BVH/BVH_utility.cpp
index f8c510ec..86ac6ae4 100644
--- a/src/BVH/BVH_utility.cpp
+++ b/src/BVH/BVH_utility.cpp
@@ -123,14 +123,14 @@ BVHModel<BV>* BVHExtract(const BVHModel<BV>& model, const Transform3f& pose, con
   std::vector<bool> keep_vertex(model.num_vertices, false);
   std::vector<bool> keep_tri   (model.num_tris,     false);
   std::size_t ntri = 0;
-  for (std::size_t i = 0; i < model.num_tris; ++i) {
+  for (std::size_t i = 0; i < (std::size_t) model.num_tris; ++i) {
     const Triangle& t = model.tri_indices[i];
 
     bool keep_this_tri = keep_vertex[t[0]] || keep_vertex[t[1]] || keep_vertex[t[2]];
 
     if (!keep_this_tri) {
       for (std::size_t j = 0; j < 3; ++j) {
-        if (aabb.contain(q * model.vertices[t[j]])) {
+        if (aabb.contain(q * model.vertices[t[(int)j]])) {
           keep_this_tri = true;
           break;
         }
@@ -140,7 +140,7 @@ BVHModel<BV>* BVHExtract(const BVHModel<BV>& model, const Transform3f& pose, con
       const Vec3f& p2 = model.vertices[t[2]];
       Vec3f c1, c2, normal;
       FCL_REAL distance;
-      if (!keep_this_tri && gjk.shapeTriangleIntersect
+      if (!keep_this_tri && gjk.shapeTriangleInteraction
           (box, box_pose, p0, p1, p2, Transform3f (), distance, c1, c2,
            normal)) {
         keep_this_tri = true;
@@ -300,7 +300,7 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, uns
 
       for(int j = 0; j < 3; ++j)
       {
-        int point_id = t[j];
+        int point_id = (int) t[j];
         const Vec3f& p = ps[point_id];
         Vec3f v(p[0], p[1], p[2]);
         P[P_id][0] = axes.col(0).dot(v);
@@ -313,7 +313,7 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, uns
       {
         for(int j = 0; j < 3; ++j)
         {
-          int point_id = t[j];
+          int point_id = (int) t[j];
           const Vec3f& p = ps2[point_id];
           // FIXME Is this right ?????
           Vec3f v(p[0], p[1], p[2]);
@@ -623,7 +623,7 @@ static inline void getExtentAndCenter_mesh(Vec3f* ps, Vec3f* ps2, Triangle* ts,
 
     for(int j = 0; j < 3; ++j)
     {
-      int point_id = t[j];
+      int point_id = (int) t[j];
       const Vec3f& p = ps[point_id];
       Vec3f proj(axes.transpose() * p);
 
@@ -638,7 +638,7 @@ static inline void getExtentAndCenter_mesh(Vec3f* ps, Vec3f* ps2, Triangle* ts,
     {
       for(int j = 0; j < 3; ++j)
       {
-        int point_id = t[j];
+        int point_id = (int) t[j];
         const Vec3f& p = ps2[point_id];
         Vec3f proj(axes.transpose() * p);
 
@@ -695,7 +695,7 @@ static inline FCL_REAL maximumDistance_mesh(Vec3f* ps, Vec3f* ps2, Triangle* ts,
 
     for(int j = 0; j < 3; ++j)
     {
-      int point_id = t[j];
+      int point_id = (int) t[j];
       const Vec3f& p = ps[point_id];
       
       FCL_REAL d = (p - query).squaredNorm();
@@ -706,7 +706,7 @@ static inline FCL_REAL maximumDistance_mesh(Vec3f* ps, Vec3f* ps2, Triangle* ts,
     {
       for(int j = 0; j < 3; ++j)
       {
-        int point_id = t[j];
+        int point_id = (int) t[j];
         const Vec3f& p = ps2[point_id];
         
         FCL_REAL d = (p - query).squaredNorm();
diff --git a/src/collision_func_matrix.cpp b/src/collision_func_matrix.cpp
index da8c0072..7da3bcb9 100644
--- a/src/collision_func_matrix.cpp
+++ b/src/collision_func_matrix.cpp
@@ -144,46 +144,38 @@ std::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3f& tf
 {
   if(request.isSatisfied(result)) return result.numContacts();
 
-  if (request.enable_distance_lower_bound) {
-    DistanceResult distanceResult;
-    DistanceRequest distanceRequest (request.enable_contact);
-    FCL_REAL distance = ShapeShapeDistance <T_SH1, T_SH2, NarrowPhaseSolver>
-      (o1, tf1, o2, tf2, nsolver, distanceRequest, distanceResult);
+  DistanceResult distanceResult;
+  DistanceRequest distanceRequest (request.enable_contact);
+  FCL_REAL distance = ShapeShapeDistance <T_SH1, T_SH2, NarrowPhaseSolver>
+    (o1, tf1, o2, tf2, nsolver, distanceRequest, distanceResult);
 
-    if (distance <= 0) {
+  if (distance <= 0) {
+    if (result.numContacts () < request.num_max_contacts) {
       Contact contact (o1, o2, distanceResult.b1, distanceResult.b2);
       const Vec3f& p1 = distanceResult.nearest_points [0];
       const Vec3f& p2 = distanceResult.nearest_points [1];
-      contact.pos = .5*(p1+p2);
-      contact.normal = (p2-p1)/(p2-p1).norm ();
+      assert (p1 == p2);
+      contact.pos = p1;
+      contact.normal = distanceResult.normal;
+      contact.penetration_depth = -distance;
       result.addContact (contact);
-      return 1;
     }
-    result.distance_lower_bound = distance;
-    return 0;
+    return 1;
   }
-
-  ShapeCollisionTraversalNode<T_SH1, T_SH2, NarrowPhaseSolver> node (request);
-  const T_SH1* obj1 = static_cast<const T_SH1*>(o1);
-  const T_SH2* obj2 = static_cast<const T_SH2*>(o2);
-
-  if(request.enable_cached_gjk_guess)
-  {
-    nsolver->enableCachedGuess(true);
-    nsolver->setCachedGuess(request.cached_gjk_guess);
-  }
-  else
-  {
-    nsolver->enableCachedGuess(true);
+  if (distance <= request.security_margin) {
+    if (result.numContacts () < request.num_max_contacts) {
+      Contact contact (o1, o2, distanceResult.b1, distanceResult.b2);
+      const Vec3f& p1 = distanceResult.nearest_points [0];
+      const Vec3f& p2 = distanceResult.nearest_points [1];
+      contact.pos = .5 * (p1 + p2);
+      contact.normal = (p2-p1).normalized ();
+      contact.penetration_depth = -distance;
+      result.addContact (contact);
+    }
+    return 1;
   }
-
-  initialize(node, *obj1, tf1, *obj2, tf2, nsolver, result);
-  collide(&node, request, result);
-
-  if(request.enable_cached_gjk_guess)
-    result.cached_gjk_guess = nsolver->getCachedGuess();
-
-  return result.numContacts();
+  result.distance_lower_bound = distance;
+  return 0;
 }
 
 template<typename T_BVH, typename T_SH, typename NarrowPhaseSolver>
diff --git a/src/narrowphase/narrowphase.cpp b/src/narrowphase/narrowphase.cpp
index 044ac277..b76fef8a 100644
--- a/src/narrowphase/narrowphase.cpp
+++ b/src/narrowphase/narrowphase.cpp
@@ -107,7 +107,7 @@ bool sphereCapsuleIntersect(const Sphere& s1, const Transform3f& tf1,
 
 bool sphereCapsuleDistance(const Sphere& s1, const Transform3f& tf1, 
                            const Capsule& s2, const Transform3f& tf2,
-                           FCL_REAL* dist, Vec3f* p1, Vec3f* p2)
+                           FCL_REAL& dist, Vec3f& p1, Vec3f& p2, Vec3f& normal)
 {
   Transform3f tf2_inv(tf2);
   tf2_inv.inverse();
@@ -119,21 +119,15 @@ bool sphereCapsuleDistance(const Sphere& s1, const Transform3f& tf1,
   Vec3f segment_point;
 
   lineSegmentPointClosestToPoint (s_c, pos1, pos2, segment_point);
-  Vec3f diff = s_c - segment_point;
-
-  FCL_REAL distance = diff.norm() - s1.radius - s2.radius;
-
-  if(dist) *dist = distance;
+  normal = s_c - segment_point;
 
-  if(p1 || p2) diff.normalize();
-  if(p1)
-  {
-    *p1 = s_c - diff * s1.radius;
-  }
+  dist = normal.norm() - s1.radius - s2.radius;
 
-  if(p2) *p2 = segment_point + diff * s1.radius;
+  normal.normalize();
+  p1 = s_c - normal * s1.radius;
+  p2 = segment_point + normal * s1.radius;
 
-  if(distance <= 0)
+  if(dist <= 0)
     return false;
 
   return true;
@@ -170,19 +164,19 @@ bool sphereSphereIntersect(const Sphere& s1, const Transform3f& tf1,
 
 bool sphereSphereDistance(const Sphere& s1, const Transform3f& tf1,
                           const Sphere& s2, const Transform3f& tf2,
-                          FCL_REAL* dist, Vec3f* p1, Vec3f* p2)
+                          FCL_REAL& dist, Vec3f& p1, Vec3f& p2, Vec3f& normal)
 {
   Vec3f o1 = tf1.getTranslation();
   Vec3f o2 = tf2.getTranslation();
   Vec3f diff = o1 - o2;
   FCL_REAL len = diff.norm();
-  FCL_REAL d (len - s1.radius - s2.radius);
+  normal = -diff/len;
+  dist = len - s1.radius - s2.radius;
 
-  if(dist) *dist = d;
-  if(p1) *p1 = o1 - diff * (s1.radius / len);
-  if(p2) *p2 = o2 + diff * (s2.radius / len);
+  p1 = o1 - diff * (s1.radius / len);
+  p2 = o2 + diff * (s2.radius / len);
 
-  return (d >=0);
+  return (dist >=0);
 }
 
 /** \brief the minimum distance from a point to a line */
@@ -2824,7 +2818,7 @@ bool GJKSolver_indep::shapeIntersect<Plane, Plane>(const Plane& s1, const Transf
 
 
 template<>
-bool GJKSolver_indep::shapeTriangleIntersect
+bool GJKSolver_indep::shapeTriangleInteraction
 (const Sphere& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2,
  const Vec3f& P3, const Transform3f& tf2, FCL_REAL& distance,
  Vec3f& p1, Vec3f& p2, Vec3f& normal) const
@@ -2835,7 +2829,7 @@ bool GJKSolver_indep::shapeTriangleIntersect
 }
 
 template<>
-bool GJKSolver_indep::shapeTriangleIntersect
+bool GJKSolver_indep::shapeTriangleInteraction
 (const Halfspace& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2,
  const Vec3f& P3, const Transform3f& tf2, FCL_REAL& distance,
  Vec3f& p1, Vec3f& p2, Vec3f& normal) const
@@ -2845,7 +2839,7 @@ bool GJKSolver_indep::shapeTriangleIntersect
 }
 
 template<>
-bool GJKSolver_indep::shapeTriangleIntersect
+bool GJKSolver_indep::shapeTriangleInteraction
 (const Plane& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2,
  const Vec3f& P3, const Transform3f& tf2, FCL_REAL& distance,
  Vec3f& p1, Vec3f& p2, Vec3f& normal) const
@@ -2877,54 +2871,39 @@ bool GJKSolver_indep::shapeTriangleIntersect
 // +------------+-----+--------+---------+------+----------+-------+------------+----------+
 
 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
+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, Vec3f& normal) const
 {
-  return details::sphereCapsuleDistance(s1, tf1, s2, tf2, dist, p1, p2);
+  return details::sphereCapsuleDistance(s1, tf1, s2, tf2, dist, p1, p2, normal);
 }
 
 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
+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, Vec3f& normal) const
 {
-  return details::sphereCapsuleDistance(s2, tf2, s1, tf1, dist, p2, p1);
+  return details::sphereCapsuleDistance(s2, tf2, s1, tf1, dist, p2, p1, normal);
 }
 
 template<>
-bool GJKSolver_indep::shapeDistance<Sphere, Sphere>(const Sphere& s1, const Transform3f& tf1,
-                                                    const Sphere& s2, const Transform3f& tf2,
-                                                    FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const
+bool GJKSolver_indep::shapeDistance<Sphere, Sphere>
+(const Sphere& s1, const Transform3f& tf1,
+ const Sphere& s2, const Transform3f& tf2,
+ FCL_REAL& dist, Vec3f& p1, Vec3f& p2, Vec3f& normal) const
 {
-  return details::sphereSphereDistance(s1, tf1, s2, tf2, dist, p1, p2);
+  return details::sphereSphereDistance(s1, tf1, s2, tf2, dist, p1, p2, normal);
 }
 
 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::shapeDistance<Capsule, Capsule>
+(const Capsule& /*s1*/, const Transform3f& /*tf1*/,
+ const Capsule& /*s2*/, const Transform3f& /*tf2*/,
+ FCL_REAL& /*dist*/, Vec3f& /*p1*/, Vec3f& /*p2*/, Vec3f& /*normal*/) const
 {
   abort ();
 }
 
-
-
-
-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
-{
-  return details::sphereTriangleDistance(s, tf, P1, P2, P3, 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);
-}
-
 } // fcl
diff --git a/src/traversal/traversal_node_base.cpp b/src/traversal/traversal_node_base.cpp
index fcaeb160..1e592eba 100644
--- a/src/traversal/traversal_node_base.cpp
+++ b/src/traversal/traversal_node_base.cpp
@@ -46,17 +46,17 @@ TraversalNodeBase::~TraversalNodeBase()
 {
 }
 
-bool TraversalNodeBase::isFirstNodeLeaf(int b) const
+  bool TraversalNodeBase::isFirstNodeLeaf(int /*b*/) const
 {
   return true;
 }
 
-bool TraversalNodeBase::isSecondNodeLeaf(int b) const
+  bool TraversalNodeBase::isSecondNodeLeaf(int /*b*/) const
 {
   return true;
 }
 
-bool TraversalNodeBase::firstOverSecond(int b1, int b2) const
+  bool TraversalNodeBase::firstOverSecond(int /*b1*/, int /*b2*/) const
 {
   return true;
 }
@@ -95,12 +95,12 @@ DistanceTraversalNodeBase::~DistanceTraversalNodeBase()
 {
 }
 
-FCL_REAL DistanceTraversalNodeBase::BVTesting(int b1, int b2) const
+  FCL_REAL DistanceTraversalNodeBase::BVTesting(int /*b1*/, int /*b2*/) const
 {
   return std::numeric_limits<FCL_REAL>::max();
 }
 
-bool DistanceTraversalNodeBase::canStop(FCL_REAL c) const
+  bool DistanceTraversalNodeBase::canStop(FCL_REAL /*c*/) const
 {
   return false;
 }
diff --git a/src/traversal/traversal_node_bvhs.cpp b/src/traversal/traversal_node_bvhs.cpp
index ebc3b72f..ed1929a3 100644
--- a/src/traversal/traversal_node_bvhs.cpp
+++ b/src/traversal/traversal_node_bvhs.cpp
@@ -74,18 +74,15 @@ static inline void meshDistanceOrientedNodeLeafTesting(int b1, int b2,
   const Vec3f& t23 = vertices2[tri_id2[2]];
 
   // nearest point pair
-  Vec3f P1, P2;
+  Vec3f P1, P2, normal;
 
   FCL_REAL d = sqrt (TriangleDistance::sqrTriDistance
 		     (t11, t12, t13, t21, t22, t23, R, T, P1, P2));
 
-  if(request.enable_nearest_points)
-    result.update(d, model1, model2, primitive_id1, primitive_id2, P1, P2);
-  else
-    result.update(d, model1, model2, primitive_id1, primitive_id2);
-}
-
+  result.update(d, model1, model2, primitive_id1, primitive_id2, P1, P2,
+                normal);
 }
+} // namespace details
 
 MeshCollisionTraversalNodeOBB::MeshCollisionTraversalNodeOBB
 (const CollisionRequest& request) :
@@ -173,17 +170,15 @@ static inline void distancePreprocessOrientedNode(const BVHModel<BV>* model1, co
   init_tri2_points[1] = vertices2[init_tri2[1]];
   init_tri2_points[2] = vertices2[init_tri2[2]];
 
-  Vec3f p1, p2;
+  Vec3f p1, p2, normal;
   FCL_REAL distance = sqrt (TriangleDistance::sqrTriDistance
 			    (init_tri1_points[0], init_tri1_points[1],
 			     init_tri1_points[2], init_tri2_points[0],
 			     init_tri2_points[1], init_tri2_points[2],
 			     R, T, p1, p2));
 
-  if(request.enable_nearest_points)
-    result.update(distance, model1, model2, init_tri_id1, init_tri_id2, p1, p2);
-  else
-    result.update(distance, model1, model2, init_tri_id1, init_tri_id2);
+  result.update(distance, model1, model2, init_tri_id1, init_tri_id2, p1, p2,
+                normal);
 }
 
 template<typename BV>
diff --git a/test/test_fcl_geometric_shapes.cpp b/test/test_fcl_geometric_shapes.cpp
index d694a63a..99e0540f 100644
--- a/test/test_fcl_geometric_shapes.cpp
+++ b/test/test_fcl_geometric_shapes.cpp
@@ -149,15 +149,15 @@ void compareContact(const S1& s1, const Transform3f& tf1,
 }
 
 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)
+void testShapeIntersection(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;
@@ -229,69 +229,69 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_spheresphere)
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(40, 0, 0));
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, false);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(40, 0, 0));
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, false);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(30, 0, 0));
   normal << 1, 0, 0;
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(30.01, 0, 0));
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, false);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(30.01, 0, 0));
   normal = transform.getRotation() * Vec3f(1, 0, 0);
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, false);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(29.9, 0, 0));
   normal << 1, 0, 0;
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(29.9, 0, 0));
   normal = transform.getRotation() * Vec3f(1, 0, 0);
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f();
   normal.setZero();  // If the centers of two sphere are at the same position, the normal is (0, 0, 0)
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
 
   tf1 = transform;
   tf2 = transform;
   normal.setZero();  // If the centers of two sphere are at the same position, the normal is (0, 0, 0)
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(-29.9, 0, 0));
   normal << -1, 0, 0;
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(-29.9, 0, 0));
   normal = transform.getRotation() * Vec3f(-1, 0, 0);
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(-30.0, 0, 0));
   normal << -1, 0, 0;
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(-30.01, 0, 0));
   normal << -1, 0, 0;
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, false);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(-30.01, 0, 0));
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, false);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);
 }
 
 bool compareContactPoints(const Vec3f& c1,const Vec3f& c2)
@@ -366,32 +366,32 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_boxbox)
   tf2 = Transform3f();
   // TODO: Need convention for normal when the centers of two objects are at same position. The current result is (1, 0, 0).
   normal << 1, 0, 0;
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
 
   tf1 = transform;
   tf2 = transform;
   // TODO: Need convention for normal when the centers of two objects are at same position. The current result is (1, 0, 0).
   normal = transform.getRotation() * Vec3f(1, 0, 0);
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(15, 0, 0));
   normal << 1, 0, 0;
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(15.01, 0, 0));
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, false);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);
 
   tf1 = Transform3f();
   tf2 = Transform3f(q);
   normal << 1, 0, 0;
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
 
   tf1 = transform;
   tf2 = transform * Transform3f(q);
   normal = transform.getRotation() * Vec3f(1, 0, 0);
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
 
   FCL_UINT32 numTests = 1e+2;
   for (FCL_UINT32 i = 0; i < numTests; ++i)
@@ -421,30 +421,30 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_spherebox)
   tf2 = Transform3f();
   // TODO: Need convention for normal when the centers of two objects are at same position. The current result is (-1, 0, 0).
   normal << -1, 0, 0;
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
 
   tf1 = transform;
   tf2 = transform;
   // TODO: Need convention for normal when the centers of two objects are at same position.
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(22.50001, 0, 0));
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, false);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(22.501, 0, 0));
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, false);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(22.4, 0, 0));
   normal << 1, 0, 0;
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal, false, tol_gjk);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal, false, tol_gjk);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(22.4, 0, 0));
   normal = transform.getRotation() * Vec3f(1, 0, 0);
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal, false, tol_gjk);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal, false, tol_gjk);
 }
 
 BOOST_AUTO_TEST_CASE(shapeIntersection_spherecapsule)
@@ -465,42 +465,42 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_spherecapsule)
   tf1 = Transform3f();
   tf2 = Transform3f();
   // TODO: Need convention for normal when the centers of two objects are at same position.
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
 
   tf1 = transform;
   tf2 = transform;
   // TODO: Need convention for normal when the centers of two objects are at same position.
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(24.9, 0, 0));
   normal << 1, 0, 0;
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(24.9, 0, 0));
   normal = transform.getRotation() * Vec3f(1, 0, 0);
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(25, 0, 0));
   normal << 1, 0, 0;
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(24.999999, 0, 0));
   normal = transform.getRotation() * Vec3f(1, 0, 0);
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(25.1, 0, 0));
   normal << 1, 0, 0;
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, false);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(25.1, 0, 0));
   normal = transform.getRotation() * Vec3f(1, 0, 0);
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, false);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);
 }
 
 BOOST_AUTO_TEST_CASE(shapeIntersection_cylindercylinder)
@@ -521,35 +521,35 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_cylindercylinder)
   tf1 = Transform3f();
   tf2 = Transform3f();
   // TODO: Need convention for normal when the centers of two objects are at same position.
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
 
   tf1 = transform;
   tf2 = transform;
   // TODO: Need convention for normal when the centers of two objects are at same position.
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(0, 9.9, 0));
   normal << 0, 1, 0;
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal, false, tol_gjk);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal, false, tol_gjk);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(9.9, 0, 0));
   normal << 1, 0, 0;
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal, false, tol_gjk);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal, false, tol_gjk);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(9.9, 0, 0));
   normal = transform.getRotation() * Vec3f(1, 0, 0);
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal, false, tol_gjk);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal, false, tol_gjk);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(10.01, 0, 0));
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, false);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(10.01, 0, 0));
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, false);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);
 }
 
 /*
@@ -571,40 +571,40 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_conecone)
   tf1 = Transform3f();
   tf2 = Transform3f();
   // TODO: Need convention for normal when the centers of two objects are at same position.
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
 
   tf1 = transform;
   tf2 = transform;
   // TODO: Need convention for normal when the centers of two objects are at same position.
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(9.9, 0, 0));
   normal << 1, 0, 0;
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal, false, tol_gjk);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal, false, tol_gjk);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(9.9, 0, 0));
   normal = transform.getRotation() * Vec3f(1, 0, 0);
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal, false, tol_gjk);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal, false, tol_gjk);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(10.001, 0, 0));
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, false);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(10.001, 0, 0));
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, false);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(0, 0, 9.9));
   normal << 0, 0, 1;
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(0, 0, 9.9));
   normal = transform.getRotation() * Vec3f(0, 0, 1);
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
 }
 */
 
@@ -627,48 +627,48 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_conecylinder)
   tf1 = Transform3f();
   tf2 = Transform3f();
   // TODO: Need convention for normal when the centers of two objects are at same position.
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
 
   tf1 = transform;
   tf2 = transform;
   // TODO: Need convention for normal when the centers of two objects are at same position.
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(9.9, 0, 0));
   normal << 1, 0, 0;
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal, false, 0.061);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal, false, 0.061);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(9.9, 0, 0));
   normal = transform.getRotation() * Vec3f(1, 0, 0);
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal, false, 0.46);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal, false, 0.46);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(10.01, 0, 0));
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, false);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(10.01, 0, 0));
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, false);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(0, 0, 9.9));
   normal << 0, 0, 1;
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(0, 0, 9.9));
   normal = transform.getRotation() * Vec3f(0, 0, 1);
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(0, 0, 10.01));
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, false);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(0, 0, 10.01));
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, false);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);
 }
 */
 
@@ -687,33 +687,36 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_spheretriangle)
   FCL_REAL distance;
   bool res;
 
-  res = solver1.shapeTriangleIntersect
+  res = solver1.shapeTriangleInteraction
     (s, Transform3f(), t[0], t[1], t[2], Transform3f(), distance, c1, c2,
      normal);
   BOOST_CHECK(res);
 
-  res =  solver1.shapeTriangleIntersect(s, transform, t[0], t[1], t[2],
-                                        transform, distance, c1, c2, normal);
+  res =  solver1.shapeTriangleInteraction(s, transform, t[0], t[1], t[2],
+                                          transform, distance, c1, c2, normal);
   BOOST_CHECK(res);
 
 
   t[0] << 30, 0, 0;
   t[1] << 9.9, -20, 0;
   t[2] << 9.9, 20, 0;
-  res = solver1.shapeTriangleIntersect(s, Transform3f(), t[0], t[1], t[2],
-                                       Transform3f(), distance, c1, c2, normal);
+  res = solver1.shapeTriangleInteraction
+    (s, Transform3f(), t[0], t[1], t[2], Transform3f(), distance, c1, c2,
+     normal);
   BOOST_CHECK(res);
 
-  res =  solver1.shapeTriangleIntersect(s, transform, t[0], t[1], t[2], transform, distance, c1, c2, normal);
+  res =  solver1.shapeTriangleInteraction
+    (s, transform, t[0], t[1], t[2], transform, distance, c1, c2, normal);
   BOOST_CHECK(res);
 
-  res = solver1.shapeTriangleIntersect(s, Transform3f(), t[0], t[1], t[2],
-                                       Transform3f(), distance, c1, c2, normal);
+  res = solver1.shapeTriangleInteraction
+    (s, Transform3f(), t[0], t[1], t[2], Transform3f(), distance, c1, c2,
+     normal);
   BOOST_CHECK(res);
   BOOST_CHECK(isEqual(normal, Vec3f(1, 0, 0), 1e-9));
 
-  res =  solver1.shapeTriangleIntersect(s, transform, t[0], t[1], t[2],
-                                        transform, distance, c1, c2, normal);
+  res =  solver1.shapeTriangleInteraction
+    (s, transform, t[0], t[1], t[2], transform, distance, c1, c2, normal);
   BOOST_CHECK(res);
   BOOST_CHECK(isEqual(normal, transform.getRotation() * Vec3f(1, 0, 0), 1e-9));
 }
@@ -734,27 +737,36 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacetriangle)
   Vec3f normal;
   bool res;
 
-  res = solver1.shapeTriangleIntersect(hs, Transform3f(), t[0], t[1], t[2], Transform3f(), distance, c1, c2, normal);
+  res = solver1.shapeTriangleInteraction
+    (hs, Transform3f(), t[0], t[1], t[2], Transform3f(), distance, c1, c2,
+     normal);
   BOOST_CHECK(res);
 
-  res =  solver1.shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, distance, c1, c2, normal);
+  res =  solver1.shapeTriangleInteraction
+    (hs, transform, t[0], t[1], t[2], transform, distance, c1, c2, normal);
   BOOST_CHECK(res);
 
 
   t[0] << 20, 0, 0;
   t[1] << 0, -20, 0;
   t[2] << 0, 20, 0;
-  res = solver1.shapeTriangleIntersect(hs, Transform3f(), t[0], t[1], t[2], Transform3f(), distance, c1, c2, normal);
+  res = solver1.shapeTriangleInteraction
+    (hs, Transform3f(), t[0], t[1], t[2], Transform3f(), distance, c1, c2,
+     normal);
   BOOST_CHECK(res);
 
-  res =  solver1.shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, distance, c1, c2, normal);
+  res =  solver1.shapeTriangleInteraction
+    (hs, transform, t[0], t[1], t[2], transform, distance, c1, c2, normal);
   BOOST_CHECK(res);
 
-  res = solver1.shapeTriangleIntersect(hs, Transform3f(), t[0], t[1], t[2], Transform3f(), distance, c1, c2, normal);
+  res = solver1.shapeTriangleInteraction
+    (hs, Transform3f(), t[0], t[1], t[2], Transform3f(), distance, c1, c2,
+     normal);
   BOOST_CHECK(res);
   BOOST_CHECK(isEqual(normal, Vec3f(1, 0, 0), 1e-9));
 
-  res =  solver1.shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, distance, c1, c2, normal);
+  res =  solver1.shapeTriangleInteraction
+    (hs, transform, t[0], t[1], t[2], transform, distance, c1, c2, normal);
   BOOST_CHECK(res);
   BOOST_CHECK(isEqual(normal, transform.getRotation() * Vec3f(1, 0, 0), 1e-9));
 }
@@ -775,10 +787,13 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planetriangle)
   Vec3f normal;
   bool res;
 
-  res = solver1.shapeTriangleIntersect(hs, Transform3f(), t[0], t[1], t[2], Transform3f(), distance, c1, c2, normal);
+  res = solver1.shapeTriangleInteraction
+    (hs, Transform3f(), t[0], t[1], t[2], Transform3f(), distance, c1, c2,
+     normal);
   BOOST_CHECK(res);
 
-  res =  solver1.shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, distance, c1, c2, normal);
+  res =  solver1.shapeTriangleInteraction
+    (hs, transform, t[0], t[1], t[2], transform, distance, c1, c2, normal);
   BOOST_CHECK(res);
 
 
@@ -786,14 +801,18 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planetriangle)
   t[1] << -0.1, -20, 0;
   t[2] << -0.1, 20, 0;
 
-  res =  solver1.shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, distance, c1, c2, normal);
+  res =  solver1.shapeTriangleInteraction
+    (hs, transform, t[0], t[1], t[2], transform, distance, c1, c2, normal);
   BOOST_CHECK(res);
 
-  res = solver1.shapeTriangleIntersect(hs, Transform3f(), t[0], t[1], t[2], Transform3f(), distance, c1, c2, normal);
+  res = solver1.shapeTriangleInteraction
+    (hs, Transform3f(), t[0], t[1], t[2], Transform3f(), distance, c1, c2,
+     normal);
   BOOST_CHECK(res);
   BOOST_CHECK(isEqual(normal, Vec3f(1, 0, 0), 1e-9));
 
-  res =  solver1.shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, distance, c1, c2, normal);
+  res =  solver1.shapeTriangleInteraction
+    (hs, transform, t[0], t[1], t[2], transform, distance, c1, c2, normal);
   BOOST_CHECK(res);
   BOOST_CHECK(isEqual(normal, transform.getRotation() * Vec3f(1, 0, 0), 1e-9));
 }
@@ -818,64 +837,64 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacesphere)
   contact << -5, 0, 0;
   depth = 10;
   normal << -1, 0, 0;
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = transform;
   tf2 = transform;
   contact = transform.transform(Vec3f(-5, 0, 0));
   depth = 10;
   normal = transform.getQuatRotation() * Vec3f(-1, 0, 0);
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(5, 0, 0));
   contact << -2.5, 0, 0;
   depth = 15;
   normal << -1, 0, 0;
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, 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() * Vec3f(-1, 0, 0);
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(-5, 0, 0));
   contact << -7.5, 0, 0;
   depth = 5;
   normal << -1, 0, 0;
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, 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() * Vec3f(-1, 0, 0);
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(-10.1, 0, 0));
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, false);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, false);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(-10.1, 0, 0));
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, false);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, false);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(10.1, 0, 0));
   contact << 0.05, 0, 0;
   depth = 20.1;
   normal << -1, 0, 0;
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, 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() * Vec3f(-1, 0, 0);
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 }
 
 BOOST_AUTO_TEST_CASE(shapeIntersection_planesphere)
@@ -898,58 +917,58 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planesphere)
   contact.setZero();
   depth = 10;
   normal << 1, 0, 0;  // (1, 0, 0) or (-1, 0, 0)
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = transform;
   tf2 = transform;
   contact = transform.transform(Vec3f(0, 0, 0));
   depth = 10;
   normal = transform.getRotation() * Vec3f(1, 0, 0);  // (1, 0, 0) or (-1, 0, 0)
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(5, 0, 0));
   contact << 5, 0, 0;
   depth = 5;
   normal << 1, 0, 0;
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(5, 0, 0));
   contact = transform.transform(Vec3f(5, 0, 0));
   depth = 5;
   normal = transform.getRotation() * Vec3f(1, 0, 0);
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(-5, 0, 0));
   contact << -5, 0, 0;
   depth = 5;
   normal << -1, 0, 0;
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(-5, 0, 0));
   contact = transform.transform(Vec3f(-5, 0, 0));
   depth = 5;
   normal = transform.getRotation() * Vec3f(-1, 0, 0);
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(-10.1, 0, 0));
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, false);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, false);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(-10.1, 0, 0));
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, false);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, false);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(10.1, 0, 0));
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, false);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, false);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(10.1, 0, 0));
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, false);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, false);
 }
 
 BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacebox)
@@ -972,68 +991,68 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacebox)
   contact << -1.25, 0, 0;
   depth = 2.5;
   normal << -1, 0, 0;
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = transform;
   tf2 = transform;
   contact = transform.transform(Vec3f(-1.25, 0, 0));
   depth = 2.5;
   normal = transform.getQuatRotation() * Vec3f(-1, 0, 0);
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(1.25, 0, 0));
   contact << -0.625, 0, 0;
   depth = 3.75;
   normal << -1, 0, 0;
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, 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() * Vec3f(-1, 0, 0);
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(-1.25, 0, 0));
   contact << -1.875, 0, 0;
   depth = 1.25;
   normal << -1, 0, 0;
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, 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() * Vec3f(-1, 0, 0);
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(2.51, 0, 0));
   contact << 0.005, 0, 0;
   depth = 5.01;
   normal << -1, 0, 0;
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, 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() * Vec3f(-1, 0, 0);
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(-2.51, 0, 0));
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, false);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, false);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(-2.51, 0, 0));
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, false);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, false);
 
   tf1 = Transform3f(transform.getQuatRotation());
   tf2 = Transform3f();
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true);
 }
 
 BOOST_AUTO_TEST_CASE(shapeIntersection_planebox)
@@ -1056,62 +1075,62 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planebox)
   contact << 0, 0, 0;
   depth = 2.5;
   normal << 1, 0, 0;  // (1, 0, 0) or (-1, 0, 0)
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = transform;
   tf2 = transform;
   contact = transform.transform(Vec3f(0, 0, 0));
   depth = 2.5;
   normal = transform.getRotation() * Vec3f(1, 0, 0);  // (1, 0, 0) or (-1, 0, 0)
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(1.25, 0, 0));
   contact << 1.25, 0, 0;
   depth = 1.25;
   normal << 1, 0, 0;
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(1.25, 0, 0));
   contact = transform.transform(Vec3f(1.25, 0, 0));
   depth = 1.25;
   normal = transform.getRotation() * Vec3f(1, 0, 0);
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(-1.25, 0, 0));
   contact << -1.25, 0, 0;
   depth = 1.25;
   normal << -1, 0, 0;
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(-1.25, 0, 0));
   contact = transform.transform(Vec3f(-1.25, 0, 0));
   depth = 1.25;
   normal = transform.getRotation() * Vec3f(-1, 0, 0);
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(2.51, 0, 0));
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, false);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, false);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(2.51, 0, 0));
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, false);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, false);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(-2.51, 0, 0));
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, false);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, false);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(-2.51, 0, 0));
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, false);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, false);
 
   tf1 = Transform3f(transform.getQuatRotation());
   tf2 = Transform3f();
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true);
 }
 
 BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecapsule)
@@ -1134,64 +1153,64 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecapsule)
   contact << -2.5, 0, 0;
   depth = 5;
   normal << -1, 0, 0;
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = transform;
   tf2 = transform;
   contact = transform.transform(Vec3f(-2.5, 0, 0));
   depth = 5;
   normal = transform.getRotation() * Vec3f(-1, 0, 0);
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(2.5, 0, 0));
   contact << -1.25, 0, 0;
   depth = 7.5;
   normal << -1, 0, 0;
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, 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.getRotation() * Vec3f(-1, 0, 0);
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(-2.5, 0, 0));
   contact << -3.75, 0, 0;
   depth = 2.5;
   normal << -1, 0, 0;
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, 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.getRotation() * Vec3f(-1, 0, 0);
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(5.1, 0, 0));
   contact << 0.05, 0, 0;
   depth = 10.1;
   normal << -1, 0, 0;
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, 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.getRotation() * Vec3f(-1, 0, 0);
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(-5.1, 0, 0));
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, false);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, false);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(-5.1, 0, 0));
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, false);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, false);
 
 
 
@@ -1203,64 +1222,64 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecapsule)
   contact << 0, -2.5, 0;
   depth = 5;
   normal << 0, -1, 0;
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = transform;
   tf2 = transform;
   contact = transform.transform(Vec3f(0, -2.5, 0));
   depth = 5;
   normal = transform.getRotation() * Vec3f(0, -1, 0);
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(0, 2.5, 0));
   contact << 0, -1.25, 0;
   depth = 7.5;
   normal << 0, -1, 0;
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, 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.getRotation() * Vec3f(0, -1, 0);
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(0, -2.5, 0));
   contact << 0, -3.75, 0;
   depth = 2.5;
   normal << 0, -1, 0;
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, 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.getRotation() * Vec3f(0, -1, 0);
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(0, 5.1, 0));
   contact << 0, 0.05, 0;
   depth = 10.1;
   normal << 0, -1, 0;
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, 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.getRotation() * Vec3f(0, -1, 0);
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(0, -5.1, 0));
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, false);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, false);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(0, -5.1, 0));
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, false);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, false);
 
 
 
@@ -1272,64 +1291,64 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecapsule)
   contact << 0, 0, -5;
   depth = 10;
   normal << 0, 0, -1;
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = transform;
   tf2 = transform;
   contact = transform.transform(Vec3f(0, 0, -5));
   depth = 10;
   normal = transform.getRotation() * Vec3f(0, 0, -1);
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(0, 0, 2.5));
   contact << 0, 0, -3.75;
   depth = 12.5;
   normal << 0, 0, -1;
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, 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.getRotation() * Vec3f(0, 0, -1);
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(0, 0, -2.5));
   contact << 0, 0, -6.25;
   depth = 7.5;
   normal << 0, 0, -1;
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, 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.getRotation() * Vec3f(0, 0, -1);
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(0, 0, 10.1));
   contact << 0, 0, 0.05;
   depth = 20.1;
   normal << 0, 0, -1;
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, 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.getRotation() * Vec3f(0, 0, -1);
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(0, 0, -10.1));
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, false);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, false);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(0, 0, -10.1));
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, false);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, false);
 }
 
 BOOST_AUTO_TEST_CASE(shapeIntersection_planecapsule)
@@ -1352,58 +1371,58 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecapsule)
   contact << 0, 0, 0;
   depth = 5;
   normal << 1, 0, 0;  // (1, 0, 0) or (-1, 0, 0)
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal, true);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal, true);
 
   tf1 = transform;
   tf2 = transform;
   contact = transform.transform(Vec3f(0, 0, 0));
   depth = 5;
   normal = transform.getRotation() * Vec3f(1, 0, 0);  // (1, 0, 0) or (-1, 0, 0)
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal, true);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal, true);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(2.5, 0, 0));
   contact << 2.5, 0, 0;
   depth = 2.5;
   normal << 1, 0, 0;
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(2.5, 0, 0));
   contact = transform.transform(Vec3f(2.5, 0, 0));
   depth = 2.5;
   normal = transform.getRotation() * Vec3f(1, 0, 0);
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(-2.5, 0, 0));
   contact << -2.5, 0, 0;
   depth = 2.5;
   normal << -1, 0, 0;
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(-2.5, 0, 0));
   contact = transform.transform(Vec3f(-2.5, 0, 0));
   depth = 2.5;
   normal = transform.getRotation() * Vec3f(-1, 0, 0);
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(5.1, 0, 0));
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, false);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, false);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(5.1, 0, 0));
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, false);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, false);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(-5.1, 0, 0));
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, false);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, false);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(-5.1, 0, 0));
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, false);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, false);
 
 
 
@@ -1415,58 +1434,58 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecapsule)
   contact << 0, 0, 0;
   depth = 5;
   normal << 0, 1, 0;  // (0, 1, 0) or (0, -1, 0)
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal, true);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal, true);
 
   tf1 = transform;
   tf2 = transform;
   contact = transform.transform(Vec3f(0, 0, 0));
   depth = 5;
   normal = transform.getRotation() * Vec3f(0, 1, 0);  // (0, 1, 0) or (0, -1, 0)
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal, true);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal, true);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(0, 2.5, 0));
   contact << 0, 2.5, 0;
   depth = 2.5;
   normal << 0, 1, 0;
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(0, 2.5, 0));
   contact = transform.transform(Vec3f(0, 2.5, 0));
   depth = 2.5;
   normal = transform.getRotation() * Vec3f(0, 1, 0);
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(0, -2.5, 0));
   contact << 0, -2.5, 0;
   depth = 2.5;
   normal << 0, -1, 0;
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(0, -2.5, 0));
   contact = transform.transform(Vec3f(0, -2.5, 0));
   depth = 2.5;
   normal = transform.getRotation() * Vec3f(0, -1, 0);
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(0, 5.1, 0));
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, false);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, false);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(0, 5.1, 0));
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, false);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, false);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(0, -5.1, 0));
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, false);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, false);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(0, -5.1, 0));
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, false);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, false);
 
 
 
@@ -1478,58 +1497,58 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecapsule)
   contact << 0, 0, 0;
   depth = 10;
   normal << 0, 0, 1;  // (0, 0, 1) or (0, 0, -1)
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal, true);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal, true);
 
   tf1 = transform;
   tf2 = transform;
   contact = transform.transform(Vec3f(0, 0, 0));
   depth = 10;
   normal = transform.getRotation() * Vec3f(0, 0, 1);  // (0, 0, 1) or (0, 0, -1)
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal, true);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal, true);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(0, 0, 2.5));
   contact << 0, 0, 2.5;
   depth = 7.5;
   normal << 0, 0, 1;
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(0, 0, 2.5));
   contact = transform.transform(Vec3f(0, 0, 2.5));
   depth = 7.5;
   normal = transform.getRotation() * Vec3f(0, 0, 1);
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(0, 0, -2.5));
   contact << 0, 0, -2.5;
   depth = 7.5;
   normal << 0, 0, -1;
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(0, 0, -2.5));
   contact = transform.transform(Vec3f(0, 0, -2.5));
   depth = 7.5;
   normal = transform.getRotation() * Vec3f(0, 0, -1);
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(0, 0, 10.1));
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, false);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, false);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(0, 0, 10.1));
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, false);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, false);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(0, 0, -10.1));
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, false);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, false);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(0, 0, -10.1));
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, false);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, false);
 }
 
 BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecylinder)
@@ -1552,64 +1571,64 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecylinder)
   contact << -2.5, 0, 0;
   depth = 5;
   normal << -1, 0, 0;
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = transform;
   tf2 = transform;
   contact = transform.transform(Vec3f(-2.5, 0, 0));
   depth = 5;
   normal = transform.getRotation() * Vec3f(-1, 0, 0);
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(2.5, 0, 0));
   contact << -1.25, 0, 0;
   depth = 7.5;
   normal << -1, 0, 0;
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, 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.getRotation() * Vec3f(-1, 0, 0);
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(-2.5, 0, 0));
   contact << -3.75, 0, 0;
   depth = 2.5;
   normal << -1, 0, 0;
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, 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.getRotation() * Vec3f(-1, 0, 0);
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(5.1, 0, 0));
   contact << 0.05, 0, 0;
   depth = 10.1;
   normal << -1, 0, 0;
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, 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.getRotation() * Vec3f(-1, 0, 0);
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(-5.1, 0, 0));
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, false);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, false);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(-5.1, 0, 0));
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, false);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, false);
 
 
 
@@ -1621,64 +1640,64 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecylinder)
   contact << 0, -2.5, 0;
   depth = 5;
   normal << 0, -1, 0;
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = transform;
   tf2 = transform;
   contact = transform.transform(Vec3f(0, -2.5, 0));
   depth = 5;
   normal = transform.getRotation() * Vec3f(0, -1, 0);
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(0, 2.5, 0));
   contact << 0, -1.25, 0;
   depth = 7.5;
   normal << 0, -1, 0;
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, 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.getRotation() * Vec3f(0, -1, 0);
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(0, -2.5, 0));
   contact << 0, -3.75, 0;
   depth = 2.5;
   normal << 0, -1, 0;
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, 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.getRotation() * Vec3f(0, -1, 0);
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(0, 5.1, 0));
   contact << 0, 0.05, 0;
   depth = 10.1;
   normal << 0, -1, 0;
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, 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.getRotation() * Vec3f(0, -1, 0);
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(0, -5.1, 0));
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, false);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, false);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(0, -5.1, 0));
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, false);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, false);
 
 
 
@@ -1690,64 +1709,64 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecylinder)
   contact << 0, 0, -2.5;
   depth = 5;
   normal << 0, 0, -1;
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = transform;
   tf2 = transform;
   contact = transform.transform(Vec3f(0, 0, -2.5));
   depth = 5;
   normal = transform.getRotation() * Vec3f(0, 0, -1);
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(0, 0, 2.5));
   contact << 0, 0, -1.25;
   depth = 7.5;
   normal << 0, 0, -1;
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, 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.getRotation() * Vec3f(0, 0, -1);
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(0, 0, -2.5));
   contact << 0, 0, -3.75;
   depth = 2.5;
   normal << 0, 0, -1;
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, 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.getRotation() * Vec3f(0, 0, -1);
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(0, 0, 5.1));
   contact << 0, 0, 0.05;
   depth = 10.1;
   normal << 0, 0, -1;
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, 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.getRotation() * Vec3f(0, 0, -1);
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(0, 0, -5.1));
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, false);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, false);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(0, 0, -5.1));
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, false);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, false);
 }
 
 BOOST_AUTO_TEST_CASE(shapeIntersection_planecylinder)
@@ -1770,58 +1789,58 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecylinder)
   contact << 0, 0, 0;
   depth = 5;
   normal << 1, 0, 0;  // (1, 0, 0) or (-1, 0, 0)
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal, true);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal, true);
 
   tf1 = transform;
   tf2 = transform;
   contact = transform.transform(Vec3f(0, 0, 0));
   depth = 5;
   normal = transform.getRotation() * Vec3f(1, 0, 0);  // (1, 0, 0) or (-1, 0, 0)
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal, true);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal, true);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(2.5, 0, 0));
   contact << 2.5, 0, 0;
   depth = 2.5;
   normal << 1, 0, 0;
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(2.5, 0, 0));
   contact = transform.transform(Vec3f(2.5, 0, 0));
   depth = 2.5;
   normal = transform.getRotation() * Vec3f(1, 0, 0);
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(-2.5, 0, 0));
   contact << -2.5, 0, 0;
   depth = 2.5;
   normal << -1, 0, 0;
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(-2.5, 0, 0));
   contact = transform.transform(Vec3f(-2.5, 0, 0));
   depth = 2.5;
   normal = transform.getRotation() * Vec3f(-1, 0, 0);
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(5.1, 0, 0));
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, false);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, false);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(5.1, 0, 0));
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, false);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, false);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(-5.1, 0, 0));
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, false);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, false);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(-5.1, 0, 0));
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, false);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, false);
 
 
 
@@ -1833,58 +1852,58 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecylinder)
   contact << 0, 0, 0;
   depth = 5;
   normal << 0, 1, 0;  // (1, 0, 0) or (-1, 0, 0)
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal, true);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal, true);
 
   tf1 = transform;
   tf2 = transform;
   contact = transform.transform(Vec3f(0, 0, 0));
   depth = 5;
   normal = transform.getRotation() * Vec3f(0, 1, 0);  // (1, 0, 0) or (-1, 0, 0)
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal, true);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal, true);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(0, 2.5, 0));
   contact << 0, 2.5, 0;
   depth = 2.5;
   normal << 0, 1, 0;
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(0, 2.5, 0));
   contact = transform.transform(Vec3f(0, 2.5, 0));
   depth = 2.5;
   normal = transform.getRotation() * Vec3f(0, 1, 0);
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(0, -2.5, 0));
   contact << 0, -2.5, 0;
   depth = 2.5;
   normal << 0, -1, 0;
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(0, -2.5, 0));
   contact = transform.transform(Vec3f(0, -2.5, 0));
   depth = 2.5;
   normal = transform.getRotation() * Vec3f(0, -1, 0);
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(0, 5.1, 0));
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, false);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, false);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(0, 5.1, 0));
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, false);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, false);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(0, -5.1, 0));
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, false);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, false);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(0, -5.1, 0));
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, false);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, false);
 
 
 
@@ -1896,58 +1915,58 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecylinder)
   contact << 0, 0, 0;
   depth = 5;
   normal << 0, 0, 1;  // (1, 0, 0) or (-1, 0, 0)
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal, true);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal, true);
 
   tf1 = transform;
   tf2 = transform;
   contact = transform.transform(Vec3f(0, 0, 0));
   depth = 5;
   normal = transform.getRotation() * Vec3f(0, 0, 1);  // (1, 0, 0) or (-1, 0, 0)
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal, true);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal, true);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(0, 0, 2.5));
   contact << 0, 0, 2.5;
   depth = 2.5;
   normal << 0, 0, 1;
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(0, 0, 2.5));
   contact = transform.transform(Vec3f(0, 0, 2.5));
   depth = 2.5;
   normal = transform.getRotation() * Vec3f(0, 0, 1);
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(0, 0, -2.5));
   contact << 0, 0, -2.5;
   depth = 2.5;
   normal << 0, 0, -1;
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(0, 0, -2.5));
   contact = transform.transform(Vec3f(0, 0, -2.5));
   depth = 2.5;
   normal = transform.getRotation() * Vec3f(0, 0, -1);
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(0, 0, 10.1));
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, false);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, false);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(0, 0, 10.1));
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, false);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, false);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(0, 0, -10.1));
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, false);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, false);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(0, 0, -10.1));
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, false);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, false);
 }
 
 
@@ -1971,64 +1990,64 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecone)
   contact << -2.5, 0, -5;
   depth = 5;
   normal << -1, 0, 0;
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = transform;
   tf2 = transform;
   contact = transform.transform(Vec3f(-2.5, 0, -5));
   depth = 5;
   normal = transform.getRotation() * Vec3f(-1, 0, 0);
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(2.5, 0, 0));
   contact << -1.25, 0, -5;
   depth = 7.5;
   normal << -1, 0, 0;
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, 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.getRotation() * Vec3f(-1, 0, 0);
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(-2.5, 0, 0));
   contact << -3.75, 0, -5;
   depth = 2.5;
   normal << -1, 0, 0;
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, 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.getRotation() * Vec3f(-1, 0, 0);
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(5.1, 0, 0));
   contact << 0.05, 0, -5;
   depth = 10.1;
   normal << -1, 0, 0;
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, 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.getRotation() * Vec3f(-1, 0, 0);
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(-5.1, 0, 0));
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, false);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, false);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(-5.1, 0, 0));
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, false);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, false);
 
 
 
@@ -2040,64 +2059,64 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecone)
   contact << 0, -2.5, -5;
   depth = 5;
   normal << 0, -1, 0;
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = transform;
   tf2 = transform;
   contact = transform.transform(Vec3f(0, -2.5, -5));
   depth = 5;
   normal = transform.getRotation() * Vec3f(0, -1, 0);
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(0, 2.5, 0));
   contact << 0, -1.25, -5;
   depth = 7.5;
   normal << 0, -1, 0;
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, 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.getRotation() * Vec3f(0, -1, 0);
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(0, -2.5, 0));
   contact << 0, -3.75, -5;
   depth = 2.5;
   normal << 0, -1, 0;
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, 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.getRotation() * Vec3f(0, -1, 0);
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(0, 5.1, 0));
   contact << 0, 0.05, -5;
   depth = 10.1;
   normal << 0, -1, 0;
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, 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.getRotation() * Vec3f(0, -1, 0);
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(0, -5.1, 0));
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, false);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, false);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(0, -5.1, 0));
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, false);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, false);
 
 
 
@@ -2109,64 +2128,64 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecone)
   contact << 0, 0, -2.5;
   depth = 5;
   normal << 0, 0, -1;
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = transform;
   tf2 = transform;
   contact = transform.transform(Vec3f(0, 0, -2.5));
   depth = 5;
   normal = transform.getRotation() * Vec3f(0, 0, -1);
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(0, 0, 2.5));
   contact << 0, 0, -1.25;
   depth = 7.5;
   normal << 0, 0, -1;
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, 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.getRotation() * Vec3f(0, 0, -1);
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(0, 0, -2.5));
   contact << 0, 0, -3.75;
   depth = 2.5;
   normal << 0, 0, -1;
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, 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.getRotation() * Vec3f(0, 0, -1);
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(0, 0, 5.1));
   contact << 0, 0, 0.05;
   depth = 10.1;
   normal << 0, 0, -1;
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, 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.getRotation() * Vec3f(0, 0, -1);
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(0, 0, -5.1));
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, false);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, false);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(0, 0, -5.1));
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, false);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, false);
 }
 
 BOOST_AUTO_TEST_CASE(shapeIntersection_planecone)
@@ -2189,58 +2208,58 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecone)
   contact << 0, 0, 0;
   depth = 5;
   normal << 1, 0, 0;  // (1, 0, 0) or (-1, 0, 0)
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal, true);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal, true);
 
   tf1 = transform;
   tf2 = transform;
   contact = transform.transform(Vec3f(0, 0, 0));
   depth = 5;
   normal = transform.getRotation() * Vec3f(-1, 0, 0);  // (1, 0, 0) or (-1, 0, 0)
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal, true);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal, true);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(2.5, 0, 0));
   contact << 2.5, 0, -2.5;
   depth = 2.5;
   normal << 1, 0, 0;
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(2.5, 0, 0));
   contact = transform.transform(Vec3f(2.5, 0, -2.5));
   depth = 2.5;
   normal = transform.getRotation() * Vec3f(1, 0, 0);
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(-2.5, 0, 0));
   contact << -2.5, 0, -2.5;
   depth = 2.5;
   normal << -1, 0, 0;
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(-2.5, 0, 0));
   contact = transform.transform(Vec3f(-2.5, 0, -2.5));
   depth = 2.5;
   normal = transform.getRotation() * Vec3f(-1, 0, 0);
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(5.1, 0, 0));
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, false);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, false);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(5.1, 0, 0));
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, false);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, false);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(-5.1, 0, 0));
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, false);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, false);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(-5.1, 0, 0));
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, false);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, false);
 
 
 
@@ -2252,58 +2271,58 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecone)
   contact << 0, 0, 0;
   depth = 5;
   normal << 0, 1, 0;  // (1, 0, 0) or (-1, 0, 0)
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal, true);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal, true);
 
   tf1 = transform;
   tf2 = transform;
   contact = transform.transform(Vec3f(0, 0, 0));
   depth = 5;
   normal = transform.getRotation() * Vec3f(0, 1, 0);  // (1, 0, 0) or (-1, 0, 0)
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal, true);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal, true);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(0, 2.5, 0));
   contact << 0, 2.5, -2.5;
   depth = 2.5;
   normal << 0, 1, 0;
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(0, 2.5, 0));
   contact = transform.transform(Vec3f(0, 2.5, -2.5));
   depth = 2.5;
   normal = transform.getRotation() * Vec3f(0, 1, 0);
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(0, -2.5, 0));
   contact << 0, -2.5, -2.5;
   depth = 2.5;
   normal << 0, -1, 0;
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(0, -2.5, 0));
   contact = transform.transform(Vec3f(0, -2.5, -2.5));
   depth = 2.5;
   normal = transform.getRotation() * Vec3f(0, -1, 0);
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(0, 5.1, 0));
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, false);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, false);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(0, 5.1, 0));
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, false);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, false);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(0, -5.1, 0));
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, false);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, false);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(0, -5.1, 0));
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, false);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, false);
 
 
 
@@ -2315,58 +2334,58 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecone)
   contact << 0, 0, 0;
   depth = 5;
   normal << 0, 0, 1;  // (1, 0, 0) or (-1, 0, 0)
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal, true);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal, true);
 
   tf1 = transform;
   tf2 = transform;
   contact = transform.transform(Vec3f(0, 0, 0));
   depth = 5;
   normal = transform.getRotation() * Vec3f(0, 0, 1);  // (1, 0, 0) or (-1, 0, 0)
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal, true);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal, true);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(0, 0, 2.5));
   contact << 0, 0, 2.5;
   depth = 2.5;
   normal << 0, 0, 1;
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(0, 0, 2.5));
   contact = transform.transform(Vec3f(0, 0, 2.5));
   depth = 2.5;
   normal = transform.getRotation() * Vec3f(0, 0, 1);
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(0, 0, -2.5));
   contact << 0, 0, -2.5;
   depth = 2.5;
   normal << 0, 0, -1;
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(0, 0, -2.5));
   contact = transform.transform(Vec3f(0, 0, -2.5));
   depth = 2.5;
   normal = transform.getRotation() * Vec3f(0, 0, -1);
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(0, 0, 10.1));
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, false);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, false);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(0, 0, 10.1));
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, false);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, false);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(0, 0, -10.1));
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, false);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, false);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(0, 0, -10.1));
-  testShapeInersection(s, tf1, hs, tf2, GST_INDEP, false);
+  testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, false);
 }
 
 
@@ -2381,54 +2400,78 @@ BOOST_AUTO_TEST_CASE(shapeDistance_spheresphere)
 
   bool res;
   FCL_REAL dist = -1;
-  Vec3f closest_p1, closest_p2;
-  res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(0, 40, 0)), &dist, &closest_p1, &closest_p2);
+  Vec3f closest_p1, closest_p2, normal;
+  res = solver1.shapeDistance (s1, Transform3f(), s2,
+                               Transform3f(Vec3f(0, 40, 0)),
+                               dist, closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 10) < 0.001);
   BOOST_CHECK(res);
 
-  res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(30.1, 0, 0)), &dist);
+  res = solver1.shapeDistance(s1, Transform3f(), s2,
+                              Transform3f(Vec3f(30.1, 0, 0)), dist,
+                              closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 0.1) < 0.001);
   BOOST_CHECK(res);
 
-  res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(29.9, 0, 0)), &dist);
+  res = solver1.shapeDistance(s1, Transform3f(), s2,
+                              Transform3f(Vec3f(29.9, 0, 0)), dist,
+                              closest_p1, closest_p2, normal);
   BOOST_CHECK(dist < 0);
   BOOST_CHECK_FALSE(res);
 
-  res = solver1.shapeDistance(s1, Transform3f(Vec3f(40, 0, 0)), s2, Transform3f(), &dist);
+  res = solver1.shapeDistance(s1, Transform3f(Vec3f(40, 0, 0)),
+                              s2, Transform3f(), dist,
+                              closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 10) < 0.001);
   BOOST_CHECK(res);
 
-  res = solver1.shapeDistance(s1, Transform3f(Vec3f(30.1, 0, 0)), s2, Transform3f(), &dist);
+  res = solver1.shapeDistance(s1, Transform3f(Vec3f(30.1, 0, 0)),
+                              s2, Transform3f(), dist,
+                              closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 0.1) < 0.001);
   BOOST_CHECK(res);
 
-  res = solver1.shapeDistance(s1, Transform3f(Vec3f(29.9, 0, 0)), s2, Transform3f(), &dist);
+  res = solver1.shapeDistance(s1, Transform3f(Vec3f(29.9, 0, 0)),
+                              s2, Transform3f(), dist,
+                              closest_p1, closest_p2, normal);
   BOOST_CHECK(dist < 0);
   BOOST_CHECK_FALSE(res);
 
 
-  res = solver1.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)), &dist);
+  res = solver1.shapeDistance(s1, transform,
+                              s2, transform * Transform3f(Vec3f(40, 0, 0)),
+                              dist, closest_p1, closest_p2, normal);
   // this is one problem: the precise is low sometimes
   BOOST_CHECK(fabs(dist - 10) < 0.1);
   BOOST_CHECK(res);
 
-  res = solver1.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(30.1, 0, 0)), &dist);
+  res = solver1.shapeDistance(s1, transform,
+                              s2, transform * Transform3f(Vec3f(30.1, 0, 0)),
+                              dist, closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 0.1) < 0.06);
   BOOST_CHECK(res);
 
-  res = solver1.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(29.9, 0, 0)), &dist);
+  res = solver1.shapeDistance(s1, transform, s2,
+                              transform * Transform3f(Vec3f(29.9, 0, 0)),
+                              dist, closest_p1, closest_p2, normal);
   BOOST_CHECK(dist < 0);
   BOOST_CHECK_FALSE(res);
 
-  res = solver1.shapeDistance(s1, transform * Transform3f(Vec3f(40, 0, 0)), s2, transform, &dist);
+  res = solver1.shapeDistance(s1, transform * Transform3f(Vec3f(40, 0, 0)),
+                              s2, transform, dist,
+                              closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 10) < 0.1);
   BOOST_CHECK(res);
 
-  res = solver1.shapeDistance(s1, transform * Transform3f(Vec3f(30.1, 0, 0)), s2, transform, &dist);
+  res = solver1.shapeDistance(s1, transform * Transform3f(Vec3f(30.1, 0, 0)),
+                              s2, transform, dist,
+                              closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 0.1) < 0.1);
   BOOST_CHECK(res);
 
-  res = solver1.shapeDistance(s1, transform * Transform3f(Vec3f(29.9, 0, 0)), s2, transform, &dist);
+  res = solver1.shapeDistance(s1, transform * Transform3f(Vec3f(29.9, 0, 0)),
+                              s2, transform, dist,
+                              closest_p1, closest_p2, normal);
   BOOST_CHECK(dist < 0);
   BOOST_CHECK_FALSE(res);
 }
@@ -2437,7 +2480,7 @@ BOOST_AUTO_TEST_CASE(shapeDistance_boxbox)
 {
   Box s1(20, 40, 50);
   Box s2(10, 10, 10);
-  Vec3f closest_p1, closest_p2;
+  Vec3f closest_p1, closest_p2, normal;
 
   Transform3f transform;
   //generateRandomTransform(extents, transform);
@@ -2445,56 +2488,80 @@ BOOST_AUTO_TEST_CASE(shapeDistance_boxbox)
   bool res;
   FCL_REAL dist;
 
-  res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(), &dist);
+  res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(), dist,
+                              closest_p1, closest_p2, normal);
   BOOST_CHECK(dist <= 0);
   BOOST_CHECK_FALSE(res);
 
-  res = solver1.shapeDistance(s1, transform, s2, transform, &dist);
+  res = solver1.shapeDistance(s1, transform, s2, transform, dist,
+                              closest_p1, closest_p2, normal);
   BOOST_CHECK(dist <= 0);
   BOOST_CHECK_FALSE(res);
 
-  res = solver1.shapeDistance(s2, Transform3f(), s2, Transform3f(Vec3f(10.1, 0, 0)), &dist, &closest_p1, &closest_p2);
+  res = solver1.shapeDistance(s2, Transform3f(),
+                              s2, Transform3f(Vec3f(10.1, 0, 0)),
+                              dist, closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 0.1) < 0.001);
   BOOST_CHECK(res);
 
-  res = solver1.shapeDistance(s2, Transform3f(), s2, Transform3f(Vec3f(20.1, 0, 0)), &dist, &closest_p1, &closest_p2);
+  res = solver1.shapeDistance(s2, Transform3f(),
+                              s2, Transform3f(Vec3f(20.1, 0, 0)), dist,
+                              closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 10.1) < 0.001);
   BOOST_CHECK(res);
 
-  res = solver1.shapeDistance(s2, Transform3f(), s2, Transform3f(Vec3f(0, 20.2, 0)), &dist, &closest_p1, &closest_p2);
+  res = solver1.shapeDistance(s2, Transform3f(),
+                              s2, Transform3f(Vec3f(0, 20.2, 0)), dist,
+                              closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 10.2) < 0.001);
   BOOST_CHECK(res);
 
-  res = solver1.shapeDistance(s2, Transform3f(), s2, Transform3f(Vec3f(10.1, 10.1, 0)), &dist, &closest_p1, &closest_p2);
+  res = solver1.shapeDistance(s2, Transform3f(),
+                              s2, Transform3f(Vec3f(10.1, 10.1, 0)),
+                              dist, closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 0.1 * 1.414) < 0.001);
   BOOST_CHECK(res);
 
-  res = solver2.shapeDistance(s2, Transform3f(), s2, Transform3f(Vec3f(10.1, 0, 0)), &dist, &closest_p1, &closest_p2);
+  res = solver2.shapeDistance(s2, Transform3f(),
+                              s2, Transform3f(Vec3f(10.1, 0, 0)), dist,
+                              closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 0.1) < 0.001);
   BOOST_CHECK(res);
 
-  res = solver2.shapeDistance(s2, Transform3f(), s2, Transform3f(Vec3f(20.1, 0, 0)), &dist, &closest_p1, &closest_p2);
+  res = solver2.shapeDistance(s2, Transform3f(),
+                              s2, Transform3f(Vec3f(20.1, 0, 0)),
+                              dist, closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 10.1) < 0.001);
   BOOST_CHECK(res);
 
-  res = solver2.shapeDistance(s2, Transform3f(), s2, Transform3f(Vec3f(0, 20.1, 0)), &dist, &closest_p1, &closest_p2);
+  res = solver2.shapeDistance(s2, Transform3f(),
+                              s2, Transform3f(Vec3f(0, 20.1, 0)),dist,
+                              closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 10.1) < 0.001);
   BOOST_CHECK(res);
 
-  res = solver2.shapeDistance(s2, Transform3f(), s2, Transform3f(Vec3f(10.1, 10.1, 0)), &dist, &closest_p1, &closest_p2);
+  res = solver2.shapeDistance(s2, Transform3f(),
+                              s2, Transform3f(Vec3f(10.1, 10.1, 0)),
+                              dist, closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 0.1 * 1.414) < 0.001);
   BOOST_CHECK(res);
 
 
-  res = solver1.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(15.1, 0, 0)), &dist);
+  res = solver1.shapeDistance(s1, transform,
+                              s2, transform * Transform3f(Vec3f(15.1, 0, 0)),
+                              dist, closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 0.1) < 0.001);
   BOOST_CHECK(res);
 
-  res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(20, 0, 0)), &dist);
+  res = solver1.shapeDistance(s1, Transform3f(),
+                              s2, Transform3f(Vec3f(20, 0, 0)),
+                              dist, closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 5) < 0.001);
   BOOST_CHECK(res);
 
-  res = solver1.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(20, 0, 0)), &dist);
+  res = solver1.shapeDistance(s1, transform,
+                              s2, transform * Transform3f(Vec3f(20, 0, 0)),
+                              dist, closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 5) < 0.001);
   BOOST_CHECK(res);
 }
@@ -2503,6 +2570,7 @@ BOOST_AUTO_TEST_CASE(shapeDistance_boxsphere)
 {
   Sphere s1(20);
   Box s2(5, 5, 5);
+  Vec3f closest_p1, closest_p2, normal;
 
   Transform3f transform;
   generateRandomTransform(extents, transform);
@@ -2510,27 +2578,37 @@ BOOST_AUTO_TEST_CASE(shapeDistance_boxsphere)
   bool res;
   FCL_REAL dist;
 
-  res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(), &dist);
+  res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(),
+                              dist, closest_p1, closest_p2, normal);
   BOOST_CHECK(dist <= 0);
   BOOST_CHECK_FALSE(res);
 
-  res = solver1.shapeDistance(s1, transform, s2, transform, &dist);
+  res = solver1.shapeDistance(s1, transform, s2, transform, dist,
+                              closest_p1, closest_p2, normal);
   BOOST_CHECK(dist <= 0);
   BOOST_CHECK_FALSE(res);
 
-  res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(22.6, 0, 0)), &dist);
+  res = solver1.shapeDistance(s1, Transform3f(),
+                              s2, Transform3f(Vec3f(22.6, 0, 0)),
+                              dist, closest_p1, closest_p2, normal);
   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);
+  res = solver1.shapeDistance(s1, transform,
+                              s2, transform * Transform3f(Vec3f(22.6, 0, 0)),
+                              dist, closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 0.1) < 0.05);
   BOOST_CHECK(res);
 
-  res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), &dist);
+  res = solver1.shapeDistance(s1, Transform3f(),
+                              s2, Transform3f(Vec3f(40, 0, 0)),
+                              dist, closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 17.5) < 0.001);
   BOOST_CHECK(res);
 
-  res = solver1.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)), &dist);
+  res = solver1.shapeDistance(s1, transform,
+                              s2, transform * Transform3f(Vec3f(40, 0, 0)),
+                              dist, closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 17.5) < 0.001);
   BOOST_CHECK(res);
 }
@@ -2539,6 +2617,7 @@ BOOST_AUTO_TEST_CASE(shapeDistance_cylindercylinder)
 {
   Cylinder s1(5, 10);
   Cylinder s2(5, 10);
+  Vec3f closest_p1, closest_p2, normal;
 
   Transform3f transform;
   generateRandomTransform(extents, transform);
@@ -2546,27 +2625,37 @@ BOOST_AUTO_TEST_CASE(shapeDistance_cylindercylinder)
   bool res;
   FCL_REAL dist;
 
-  res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(), &dist);
+  res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(),
+                              dist, closest_p1, closest_p2, normal);
   BOOST_CHECK(dist <= 0);
   BOOST_CHECK_FALSE(res);
 
-  res = solver1.shapeDistance(s1, transform, s2, transform, &dist);
+  res = solver1.shapeDistance(s1, transform, s2, transform, dist,
+                              closest_p1, closest_p2, normal);
   BOOST_CHECK(dist <= 0);
   BOOST_CHECK_FALSE(res);
 
-  res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(10.1, 0, 0)), &dist);
+  res = solver1.shapeDistance(s1, Transform3f(),
+                              s2, Transform3f(Vec3f(10.1, 0, 0)),
+                              dist, closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 0.1) < 0.001);
   BOOST_CHECK(res);
 
-  res = solver1.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(10.1, 0, 0)), &dist);
+  res = solver1.shapeDistance(s1, transform,
+                              s2, transform * Transform3f(Vec3f(10.1, 0, 0)),
+                              dist, closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 0.1) < 0.001);
   BOOST_CHECK(res);
 
-  res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), &dist);
+  res = solver1.shapeDistance(s1, Transform3f(),
+                              s2, Transform3f(Vec3f(40, 0, 0)), dist,
+                              closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 30) < 0.001);
   BOOST_CHECK(res);
 
-  res = solver1.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)), &dist);
+  res = solver1.shapeDistance(s1, transform,
+                              s2, transform * Transform3f(Vec3f(40, 0, 0)),
+                              dist, closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 30) < 0.001);
   BOOST_CHECK(res);
 }
@@ -2577,6 +2666,7 @@ BOOST_AUTO_TEST_CASE(shapeDistance_conecone)
 {
   Cone s1(5, 10);
   Cone s2(5, 10);
+  Vec3f closest_p1, closest_p2, normal;
 
   Transform3f transform;
   generateRandomTransform(extents, transform);
@@ -2584,27 +2674,37 @@ BOOST_AUTO_TEST_CASE(shapeDistance_conecone)
   bool res;
   FCL_REAL dist;
 
-  res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(), &dist);
+  res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(), dist,
+                              closest_p1, closest_p2, normal);
   BOOST_CHECK(dist <= 0);
   BOOST_CHECK_FALSE(res);
 
-  res = solver1.shapeDistance(s1, transform, s2, transform, &dist);
+  res = solver1.shapeDistance(s1, transform, s2, transform, dist,
+                              closest_p1, closest_p2, normal);
   BOOST_CHECK(dist <= 0);
   BOOST_CHECK_FALSE(res);
 
-  res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(10.1, 0, 0)), &dist);
+  res = solver1.shapeDistance(s1, Transform3f(),
+                              s2, Transform3f(Vec3f(10.1, 0, 0)), dist,
+                              closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 0.1) < 0.001);
   BOOST_CHECK(res);
 
-  res = solver1.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(10.1, 0, 0)), &dist);
+  res = solver1.shapeDistance(s1, transform,
+                              s2, transform * Transform3f(Vec3f(10.1, 0, 0)),
+                              dist, closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 0.1) < 0.001);
   BOOST_CHECK(res);
 
-  res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(0, 0, 40)), &dist);
+  res = solver1.shapeDistance(s1, Transform3f(),
+                              s2, Transform3f(Vec3f(0, 0, 40)), dist,
+                              closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 30) < 1);
   BOOST_CHECK(res);
 
-  res = solver1.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(0, 0, 40)), &dist);
+  res = solver1.shapeDistance(s1, transform,
+                              s2, transform * Transform3f(Vec3f(0, 0, 40)),
+                              dist, closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 30) < 1);
   BOOST_CHECK(res);
 }
@@ -2613,6 +2713,7 @@ BOOST_AUTO_TEST_CASE(shapeDistance_conecylinder)
 {
   Cylinder s1(5, 10);
   Cone s2(5, 10);
+  Vec3f closest_p1, closest_p2, normal;
 
   Transform3f transform;
   generateRandomTransform(extents, transform);
@@ -2620,27 +2721,37 @@ BOOST_AUTO_TEST_CASE(shapeDistance_conecylinder)
   bool res;
   FCL_REAL dist;
 
-  res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(), &dist);
+  res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(), dist,
+                              closest_p1, closest_p2, normal);
   BOOST_CHECK(dist <= 0);
   BOOST_CHECK_FALSE(res);
 
-  res = solver1.shapeDistance(s1, transform, s2, transform, &dist);
+  res = solver1.shapeDistance(s1, transform, s2, transform, dist,
+                              closest_p1, closest_p2, normal);
   BOOST_CHECK(dist <= 0);
   BOOST_CHECK_FALSE(res);
 
-  res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(10.1, 0, 0)), &dist);
+  res = solver1.shapeDistance(s1, Transform3f(),
+                              s2, Transform3f(Vec3f(10.1, 0, 0)),
+                              dist, closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 0.1) < 0.01);
   BOOST_CHECK(res);
 
-  res = solver1.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(10.1, 0, 0)), &dist);
+  res = solver1.shapeDistance(s1, transform,
+                              s2, transform * Transform3f(Vec3f(10.1, 0, 0)),
+                              dist, closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 0.1) < 0.02);
   BOOST_CHECK(res);
 
-  res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), &dist);
+  res = solver1.shapeDistance(s1, Transform3f(),
+                              s2, Transform3f(Vec3f(40, 0, 0)),
+                              dist, closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 30) < 0.01);
   BOOST_CHECK(res);
 
-  res = solver1.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)), &dist);
+  res = solver1.shapeDistance(s1, transform,
+                              s2, transform * Transform3f(Vec3f(40, 0, 0)),
+                              dist, closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 30) < 0.1);
   BOOST_CHECK(res);
 }
@@ -2664,69 +2775,69 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spheresphere)
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(40, 0, 0));
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, false);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(40, 0, 0));
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, false);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(30, 0, 0));
   normal << 1, 0, 0;
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(30.01, 0, 0));
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, false);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(30.01, 0, 0));
   normal = transform.getRotation() * Vec3f(1, 0, 0);
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, false);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(29.9, 0, 0));
   normal << 1, 0, 0;
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(29.9, 0, 0));
   normal = transform.getRotation() * Vec3f(1, 0, 0);
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f();
   normal.setZero();  // If the centers of two sphere are at the same position, the normal is (0, 0, 0)
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
 
   tf1 = transform;
   tf2 = transform;
   normal.setZero();  // If the centers of two sphere are at the same position, the normal is (0, 0, 0)
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(-29.9, 0, 0));
   normal << -1, 0, 0;
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(-29.9, 0, 0));
   normal = transform.getRotation() * Vec3f(-1, 0, 0);
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(-30.0, 0, 0));
   normal << -1, 0, 0;
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(-30.01, 0, 0));
   normal << -1, 0, 0;
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, false);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(-30.01, 0, 0));
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, false);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);
 }
 
 BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_boxbox)
@@ -2751,32 +2862,32 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_boxbox)
   tf2 = Transform3f();
   // TODO: Need convention for normal when the centers of two objects are at same position. The current result is (1, 0, 0).
   normal << 1, 0, 0;
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
 
   tf1 = transform;
   tf2 = transform;
   // TODO: Need convention for normal when the centers of two objects are at same position. The current result is (1, 0, 0).
   normal = transform.getRotation() * Vec3f(1, 0, 0);
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(15, 0, 0));
   normal << 1, 0, 0;
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(15.01, 0, 0));
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, false);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);
 
   tf1 = Transform3f();
   tf2 = Transform3f(q);
   normal << 1, 0, 0;
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
 
   tf1 = transform;
   tf2 = transform * Transform3f(q);
   normal = transform.getRotation() * Vec3f(1, 0, 0);
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
 }
 
 BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spherebox)
@@ -2797,33 +2908,33 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spherebox)
   tf1 = Transform3f();
   tf2 = Transform3f();
   // TODO: Need convention for normal when the centers of two objects are at same position.
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
 
   tf1 = transform;
   tf2 = transform;
   // TODO: Need convention for normal when the centers of two objects are at same position.
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(22.5, 0, 0));
   normal << 1, 0, 0;
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal, false, 1e-7);  // built-in GJK solver requires larger tolerance than libccd
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal, false, 1e-7);  // built-in GJK solver requires larger tolerance than libccd
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(22.51, 0, 0));
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, false);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(22.4, 0, 0));
   normal << 1, 0, 0;
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal, false, 1e-2);  // built-in GJK solver requires larger tolerance than libccd
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal, false, 1e-2);  // built-in GJK solver requires larger tolerance than libccd
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(22.4, 0, 0));
   normal = transform.getRotation() * Vec3f(1, 0, 0);
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
   // built-in GJK solver returns incorrect normal.
-  // testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
+  // testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
 }
 
 BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spherecapsule)
@@ -2844,32 +2955,32 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spherecapsule)
   tf1 = Transform3f();
   tf2 = Transform3f();
   // TODO: Need convention for normal when the centers of two objects are at same position.
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
 
   tf1 = transform;
   tf2 = transform;
   // TODO: Need convention for normal when the centers of two objects are at same position.
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(24.9, 0, 0));
   normal << 1, 0, 0;
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(24.9, 0, 0));
   normal = transform.getRotation() * Vec3f(1, 0, 0);
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(25, 0, 0));
   normal << 1, 0, 0;
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(25.1, 0, 0));
   normal = transform.getRotation() * Vec3f(1, 0, 0);
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, false);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);
 }
 
 BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_cylindercylinder)
@@ -2890,33 +3001,33 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_cylindercylinder)
   tf1 = Transform3f();
   tf2 = Transform3f();
   // TODO: Need convention for normal when the centers of two objects are at same position.
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
 
   tf1 = transform;
   tf2 = transform;
   // TODO: Need convention for normal when the centers of two objects are at same position.
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(9.9, 0, 0));
   normal << 1, 0, 0;
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal, false, 3e-1);  // built-in GJK solver requires larger tolerance than libccd
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal, false, 3e-1);  // built-in GJK solver requires larger tolerance than libccd
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(9.9, 0, 0));
   normal = transform.getRotation() * Vec3f(1, 0, 0);
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true);
   // built-in GJK solver returns incorrect normal.
-  // testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
+  // testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(10, 0, 0));
   normal << 1, 0, 0;
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(10.01, 0, 0));
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, false);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);
 }
 
 BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_conecone)
@@ -2937,44 +3048,44 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_conecone)
   tf1 = Transform3f();
   tf2 = Transform3f();
   // TODO: Need convention for normal when the centers of two objects are at same position.
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
 
   tf1 = transform;
   tf2 = transform;
   // TODO: Need convention for normal when the centers of two objects are at same position.
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(9.9, 0, 0));
   normal << 1, 0, 0;
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal, false, 5.7e-1);  // built-in GJK solver requires larger tolerance than libccd
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal, false, 5.7e-1);  // built-in GJK solver requires larger tolerance than libccd
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(9.9, 0, 0));
   normal = transform.getRotation() * Vec3f(1, 0, 0);
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
   // built-in GJK solver returns incorrect normal.
-  // testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
+  // testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(10.1, 0, 0));
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, false);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(10.1, 0, 0));
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, false);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(0, 0, 9.9));
   normal << 0, 0, 1;
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(0, 0, 9.9));
   normal = transform.getRotation() * Vec3f(0, 0, 1);
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
   // built-in GJK solver returns incorrect normal.
-  // testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
+  // testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
 }
 
 BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_conecylinder)
@@ -2995,49 +3106,49 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_conecylinder)
   tf1 = Transform3f();
   tf2 = Transform3f();
   // TODO: Need convention for normal when the centers of two objects are at same position.
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
 
   tf1 = transform;
   tf2 = transform;
   // TODO: Need convention for normal when the centers of two objects are at same position.
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(9.9, 0, 0));
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(9.9, 0, 0));
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(10, 0, 0));
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(10, 0, 0));
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(0, 0, 9.9));
   normal << 0, 0, 1;
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(0, 0, 9.9));
   normal = transform.getRotation() * Vec3f(0, 0, 1);
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL);
   // built-in GJK solver returns incorrect normal.
-  // testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
+  // testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
 
   tf1 = Transform3f();
   tf2 = Transform3f(Vec3f(0, 0, 10));
   normal << 0, 0, 1;
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
 
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(0, 0, 10.1));
-  testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, false);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);
 }
 
 
@@ -3057,30 +3168,35 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spheretriangle)
   Vec3f normal;
   bool res;
 
-  res = solver2.shapeTriangleIntersect(s, Transform3f(), t[0], t[1], t[2],
-                                       Transform3f(), distance, c1, c2, normal);
+  res = solver2.shapeTriangleInteraction
+    (s, Transform3f(), t[0], t[1], t[2], Transform3f(), distance, c1, c2,
+     normal);
   BOOST_CHECK(res);
 
-  res =  solver2.shapeTriangleIntersect(s, transform, t[0], t[1], t[2], transform, distance, c1, c2, normal);
+  res =  solver2.shapeTriangleInteraction
+    (s, transform, t[0], t[1], t[2], transform, distance, c1, c2, normal);
   BOOST_CHECK(res);
 
   t[0] << 30, 0, 0;
   t[1] << 9.9, -20, 0;
   t[2] << 9.9, 20, 0;
-  res = solver2.shapeTriangleIntersect(s, Transform3f(), t[0], t[1], t[2],
-                                       Transform3f(), distance, c1, c2, normal);
+  res = solver2.shapeTriangleInteraction
+    (s, Transform3f(), t[0], t[1], t[2], Transform3f(), distance, c1, c2,
+     normal);
   BOOST_CHECK(res);
 
-  res =  solver2.shapeTriangleIntersect(s, transform, t[0], t[1], t[2], transform, distance, c1, c2, normal);
+  res =  solver2.shapeTriangleInteraction
+    (s, transform, t[0], t[1], t[2], transform, distance, c1, c2, normal);
   BOOST_CHECK(res);
 
-  res = solver2.shapeTriangleIntersect(s, Transform3f(), t[0], t[1], t[2],
-                                       Transform3f(), distance, c1, c2, normal);
+  res = solver2.shapeTriangleInteraction
+    (s, Transform3f(), t[0], t[1], t[2], Transform3f(), distance, c1, c2,
+     normal);
   BOOST_CHECK(res);
   BOOST_CHECK(isEqual(normal, Vec3f(1, 0, 0), 1e-9));
 
-  res =  solver2.shapeTriangleIntersect(s, transform, t[0], t[1], t[2],
-                                        transform, distance, c1, c2, normal);
+  res =  solver2.shapeTriangleInteraction
+    (s, transform, t[0], t[1], t[2], transform, distance, c1, c2, normal);
   BOOST_CHECK(res);
   BOOST_CHECK(isEqual(normal, transform.getRotation() * Vec3f(1, 0, 0), 1e-9));
 }
@@ -3101,30 +3217,36 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_halfspacetriangle)
   Vec3f normal;
   bool res;
 
-  res = solver2.shapeTriangleIntersect(hs, Transform3f(), t[0], t[1], t[2], Transform3f(), distance, c1, c2, normal);
+  res = solver2.shapeTriangleInteraction
+    (hs, Transform3f(), t[0], t[1], t[2], Transform3f(), distance, c1, c2,
+     normal);
   BOOST_CHECK(res);
 
-  res =  solver2.shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, distance, c1, c2, normal);
+  res = solver2.shapeTriangleInteraction
+    (hs, transform, t[0], t[1], t[2], transform, distance, c1, c2, normal);
   BOOST_CHECK(res);
 
 
   t[0] << 20, 0, 0;
   t[1] << 0, -20, 0;
   t[2] << 0, 20, 0;
-  res = solver2.shapeTriangleIntersect(hs, Transform3f(), t[0], t[1], t[2], Transform3f(), distance, c1, c2, normal);
+  res = solver2.shapeTriangleInteraction
+    (hs, Transform3f(), t[0], t[1], t[2], Transform3f(), distance, c1, c2,
+     normal);
   BOOST_CHECK(res);
 
-  res =  solver2.shapeTriangleIntersect(hs, transform, t[0], t[1], t[2],
-                                        transform, distance, c1, c2, normal);
+  res =  solver2.shapeTriangleInteraction
+    (hs, transform, t[0], t[1], t[2], transform, distance, c1, c2, normal);
   BOOST_CHECK(res);
 
-  res = solver2.shapeTriangleIntersect(hs, Transform3f(), t[0], t[1], t[2],
-                                       Transform3f(), distance, c1, c2, normal);
+  res = solver2.shapeTriangleInteraction
+    (hs, Transform3f(), t[0], t[1], t[2], Transform3f(), distance, c1, c2,
+     normal);
   BOOST_CHECK(res);
   BOOST_CHECK(isEqual(normal, Vec3f(1, 0, 0), 1e-9));
 
-  res =  solver2.shapeTriangleIntersect(hs, transform, t[0], t[1], t[2],
-                                        transform, distance, c1, c2, normal);
+  res =  solver2.shapeTriangleInteraction
+    (hs, transform, t[0], t[1], t[2], transform, distance, c1, c2, normal);
   BOOST_CHECK(res);
   BOOST_CHECK(isEqual(normal, transform.getRotation() * Vec3f(1, 0, 0), 1e-9));
 }
@@ -3145,29 +3267,36 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_planetriangle)
   Vec3f normal;
   bool res;
 
-  res = solver1.shapeTriangleIntersect(hs, Transform3f(), t[0], t[1], t[2], Transform3f(), distance, c1, c2, normal);
+  res = solver1.shapeTriangleInteraction
+    (hs, Transform3f(), t[0], t[1], t[2], Transform3f(), distance, c1, c2,
+     normal);
   BOOST_CHECK(res);
 
-  res =  solver1.shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, distance, c1, c2, normal);
+  res =  solver1.shapeTriangleInteraction
+    (hs, transform, t[0], t[1], t[2], transform, distance, c1, c2, normal);
   BOOST_CHECK(res);
 
 
   t[0] << 20, 0, 0;
   t[1] << -0.1, -20, 0;
   t[2] << -0.1, 20, 0;
-  res = solver2.shapeTriangleIntersect(hs, Transform3f(), t[0], t[1], t[2], Transform3f(), distance, c1, c2, normal);
+  res = solver2.shapeTriangleInteraction
+    (hs, Transform3f(), t[0], t[1], t[2], Transform3f(), distance, c1, c2,
+     normal);
   BOOST_CHECK(res);
 
-  res =  solver2.shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, distance, c1, c2, normal);
+  res =  solver2.shapeTriangleInteraction
+    (hs, transform, t[0], t[1], t[2], transform, distance, c1, c2, normal);
   BOOST_CHECK(res);
 
-  res = solver2.shapeTriangleIntersect(hs, Transform3f(), t[0], t[1], t[2],
-                                       Transform3f(), distance, c1, c2, normal);
+  res = solver2.shapeTriangleInteraction
+    (hs, Transform3f(), t[0], t[1], t[2], Transform3f(), distance, c1, c2,
+     normal);
   BOOST_CHECK(res);
   BOOST_CHECK(isEqual(normal, Vec3f(1, 0, 0), 1e-9));
 
-  res =  solver2.shapeTriangleIntersect(hs, transform, t[0], t[1], t[2],
-                                        transform, distance, c1, c2, normal);
+  res =  solver2.shapeTriangleInteraction
+    (hs, transform, t[0], t[1], t[2], transform, distance, c1, c2, normal);
   BOOST_CHECK(res);
   BOOST_CHECK(isEqual(normal, transform.getRotation() * Vec3f(1, 0, 0), 1e-9));
 }
@@ -3179,6 +3308,7 @@ BOOST_AUTO_TEST_CASE(spheresphere)
 {
   Sphere s1(20);
   Sphere s2(10);
+  Vec3f closest_p1, closest_p2, normal;
 
   Transform3f transform;
   generateRandomTransform(extents, transform);
@@ -3186,52 +3316,76 @@ BOOST_AUTO_TEST_CASE(spheresphere)
   bool res;
   FCL_REAL dist = -1;
 
-  res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), &dist);
+  res = solver2.shapeDistance(s1, Transform3f(),
+                              s2, Transform3f(Vec3f(40, 0, 0)),
+                              dist, closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 10) < 0.001);
   BOOST_CHECK(res);
 
-  res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(30.1, 0, 0)), &dist);
+  res = solver2.shapeDistance(s1, Transform3f(),
+                              s2, Transform3f(Vec3f(30.1, 0, 0)),
+                              dist, closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 0.1) < 0.001);
   BOOST_CHECK(res);
 
-  res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(29.9, 0, 0)), &dist);
+  res = solver2.shapeDistance(s1, Transform3f(),
+                              s2, Transform3f(Vec3f(29.9, 0, 0)),
+                              dist, closest_p1, closest_p2, normal);
   BOOST_CHECK(dist <= 0);
   BOOST_CHECK_FALSE(res);
 
-  res = solver2.shapeDistance(s1, Transform3f(Vec3f(40, 0, 0)), s2, Transform3f(), &dist);
+  res = solver2.shapeDistance(s1, Transform3f(Vec3f(40, 0, 0)),
+                              s2, Transform3f(), dist, closest_p1,
+                              closest_p2, normal);
   BOOST_CHECK(fabs(dist - 10) < 0.001);
   BOOST_CHECK(res);
 
-  res = solver2.shapeDistance(s1, Transform3f(Vec3f(30.1, 0, 0)), s2, Transform3f(), &dist);
+  res = solver2.shapeDistance(s1, Transform3f(Vec3f(30.1, 0, 0)),
+                              s2, Transform3f(), dist, closest_p1,
+                              closest_p2, normal);
   BOOST_CHECK(fabs(dist - 0.1) < 0.001);
   BOOST_CHECK(res);
 
-  res = solver2.shapeDistance(s1, Transform3f(Vec3f(29.9, 0, 0)), s2, Transform3f(), &dist);
+  res = solver2.shapeDistance(s1, Transform3f(Vec3f(29.9, 0, 0)),
+                              s2, Transform3f(), dist, closest_p1, closest_p2,
+                              normal);
   BOOST_CHECK(dist <= 0);
   BOOST_CHECK_FALSE(res);
 
 
-  res = solver2.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)), &dist);
+  res = solver2.shapeDistance(s1, transform,
+                              s2, transform * Transform3f(Vec3f(40, 0, 0)),
+                              dist, closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 10) < 0.001);
   BOOST_CHECK(res);
 
-  res = solver2.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(30.1, 0, 0)), &dist);
+  res = solver2.shapeDistance(s1, transform,
+                              s2, transform * Transform3f(Vec3f(30.1, 0, 0)),
+                              dist, closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 0.1) < 0.001);
   BOOST_CHECK(res);
 
-  res = solver2.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(29.9, 0, 0)), &dist);
+  res = solver2.shapeDistance(s1, transform,
+                              s2, transform * Transform3f(Vec3f(29.9, 0, 0)),
+                              dist, closest_p1, closest_p2, normal);
   BOOST_CHECK(dist <= 0);
   BOOST_CHECK_FALSE(res);
 
-  res = solver2.shapeDistance(s1, transform * Transform3f(Vec3f(40, 0, 0)), s2, transform, &dist);
+  res = solver2.shapeDistance(s1, transform * Transform3f(Vec3f(40, 0, 0)),
+                              s2, transform, dist, closest_p1, closest_p2,
+                              normal);
   BOOST_CHECK(fabs(dist - 10) < 0.001);
   BOOST_CHECK(res);
 
-  res = solver2.shapeDistance(s1, transform * Transform3f(Vec3f(30.1, 0, 0)), s2, transform, &dist);
+  res = solver2.shapeDistance(s1, transform * Transform3f(Vec3f(30.1, 0, 0)),
+                              s2, transform, dist, closest_p1, closest_p2,
+                              normal);
   BOOST_CHECK(fabs(dist - 0.1) < 0.001);
   BOOST_CHECK(res);
 
-  res = solver2.shapeDistance(s1, transform * Transform3f(Vec3f(29.9, 0, 0)), s2, transform, &dist);
+  res = solver2.shapeDistance(s1, transform * Transform3f(Vec3f(29.9, 0, 0)),
+                              s2, transform, dist, closest_p1, closest_p2,
+                              normal);
   BOOST_CHECK(dist <= 0);
   BOOST_CHECK_FALSE(res);
 }
@@ -3240,6 +3394,7 @@ BOOST_AUTO_TEST_CASE(boxbox)
 {
   Box s1(20, 40, 50);
   Box s2(10, 10, 10);
+  Vec3f closest_p1, closest_p2, normal;
 
   Transform3f transform;
   generateRandomTransform(extents, transform);
@@ -3247,27 +3402,37 @@ BOOST_AUTO_TEST_CASE(boxbox)
   bool res;
   FCL_REAL dist;
 
-  res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(), &dist);
+  res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(),
+                              dist, closest_p1, closest_p2, normal);
   BOOST_CHECK(dist <= 0);
   BOOST_CHECK_FALSE(res);
 
-  res = solver2.shapeDistance(s1, transform, s2, transform, &dist);
+  res = solver2.shapeDistance(s1, transform, s2, transform,
+                              dist, closest_p1, closest_p2, normal);
   BOOST_CHECK(dist <= 0);
   BOOST_CHECK_FALSE(res);
 
-  res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(15.1, 0, 0)), &dist);
+  res = solver2.shapeDistance(s1, Transform3f(),
+                              s2, Transform3f(Vec3f(15.1, 0, 0)),
+                              dist, closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 0.1) < 0.001);
   BOOST_CHECK(res);
 
-  res = solver2.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(15.1, 0, 0)), &dist);
+  res = solver2.shapeDistance(s1, transform,
+                              s2, transform * Transform3f(Vec3f(15.1, 0, 0)),
+                              dist, closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 0.1) < 0.001);
   BOOST_CHECK(res);
 
-  res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(20, 0, 0)), &dist);
+  res = solver2.shapeDistance(s1, Transform3f(),
+                              s2, Transform3f(Vec3f(20, 0, 0)),
+                              dist, closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 5) < 0.001);
   BOOST_CHECK(res);
 
-  res = solver2.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(20, 0, 0)), &dist);
+  res = solver2.shapeDistance(s1, transform,
+                              s2, transform * Transform3f(Vec3f(20, 0, 0)),
+                              dist, closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 5) < 0.001);
   BOOST_CHECK(res);
 }
@@ -3276,6 +3441,7 @@ BOOST_AUTO_TEST_CASE(boxsphere)
 {
   Sphere s1(20);
   Box s2(5, 5, 5);
+  Vec3f closest_p1, closest_p2, normal;
 
   Transform3f transform;
   generateRandomTransform(extents, transform);
@@ -3283,27 +3449,37 @@ BOOST_AUTO_TEST_CASE(boxsphere)
   bool res;
   FCL_REAL dist;
 
-  res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(), &dist);
+  res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(),
+                              dist, closest_p1, closest_p2, normal);
   BOOST_CHECK(dist <= 0);
   BOOST_CHECK_FALSE(res);
 
-  res = solver2.shapeDistance(s1, transform, s2, transform, &dist);
+  res = solver2.shapeDistance(s1, transform, s2, transform,
+                              dist, closest_p1, closest_p2, normal);
   BOOST_CHECK(dist <= 0);
   BOOST_CHECK_FALSE(res);
 
-  res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(22.6, 0, 0)), &dist);
+  res = solver2.shapeDistance(s1, Transform3f(),
+                              s2, Transform3f(Vec3f(22.6, 0, 0)),
+                              dist, closest_p1, closest_p2, normal);
   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);
+  res = solver2.shapeDistance(s1, transform, s2,
+                              transform * Transform3f(Vec3f(22.6, 0, 0)),
+                              dist, closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 0.1) < 0.01);
   BOOST_CHECK(res);
 
-  res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), &dist);
+  res = solver2.shapeDistance(s1, Transform3f(),
+                              s2, Transform3f(Vec3f(40, 0, 0)),
+                              dist, closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 17.5) < 0.001);
   BOOST_CHECK(res);
 
-  res = solver2.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)), &dist);
+  res = solver2.shapeDistance(s1, transform,
+                              s2, transform * Transform3f(Vec3f(40, 0, 0)),
+                              dist, closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 17.5) < 0.001);
   BOOST_CHECK(res);
 }
@@ -3312,6 +3488,7 @@ BOOST_AUTO_TEST_CASE(cylindercylinder)
 {
   Cylinder s1(5, 10);
   Cylinder s2(5, 10);
+  Vec3f closest_p1, closest_p2, normal;
 
   Transform3f transform;
   generateRandomTransform(extents, transform);
@@ -3319,27 +3496,37 @@ BOOST_AUTO_TEST_CASE(cylindercylinder)
   bool res;
   FCL_REAL dist;
 
-  res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(), &dist);
+  res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(),
+                              dist, closest_p1, closest_p2, normal);
   BOOST_CHECK(dist <= 0);
   BOOST_CHECK_FALSE(res);
 
-  res = solver2.shapeDistance(s1, transform, s2, transform, &dist);
+  res = solver2.shapeDistance(s1, transform, s2, transform,
+                              dist, closest_p1, closest_p2, normal);
   BOOST_CHECK(dist <= 0);
   BOOST_CHECK_FALSE(res);
 
-  res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(10.1, 0, 0)), &dist);
+  res = solver2.shapeDistance(s1, Transform3f(),
+                              s2, Transform3f(Vec3f(10.1, 0, 0)),
+                              dist, closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 0.1) < 0.001);
   BOOST_CHECK(res);
 
-  res = solver2.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(10.1, 0, 0)), &dist);
+  res = solver2.shapeDistance(s1, transform,
+                              s2, transform * Transform3f(Vec3f(10.1, 0, 0)),
+                              dist, closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 0.1) < 0.001);
   BOOST_CHECK(res);
 
-  res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), &dist);
+  res = solver2.shapeDistance(s1, Transform3f(),
+                              s2, Transform3f(Vec3f(40, 0, 0)),
+                              dist, closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 30) < 0.001);
   BOOST_CHECK(res);
 
-  res = solver2.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)), &dist);
+  res = solver2.shapeDistance(s1, transform,
+                              s2, transform * Transform3f(Vec3f(40, 0, 0)),
+                              dist, closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 30) < 0.001);
   BOOST_CHECK(res);
 }
@@ -3350,6 +3537,7 @@ BOOST_AUTO_TEST_CASE(conecone)
 {
   Cone s1(5, 10);
   Cone s2(5, 10);
+  Vec3f closest_p1, closest_p2, normal;
 
   Transform3f transform;
   generateRandomTransform(extents, transform);
@@ -3357,27 +3545,37 @@ BOOST_AUTO_TEST_CASE(conecone)
   bool res;
   FCL_REAL dist;
 
-  res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(), &dist);
+  res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(),
+                              dist, closest_p1, closest_p2, normal);
   BOOST_CHECK(dist <= 0);
   BOOST_CHECK_FALSE(res);
 
-  res = solver2.shapeDistance(s1, transform, s2, transform, &dist);
+  res = solver2.shapeDistance(s1, transform, s2, transform,
+                              dist, closest_p1, closest_p2, normal);
   BOOST_CHECK(dist <= 0);
   BOOST_CHECK_FALSE(res);
 
-  res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(10.1, 0, 0)), &dist);
+  res = solver2.shapeDistance(s1, Transform3f(),
+                              s2, Transform3f(Vec3f(10.1, 0, 0)),
+                              dist, closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 0.1) < 0.001);
   BOOST_CHECK(res);
 
-  res = solver2.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(10.1, 0, 0)), &dist);
+  res = solver2.shapeDistance(s1, transform,
+                              s2, transform * Transform3f(Vec3f(10.1, 0, 0)),
+                              dist, closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 0.1) < 0.001);
   BOOST_CHECK(res);
 
-  res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(0, 0, 40)), &dist);
+  res = solver2.shapeDistance(s1, Transform3f(),
+                              s2, Transform3f(Vec3f(0, 0, 40)),
+                              dist, closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 30) < 0.001);
   BOOST_CHECK(res);
 
-  res = solver2.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(0, 0, 40)), &dist);
+  res = solver2.shapeDistance(s1, transform,
+                              s2, transform * Transform3f(Vec3f(0, 0, 40)),
+                              dist, closest_p1, closest_p2, normal);
   BOOST_CHECK(fabs(dist - 30) < 0.001);
   BOOST_CHECK(res);
 }
@@ -3485,29 +3683,32 @@ void testReversibleShapeDistance(const S1& s1, const S2& s2, FCL_REAL distance)
   Vec3f p1B;
   Vec3f p2A;
   Vec3f p2B;
+  Vec3f normalA, normalB;
 
   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);
+  resA = solver1.shapeDistance(s1, tf1, s2, tf2, distA, p1A, p2A, normalA);
+  resB = solver1.shapeDistance(s2, tf2, s1, tf1, distB, p1B, p2B, normalB);
 
   BOOST_CHECK(resA);
   BOOST_CHECK(resB);
-  BOOST_CHECK_CLOSE(distA, distB, tol);  // distances should be same
-  BOOST_CHECK(isEqual(p1A, p2B, tol));  // closest points should in reverse order
+  BOOST_CHECK_CLOSE(distA, distB, tol); // distances should be same
+  BOOST_CHECK(isEqual(p1A, p2B, tol)); // closest points should in reverse order
   BOOST_CHECK(isEqual(p2A, p1B, tol));
+  BOOST_CHECK((normalB + normalA).norm () < tol);
 
-  resA = solver2.shapeDistance(s1, tf1, s2, tf2, &distA, &p1A, &p2A);
-  resB = solver2.shapeDistance(s2, tf2, s1, tf1, &distB, &p1B, &p2B);
+  resA = solver2.shapeDistance(s1, tf1, s2, tf2, distA, p1A, p2A, normalA);
+  resB = solver2.shapeDistance(s2, tf2, s1, tf1, distB, p1B, p2B, normalB);
 
   BOOST_CHECK(resA);
   BOOST_CHECK(resB);
   BOOST_CHECK_CLOSE(distA, distB, tol);
   BOOST_CHECK(isEqual(p1A, p2B, tol));
   BOOST_CHECK(isEqual(p2A, p1B, tol));
+  BOOST_CHECK((normalB + normalA).norm () < tol);
 }
 
 BOOST_AUTO_TEST_CASE(reversibleShapeDistance_allshapes)
diff --git a/test/test_fcl_gjk.cpp b/test/test_fcl_gjk.cpp
index 6d258718..e80ef5e4 100644
--- a/test/test_fcl_gjk.cpp
+++ b/test/test_fcl_gjk.cpp
@@ -85,14 +85,15 @@ BOOST_AUTO_TEST_CASE(distance_triangle_triangle_1)
     Vec3f Q1 (Vec3f::Random ()), Q2 (Vec3f::Random ()), Q3 (Vec3f::Random ());
     TriangleP tri1 (P1, P2, P3);
     TriangleP tri2 (Q1, Q2, Q3);
+    Vec3f normal;
 
     bool res;
     start = clock ();
-    res = solver.shapeDistance (tri1, tf1, tri2, tf2, &distance, &p1, &p2);
+    res = solver.shapeDistance (tri1, tf1, tri2, tf2, distance, p1, p2, normal);
     end = clock ();
     results [i].timeGjk = end - start;
     results [i].collision = !res;
-    BOOST_CHECK (res == (distance > 0));
+    assert (res == (distance > 0));
     if (!res) {
       ++nCol; distance = 0;
     }
-- 
GitLab