diff --git a/include/fcl/BV/kIOS.h b/include/fcl/BV/kIOS.h
index 091be68ea..1a168bcaf 100644
--- a/include/fcl/BV/kIOS.h
+++ b/include/fcl/BV/kIOS.h
@@ -179,7 +179,7 @@ Scalar distance(
 //==============================================================================
 template <typename Scalar>
 typename kIOS<Scalar>::kIOS_Sphere kIOS<Scalar>::encloseSphere(
-    const kIOS<Scalar>::kIOS_Sphere& s0, const kIOS<Scalar>::kIOS_Sphere& s1)
+    const typename kIOS<Scalar>::kIOS_Sphere& s0, const typename kIOS<Scalar>::kIOS_Sphere& s1)
 {
   Vector3<Scalar> d = s1.o - s0.o;
   Scalar dist2 = d.squaredNorm();
diff --git a/include/fcl/narrowphase/gjk_solver_libccd.h b/include/fcl/narrowphase/gjk_solver_libccd.h
index 4bbb2ba47..e2c642dce 100755
--- a/include/fcl/narrowphase/gjk_solver_libccd.h
+++ b/include/fcl/narrowphase/gjk_solver_libccd.h
@@ -953,7 +953,7 @@ void GJKSolver_libccd<Scalar>::enableCachedGuess(bool if_enable) const
 //==============================================================================
 template<typename Scalar>
 void GJKSolver_libccd<Scalar>::setCachedGuess(
-    const Vector3<GJKSolver_libccd<Scalar>::Scalar>& guess) const
+    const Vector3<Scalar>& guess) const
 {
   // TODO: need change libccd to exploit spatial coherence
 }
diff --git a/include/fcl/shape/sphere.h b/include/fcl/shape/sphere.h
index 2582b726e..7d5afb1c6 100644
--- a/include/fcl/shape/sphere.h
+++ b/include/fcl/shape/sphere.h
@@ -108,7 +108,7 @@ NODE_TYPE Sphere<ScalarT>::getNodeType() const
 template <typename ScalarT>
 Matrix3<ScalarT> Sphere<ScalarT>::computeMomentofInertia() const
 {
-  ScalarT I = 0.4 * radius * radius * computeVolume();
+  ScalarT I = (ScalarT)0.4 * radius * radius * computeVolume();
 
   return Vector3<ScalarT>::Constant(I).asDiagonal();
 }
@@ -117,7 +117,7 @@ Matrix3<ScalarT> Sphere<ScalarT>::computeMomentofInertia() const
 template <typename ScalarT>
 ScalarT Sphere<ScalarT>::computeVolume() const
 {
-  return 4.0 * constants<Scalar>::pi() * radius * radius * radius / 3.0;
+  return (ScalarT)4.0 * constants<ScalarT>::pi() * radius * radius * radius / (ScalarT)3.0;
 }
 
 //==============================================================================
diff --git a/include/fcl/traversal/distance/mesh_conservative_advancement_traversal_node.h b/include/fcl/traversal/distance/mesh_conservative_advancement_traversal_node.h
index 4e2c06c11..9de87f883 100644
--- a/include/fcl/traversal/distance/mesh_conservative_advancement_traversal_node.h
+++ b/include/fcl/traversal/distance/mesh_conservative_advancement_traversal_node.h
@@ -111,7 +111,21 @@ class MeshConservativeAdvancementTraversalNodeRSS
 public:
   MeshConservativeAdvancementTraversalNodeRSS(Scalar w_ = 1);
 
-  Scalar BVTesting(int b1, int b2) const;
+  Scalar BVTesting(int b1, int b2) const
+  {
+    if (this->enable_statistics)
+      this->num_bv_tests++;
+
+    Vector3<Scalar> P1, P2;
+    Scalar d = distance(
+        tf,
+		this->model1->getBV(b1).bv,
+		this->model2->getBV(b2).bv, &P1, &P2);
+
+    this->stack.emplace_back(P1, P2, b1, b2, d);
+
+    return d;
+  }
 
   void leafTesting(int b1, int b2) const;
 
@@ -143,7 +157,21 @@ class MeshConservativeAdvancementTraversalNodeOBBRSS
 public:
   MeshConservativeAdvancementTraversalNodeOBBRSS(Scalar w_ = 1);
 
-  Scalar BVTesting(int b1, int b2) const;
+  Scalar BVTesting(int b1, int b2) const
+  {
+    if (this->enable_statistics)
+      this->num_bv_tests++;
+
+    Vector3<Scalar> P1, P2;
+    Scalar d = distance(
+        tf,
+        this->model1->getBV(b1).bv,
+        this->model2->getBV(b2).bv, &P1, &P2);
+
+    this->stack.emplace_back(P1, P2, b1, b2, d);
+
+    return d;
+  }
 
   void leafTesting(int b1, int b2) const;
 
@@ -234,8 +262,7 @@ void meshConservativeAdvancementOrientedNodeLeafTesting(
 //==============================================================================
 template <typename BV>
 MeshConservativeAdvancementTraversalNode<BV>::
-MeshConservativeAdvancementTraversalNode(
-    MeshConservativeAdvancementTraversalNode<BV>::Scalar w_)
+MeshConservativeAdvancementTraversalNode(typename BV::Scalar w_)
   : MeshDistanceTraversalNode<BV>()
 {
   delta_t = 1;
@@ -250,7 +277,7 @@ MeshConservativeAdvancementTraversalNode(
 
 //==============================================================================
 template <typename BV>
-typename MeshConservativeAdvancementTraversalNode<BV>::Scalar
+typename BV::Scalar
 MeshConservativeAdvancementTraversalNode<BV>::BVTesting(int b1, int b2) const
 {
   if(this->enable_statistics) this->num_bv_tests++;
@@ -387,7 +414,7 @@ struct CanStopImpl
 //==============================================================================
 template <typename BV>
 bool MeshConservativeAdvancementTraversalNode<BV>::canStop(
-    MeshConservativeAdvancementTraversalNode<BV>::Scalar c) const
+    typename BV::Scalar c) const
 {
   return CanStopImpl<typename BV::Scalar, BV>::run(*this, c);
 }
@@ -521,25 +548,6 @@ MeshConservativeAdvancementTraversalNodeRSS<Scalar>::MeshConservativeAdvancement
   tf.linear().setIdentity();
 }
 
-//==============================================================================
-template <typename Scalar>
-Scalar MeshConservativeAdvancementTraversalNodeRSS<Scalar>::BVTesting(int b1, int b2) const
-{
-  if(this->enable_statistics)
-    this->num_bv_tests++;
-
-  Vector3<Scalar> P1, P2;
-  Scalar d = distance(
-        tf.linear(),
-        tf.translation(),
-        this->model1->getBV(b1).bv,
-        this->model2->getBV(b2).bv, &P1, &P2);
-
-  this->stack.emplace_back(P1, P2, b1, b2, d);
-
-  return d;
-}
-
 //==============================================================================
 template <typename Scalar>
 void MeshConservativeAdvancementTraversalNodeRSS<Scalar>::leafTesting(int b1, int b2) const
@@ -594,26 +602,6 @@ MeshConservativeAdvancementTraversalNodeOBBRSS(Scalar w_)
   tf.linear().setIdentity();
 }
 
-//==============================================================================
-template <typename Scalar>
-Scalar MeshConservativeAdvancementTraversalNodeOBBRSS<Scalar>::
-BVTesting(int b1, int b2) const
-{
-  if(this->enable_statistics)
-    this->num_bv_tests++;
-
-  Vector3<Scalar> P1, P2;
-  Scalar d = distance(
-        tf.linear(),
-        tf.translation(),
-        this->model1->getBV(b1).bv,
-        this->model2->getBV(b2).bv, &P1, &P2);
-
-  this->stack.emplace_back(P1, P2, b1, b2, d);
-
-  return d;
-}
-
 //==============================================================================
 template <typename Scalar>
 void MeshConservativeAdvancementTraversalNodeOBBRSS<Scalar>::
diff --git a/include/fcl/traversal/distance/mesh_distance_traversal_node.h b/include/fcl/traversal/distance/mesh_distance_traversal_node.h
index aac46453f..c0b9c4eca 100644
--- a/include/fcl/traversal/distance/mesh_distance_traversal_node.h
+++ b/include/fcl/traversal/distance/mesh_distance_traversal_node.h
@@ -100,7 +100,12 @@ class MeshDistanceTraversalNodeRSS
 
   void postprocess();
 
-  Scalar BVTesting(int b1, int b2) const;
+  Scalar BVTesting(int b1, int b2) const
+  {
+    if (this->enable_statistics) this->num_bv_tests++;
+
+    return distance(tf, this->model1->getBV(b1).bv, this->model2->getBV(b2).bv);
+  }
 
   void leafTesting(int b1, int b2) const;
 
@@ -135,7 +140,12 @@ class MeshDistanceTraversalNodekIOS
   
   void postprocess();
 
-  Scalar BVTesting(int b1, int b2) const;
+  Scalar BVTesting(int b1, int b2) const
+  {
+    if (this->enable_statistics) this->num_bv_tests++;
+
+    return distance(tf, this->model1->getBV(b1).bv, this->model2->getBV(b2).bv);
+  }
 
   void leafTesting(int b1, int b2) const;
 
@@ -170,7 +180,12 @@ class MeshDistanceTraversalNodeOBBRSS
 
   void postprocess();
 
-  Scalar BVTesting(int b1, int b2) const;
+  Scalar BVTesting(int b1, int b2) const
+  {
+    if (this->enable_statistics) this->num_bv_tests++;
+
+    return distance(tf, this->model1->getBV(b1).bv, this->model2->getBV(b2).bv);
+  }
 
   void leafTesting(int b1, int b2) const;
 
@@ -330,7 +345,7 @@ void MeshDistanceTraversalNode<BV>::leafTesting(int b1, int b2) const
 
 //==============================================================================
 template <typename BV>
-bool MeshDistanceTraversalNode<BV>::canStop(MeshDistanceTraversalNode<BV>::Scalar c) const
+bool MeshDistanceTraversalNode<BV>::canStop(typename BV::Scalar c) const
 {
   if((c >= this->result->min_distance - abs_err) && (c * (1 + rel_err) >= this->result->min_distance))
     return true;
@@ -444,15 +459,6 @@ void MeshDistanceTraversalNodeRSS<Scalar>::postprocess()
         *this->result);
 }
 
-//==============================================================================
-template <typename Scalar>
-Scalar MeshDistanceTraversalNodeRSS<Scalar>::BVTesting(int b1, int b2) const
-{
-  if(this->enable_statistics) this->num_bv_tests++;
-
-  return distance(tf, this->model1->getBV(b1).bv, this->model2->getBV(b2).bv);
-}
-
 //==============================================================================
 template <typename Scalar>
 void MeshDistanceTraversalNodeRSS<Scalar>::leafTesting(int b1, int b2) const
@@ -512,15 +518,6 @@ void MeshDistanceTraversalNodekIOS<Scalar>::postprocess()
         *this->result);
 }
 
-//==============================================================================
-template <typename Scalar>
-Scalar MeshDistanceTraversalNodekIOS<Scalar>::BVTesting(int b1, int b2) const
-{
-  if(this->enable_statistics) this->num_bv_tests++;
-
-  return distance(tf, this->model1->getBV(b1).bv, this->model2->getBV(b2).bv);
-}
-
 //==============================================================================
 template <typename Scalar>
 void MeshDistanceTraversalNodekIOS<Scalar>::leafTesting(int b1, int b2) const
@@ -580,15 +577,6 @@ void MeshDistanceTraversalNodeOBBRSS<Scalar>::postprocess()
         *this->result);
 }
 
-//==============================================================================
-template <typename Scalar>
-Scalar MeshDistanceTraversalNodeOBBRSS<Scalar>::BVTesting(int b1, int b2) const
-{
-  if(this->enable_statistics) this->num_bv_tests++;
-
-  return distance(tf, this->model1->getBV(b1).bv, this->model2->getBV(b2).bv);
-}
-
 //==============================================================================
 template <typename Scalar>
 void MeshDistanceTraversalNodeOBBRSS<Scalar>::leafTesting(int b1, int b2) const
diff --git a/include/fcl/traversal/distance/shape_mesh_conservative_advancement_traversal_node.h b/include/fcl/traversal/distance/shape_mesh_conservative_advancement_traversal_node.h
index f3bdbd2b0..f80f5397c 100644
--- a/include/fcl/traversal/distance/shape_mesh_conservative_advancement_traversal_node.h
+++ b/include/fcl/traversal/distance/shape_mesh_conservative_advancement_traversal_node.h
@@ -477,7 +477,7 @@ leafTesting(int b1, int b2) const
 //==============================================================================
 template <typename S, typename NarrowPhaseSolver>
 bool ShapeMeshConservativeAdvancementTraversalNodeOBBRSS<S, NarrowPhaseSolver>::
-canStop(ShapeMeshConservativeAdvancementTraversalNodeOBBRSS<S, NarrowPhaseSolver>::Scalar c) const
+canStop(typename NarrowPhaseSolver::Scalar c) const
 {
   return details::meshShapeConservativeAdvancementOrientedNodeCanStop(
         c,