diff --git a/.travis.yml b/.travis.yml
index eee60d261353c9384d1b2f880fd79402c2498d9a..b4b0109aca0251acbb2a1cb14e6078b71a0b79e8 100644
--- a/.travis.yml
+++ b/.travis.yml
@@ -3,19 +3,37 @@ language: cpp
 env:
   global:
     - CTEST_PARALLEL_LEVEL=4
-  matrix:
-    - BUILD_TYPE=Debug
-    - BUILD_TYPE=Release
+    - CTEST_OUTPUT_ON_FAILURE=1
+    - CXX_FLAGS_DEBUG="-O1"
 
 matrix:
   include:
-    - dist: trusty
-      compiler: gcc
-    - dist: xenial
-      compiler: gcc
-    - dist: xenial
+    - name: "Trusty - Debug - g++"
+      env: BUILD_TYPE=Debug
+      dist: trusty
+      compiler: g++
+
+    - name: "Xenial - Debug - g++"
+      env: BUILD_TYPE=Debug
+      dist: xenial
+      compiler: g++
+    - name: "Xenial - Release - g++"
+      env: BUILD_TYPE=Release
+      dist: xenial
+      compiler: g++
+    - name: "Xenial - Debug - clang"
+      env: BUILD_TYPE=Debug
+      dist: xenial
       compiler: clang
-    - os: osx
+
+    - name: "Bionic - Debug - g++"
+      env: BUILD_TYPE=Debug
+      dist: xenial
+      compiler: g++
+
+    - name: "OSX - Debug - clang"
+      env: BUILD_TYPE=Debug
+      os: osx
       compiler: clang
 
 install:
@@ -29,13 +47,13 @@ script:
   - cd build
 
   # Configure
-  - cmake -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DCMAKE_CXX_FLAGS=-w ..
+  - cmake -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DCMAKE_CXX_FLAGS=-w -DCMAKE_CXX_FLAGS_DEBUG=${CXX_FLAGS_DEBUG} ..
 
   # Build
   - make -j4
 
   # Run unit tests
-  - travis_wait 30 make test || travis_wait 30 ctest -VV
+  - travis_wait 30 ctest
 
   # Make sure we can install and uninstall with no issues
   - sudo make -j4 install
diff --git a/cmake b/cmake
index 441552634e4c427956be7b2834a6bbf894b24f0c..b58bd669f6567662eefb8a410e8e40aeba4f1060 160000
--- a/cmake
+++ b/cmake
@@ -1 +1 @@
-Subproject commit 441552634e4c427956be7b2834a6bbf894b24f0c
+Subproject commit b58bd669f6567662eefb8a410e8e40aeba4f1060
diff --git a/include/hpp/fcl/mesh_loader/assimp.h b/include/hpp/fcl/mesh_loader/assimp.h
index 758a3b8a39c40697b00b7d51bb84eeb131698a58..79ba1808605527ce69dcd8a946639bb9abea4868 100644
--- a/include/hpp/fcl/mesh_loader/assimp.h
+++ b/include/hpp/fcl/mesh_loader/assimp.h
@@ -41,6 +41,9 @@
 #include <hpp/fcl/BVH/BVH_model.h>
 
 class aiScene;
+namespace Assimp {
+  class Importer;
+}
 
 namespace hpp
 {
@@ -62,7 +65,8 @@ struct Loader {
 
   void load (const std::string& resource_path);
 
-  aiScene* scene;
+  Assimp::Importer* importer;
+  aiScene const* scene;
 };
 
 /**
diff --git a/src/BVH/BVH_model.cpp b/src/BVH/BVH_model.cpp
index 7fabab7b61ed40161b12597878c6ff0b8cd7053f..350f81602f4c5b09d0894135163029df65cc6f46 100644
--- a/src/BVH/BVH_model.cpp
+++ b/src/BVH/BVH_model.cpp
@@ -764,8 +764,14 @@ int BVHModel<BV>::refitTree(bool bottomup)
 template<typename BV>
 int BVHModel<BV>::refitTree_bottomup()
 {
+  // TODO the recomputation of the BV is done manually, without using
+  // bv_fitter. The manual BV recomputation seems bugged. Using bv_fitter
+  // seems to correct the bug.
+  //bv_fitter->set(vertices, tri_indices, getModelType());
+
   int res = recursiveRefitTree_bottomup(0);
 
+  //bv_fitter->clear();
   return res;
 }
 
@@ -812,6 +818,9 @@ int BVHModel<BV>::recursiveRefitTree_bottomup(int bv_id)
       }
       else
       {
+        //TODO use bv_fitter to build BV. See comment in refitTree_bottomup
+        //unsigned int* cur_primitive_indices = primitive_indices + bvnode->first_primitive;
+        //bv = bv_fitter->fit(cur_primitive_indices, bvnode->num_primitives);
         Vec3f v[3];
         for(int i = 0; i < 3; ++i)
         {
@@ -834,6 +843,9 @@ int BVHModel<BV>::recursiveRefitTree_bottomup(int bv_id)
     recursiveRefitTree_bottomup(bvnode->leftChild());
     recursiveRefitTree_bottomup(bvnode->rightChild());
     bvnode->bv = bvs[bvnode->leftChild()].bv + bvs[bvnode->rightChild()].bv;
+    //TODO use bv_fitter to build BV. See comment in refitTree_bottomup
+    //unsigned int* cur_primitive_indices = primitive_indices + bvnode->first_primitive;
+    //bvnode->bv = bv_fitter->fit(cur_primitive_indices, bvnode->num_primitives);
   }
 
   return BVH_OK;
diff --git a/src/mesh_loader/assimp.cpp b/src/mesh_loader/assimp.cpp
index 26d157d8c876bb309b131430209f47442a73f6c2..d529ec6dc6f6e1d8554caec9616a11467890fc60 100644
--- a/src/mesh_loader/assimp.cpp
+++ b/src/mesh_loader/assimp.cpp
@@ -58,18 +58,10 @@ namespace fcl
 namespace internal
 {
 
-Loader::Loader () : scene (NULL) {}
-
-Loader::~Loader ()
+Loader::Loader () : importer (new Assimp::Importer())
 {
- if (scene) delete scene;
-}
-
-void Loader::load (const std::string & resource_path)
-{
-  Assimp::Importer importer;
   // set list of ignored parameters (parameters used for rendering)
-  importer.SetPropertyInteger(AI_CONFIG_PP_RVC_FLAGS,
+  importer->SetPropertyInteger(AI_CONFIG_PP_RVC_FLAGS,
       aiComponent_TANGENTS_AND_BITANGENTS|
       aiComponent_COLORS |
       aiComponent_BONEWEIGHTS |
@@ -82,7 +74,16 @@ void Loader::load (const std::string & resource_path)
       aiComponent_NORMALS
       );
 
-  importer.ReadFile(resource_path.c_str(),
+}
+
+Loader::~Loader ()
+{
+ if (importer) delete importer;
+}
+
+void Loader::load (const std::string & resource_path)
+{
+  scene = importer->ReadFile(resource_path.c_str(),
       aiProcess_SortByPType |
       aiProcess_Triangulate |
       aiProcess_RemoveComponent |
@@ -94,11 +95,10 @@ void Loader::load (const std::string & resource_path)
       aiProcess_JoinIdenticalVertices
       );
 
-  scene = importer.GetOrphanedScene();
   if (!scene)
   {
     const std::string exception_message (std::string ("Could not load resource ") + resource_path + std::string("\n") +
-                                         importer.GetErrorString () + std::string("\n") +
+                                         importer->GetErrorString () + std::string("\n") +
                                          "Hint: the mesh directory may be wrong.");
     throw std::invalid_argument(exception_message);
   }
diff --git a/src/narrowphase/gjk.cpp b/src/narrowphase/gjk.cpp
index fa37055895d3843d2210fe093714782e94437ee5..9db577c471f7237298e08ede37046659b96bf51a 100644
--- a/src/narrowphase/gjk.cpp
+++ b/src/narrowphase/gjk.cpp
@@ -248,6 +248,14 @@ void getSupportFuncTpl (const MinkowskiDiff& md,
     bool ( (bool)shape_traits<Shape0>::NeedNormalizedDir
         || (bool)shape_traits<Shape1>::NeedNormalizedDir)
   };
+#ifndef NDEBUG
+  // Need normalized direction and direction is normalized
+  assert(!NeedNormalizedDir || !dirIsNormalized || fabs(dir.squaredNorm() - 1) < 1e-6);
+  // Need normalized direction but direction is not normalized.
+  assert(!NeedNormalizedDir ||  dirIsNormalized || fabs(dir.normalized().squaredNorm() - 1) < 1e-6);
+  // Don't need normalized direction. Check that dir is not zero.
+  assert( NeedNormalizedDir || dir.cwiseAbs().maxCoeff() >= 1e-6);
+#endif
   getSupportTpl<Shape0, Shape1, TransformIsIdentity> (
       static_cast <const Shape0*>(md.shapes[0]),
       static_cast <const Shape1*>(md.shapes[1]),
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index e0d1165370ac0af9986bd20ee114832587fef02c..33b1a3df3fb4ea1a65f433b34976bb984f841745 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -13,43 +13,46 @@ macro(add_fcl_test test_name)
   target_link_libraries(${test_name}
     hpp-fcl
     ${Boost_LIBRARIES}
+    utility
     )
+  PKG_CONFIG_USE_DEPENDENCY(${test_name} assimp)
   add_test(${test_name} ${EXECUTABLE_OUTPUT_PATH}/${test_name})
 endmacro(add_fcl_test)
 
 include_directories(${CMAKE_CURRENT_BINARY_DIR})
 include_directories(SYSTEM ${Boost_INCLUDE_DIRS})
 
+add_library(utility STATIC utility.cpp)
 
 add_fcl_test(math math.cpp)
 
-add_fcl_test(collision collision.cpp utility.cpp)
-add_fcl_test(distance distance.cpp utility.cpp)
-add_fcl_test(distance_lower_bound distance_lower_bound.cpp utility.cpp)
-add_fcl_test(geometric_shapes geometric_shapes.cpp utility.cpp)
-#add_fcl_test(broadphase broadphase.cpp utility.cpp)
-#add_fcl_test(shape_mesh_consistency shape_mesh_consistency.cpp utility.cpp)
-add_fcl_test(frontlist frontlist.cpp utility.cpp)
-#add_fcl_test(math math.cpp utility.cpp)
+add_fcl_test(collision collision.cpp)
+add_fcl_test(distance distance.cpp)
+add_fcl_test(distance_lower_bound distance_lower_bound.cpp)
+add_fcl_test(geometric_shapes geometric_shapes.cpp)
+#add_fcl_test(broadphase broadphase.cpp)
+#add_fcl_test(shape_mesh_consistency shape_mesh_consistency.cpp)
+add_fcl_test(frontlist frontlist.cpp)
+#add_fcl_test(math math.cpp)
 
 # add_fcl_test(sphere_capsule sphere_capsule.cpp)
-add_fcl_test(capsule_capsule capsule_capsule.cpp utility.cpp)
-add_fcl_test(box_box_distance box_box_distance.cpp utility.cpp)
+add_fcl_test(capsule_capsule capsule_capsule.cpp)
+add_fcl_test(box_box_distance box_box_distance.cpp)
 add_fcl_test(simple simple.cpp)
-add_fcl_test(capsule_box_1 capsule_box_1.cpp utility.cpp)
-add_fcl_test(capsule_box_2 capsule_box_2.cpp utility.cpp)
+add_fcl_test(capsule_box_1 capsule_box_1.cpp)
+add_fcl_test(capsule_box_2 capsule_box_2.cpp)
 add_fcl_test(obb obb.cpp)
 
-add_fcl_test(bvh_models bvh_models.cpp utility.cpp)
+add_fcl_test(bvh_models bvh_models.cpp)
 
-add_fcl_test(profiling profiling.cpp utility.cpp)
+add_fcl_test(profiling profiling.cpp)
 PKG_CONFIG_USE_DEPENDENCY(profiling assimp)
 
 add_fcl_test(gjk gjk.cpp)
 if(HPP_FCL_HAVE_OCTOMAP)
-  add_fcl_test(octree octree.cpp utility.cpp)
+  add_fcl_test(octree octree.cpp)
 endif(HPP_FCL_HAVE_OCTOMAP)
 
 ## Benchmark
-add_executable(test-benchmark benchmark.cpp utility.cpp)
-target_link_libraries(test-benchmark hpp-fcl ${Boost_LIBRARIES})
+add_executable(test-benchmark benchmark.cpp)
+target_link_libraries(test-benchmark hpp-fcl ${Boost_LIBRARIES} utility)
diff --git a/test/collision.cpp b/test/collision.cpp
index d6c0c911dad67d6c78efe1cf4b37166fea12b029..e2e791d83765c316eb45ebbb9c28da69560c5e51 100644
--- a/test/collision.cpp
+++ b/test/collision.cpp
@@ -221,7 +221,7 @@ bool bs_hp = false;
 #define BENCHMARK_NEXT() if (bench_stream!=NULL && !bs_nl) { *bench_stream << '\n';  bs_nl = true; bs_hp = true; }
 
 typedef std::vector<Contact> Contacts_t;
-typedef boost::mpl::vector<OBB, RSS, AABB, KDOP<24>, KDOP<18>, KDOP<16>, kIOS, OBBRSS> BVs_t;
+typedef boost::mpl::vector<OBB, RSS, KDOP<24>, KDOP<18>, KDOP<16>, kIOS, OBBRSS> BVs_t;
 std::vector<SplitMethodType> splitMethods = boost::assign::list_of (SPLIT_METHOD_MEAN)(SPLIT_METHOD_MEDIAN)(SPLIT_METHOD_BV_CENTER);
 
 typedef boost::chrono::high_resolution_clock clock_type;
@@ -475,6 +475,50 @@ struct mesh_mesh_run_test
     }
   }
 
+  void check_contacts (std::size_t i0, std::size_t i1, bool warn)
+  {
+    for(std::size_t i = i0; i < i1; ++i) {
+      Contacts_t in_ref_but_not_in_i;
+      std::set_difference (
+          contacts_ref[i].begin(), contacts_ref[i].end(),
+          contacts    [i].begin(), contacts    [i].end(),
+          std::inserter(in_ref_but_not_in_i, in_ref_but_not_in_i.begin()));
+      if(!in_ref_but_not_in_i.empty()) {
+        for(std::size_t j = 0; j < in_ref_but_not_in_i.size(); ++j) {
+          if (warn) {
+            BOOST_WARN_MESSAGE (false, "Missed contacts: "
+                << in_ref_but_not_in_i[j].b1 << ", "
+                << in_ref_but_not_in_i[j].b2);
+          } else {
+            BOOST_CHECK_MESSAGE(false, "Missed contacts: "
+                << in_ref_but_not_in_i[j].b1 << ", "
+                << in_ref_but_not_in_i[j].b2);
+          }
+        }
+      }
+
+      Contacts_t in_i_but_not_in_ref;
+      std::set_difference (
+          contacts    [i].begin(), contacts    [i].end(),
+          contacts_ref[i].begin(), contacts_ref[i].end(),
+          std::inserter(in_i_but_not_in_ref, in_i_but_not_in_ref.begin()));
+
+      if(!in_i_but_not_in_ref.empty()) {
+        for(std::size_t j = 0; j < in_i_but_not_in_ref.size(); ++j) {
+          if (warn) {
+            BOOST_WARN_MESSAGE (false, "False contacts: "
+                << in_i_but_not_in_ref[j].b1 << ", "
+                << in_i_but_not_in_ref[j].b2);
+          } else {
+            BOOST_CHECK_MESSAGE(false, "False contacts: "
+                << in_i_but_not_in_ref[j].b1 << ", "
+                << in_i_but_not_in_ref[j].b2);
+          }
+        }
+      }
+    }
+  }
+
   template<typename BV>
   void check ()
   {
@@ -485,31 +529,22 @@ struct mesh_mesh_run_test
     BOOST_REQUIRE_EQUAL(contacts.size(), contacts_ref.size());
 
     if (traits<BV, Oriented, Recursive>::IS_IMPLEMENTED) {
-      for(std::size_t i = 0; i < N; ++i) {
-        BOOST_CHECK_EQUAL(contacts_ref[i].size(), contacts[i].size());
-        for(std::size_t j = 0; j < contacts[i].size(); ++j) {
-          BOOST_CHECK_EQUAL(contacts_ref[i][j].b1, contacts[i][j].b1);
-          BOOST_CHECK_EQUAL(contacts_ref[i][j].b2, contacts[i][j].b2);
-        }
-      }
+      BOOST_TEST_MESSAGE (getindent() << "BV: " << str<BV>() << " oriented");
+      ++indent;
+      check_contacts (0, N, false);
+      --indent;
     }
     if (traits<BV, NonOriented, Recursive>::IS_IMPLEMENTED) {
-      for(std::size_t i = N; i < 2*N; ++i) {
-        BOOST_CHECK_EQUAL(contacts_ref[i].size(), contacts[i].size());
-        for(std::size_t j = 0; j < contacts[i].size(); ++j) {
-          BOOST_CHECK_EQUAL(contacts_ref[i][j].b1, contacts[i][j].b1);
-          BOOST_CHECK_EQUAL(contacts_ref[i][j].b2, contacts[i][j].b2);
-        }
-      }
+      BOOST_TEST_MESSAGE (getindent() << "BV: " << str<BV>());
+      ++indent;
+      check_contacts (N, 2*N, true);
+      --indent;
     }
     if (traits<BV, Oriented, NonRecursive>::IS_IMPLEMENTED) {
-      for(std::size_t i = 2*N; i < 3*N; ++i) {
-        BOOST_CHECK_EQUAL(contacts_ref[i].size(), contacts[i].size());
-        for(std::size_t j = 0; j < contacts[i].size(); ++j) {
-          BOOST_CHECK_EQUAL(contacts_ref[i][j].b1, contacts[i][j].b1);
-          BOOST_CHECK_EQUAL(contacts_ref[i][j].b2, contacts[i][j].b2);
-        }
-      }
+      BOOST_TEST_MESSAGE (getindent() << "BV: " << str<BV>() << " oriented non-recursive");
+      ++indent;
+      check_contacts (2*N, 3*N, false);
+      --indent;
     }
   }
 
diff --git a/test/geometric_shapes.cpp b/test/geometric_shapes.cpp
index e65aad57e4ad143f95d317f53a33a03e07c9e7e4..3d48154deab50f68d82ddfdda9be962d54c02aba 100644
--- a/test/geometric_shapes.cpp
+++ b/test/geometric_shapes.cpp
@@ -56,28 +56,6 @@ FCL_REAL tol_gjk = 0.01;
 GJKSolver solver1;
 GJKSolver solver2;
 
-Eigen::IOFormat fmt(Eigen::StreamPrecision, Eigen::DontAlignCols, ", ", ", ", "", "", "", "");
-Eigen::IOFormat pyfmt(Eigen::StreamPrecision, Eigen::DontAlignCols, ", ", ", ", "", "", "[", "]");
-
-typedef Eigen::AngleAxis<FCL_REAL> AngleAxis;
-//static const Vec3f UnitX (1, 0, 0);
-//static const Vec3f UnitY (0, 1, 0);
-static const Vec3f UnitZ (0, 0, 1);
-
-
-namespace hpp {
-namespace fcl {
-std::ostream& operator<< (std::ostream& os, const Transform3f& tf)
-{
-  return os << "[ " <<
-    tf.getTranslation().format(fmt)
-    << ", "
-    << tf.getQuatRotation().coeffs().format(fmt)
-    << " ]" ;
-}
-}
-}
-
 #define BOOST_CHECK_FALSE(p) BOOST_CHECK(!(p))
 
 template <typename S1, typename S2>
diff --git a/test/obb.cpp b/test/obb.cpp
index 9438596796f27b66d406c9192dabb5cd37c3343f..0536ef32acfbb399de9998170b38fb363c9a3c6d 100644
--- a/test/obb.cpp
+++ b/test/obb.cpp
@@ -1321,7 +1321,7 @@ BenchmarkResult benchmark_obb_case (
   return result;
 }
 
-std::size_t obb_overlap_and_lower_bound_distance()
+std::size_t obb_overlap_and_lower_bound_distance(std::ostream* output)
 {
   std::size_t nbFailure = 0;
 
@@ -1336,8 +1336,7 @@ std::size_t obb_overlap_and_lower_bound_distance()
   static const size_t nbRunForTimeMeas  = 1000;
   static const FCL_REAL extentNorm = 1.;
 
-  std::ostream& output = std::cout;
-  output << BenchmarkResult::headers << '\n';
+  if (output != NULL) *output << BenchmarkResult::headers << '\n';
   
   BenchmarkResult result;
   for (std::size_t iobb = 0; iobb < nbRandomOBB; ++iobb) {
@@ -1345,15 +1344,21 @@ std::size_t obb_overlap_and_lower_bound_distance()
     for (std::size_t itf = 0; itf < nbTransformPerOBB; ++itf) {
       randomTransform (B, T, a, b, extentNorm);
       result = benchmark_obb_case (B, T, a, b, request, nbRunForTimeMeas);
-      output << result << '\n';
+      if (output != NULL) *output << result << '\n';
       if (result.failure) nbFailure++;
     }
   }
   return nbFailure;
 }
 
-int main ()
+int main (int argc, char** argv)
 {
+  std::ostream* output = NULL;
+  if (argc > 1 && strcmp(argv[1], "--generate-output") == 0)
+  {
+    output = &std::cout;
+  }
+
   bool cpuScalingEnabled = checkCpuScalingEnabled();
   if (cpuScalingEnabled)
     std::cerr <<
@@ -1365,7 +1370,7 @@ int main ()
       "\n"
       ;
 
-  std::size_t nbFailure = obb_overlap_and_lower_bound_distance();
+  std::size_t nbFailure = obb_overlap_and_lower_bound_distance(output);
   if (nbFailure > INT_MAX) return INT_MAX;
   return (int)nbFailure;
 }
diff --git a/test/utility.cpp b/test/utility.cpp
index f5708d948c5b9970b56a7c38b9be2a4b0ca1b048..e5e54baefff8ba545b6c08626e04fe312fb81efa 100644
--- a/test/utility.cpp
+++ b/test/utility.cpp
@@ -10,7 +10,6 @@ namespace hpp
 namespace fcl
 {
 
-
 Timer::Timer()
 {
 #ifdef _WIN32
@@ -94,6 +93,13 @@ double Timer::getElapsedTime()
 }
 
 
+const Eigen::IOFormat vfmt  = Eigen::IOFormat (Eigen::StreamPrecision, Eigen::DontAlignCols, ", ", ", ", "", "", "", "");
+const Eigen::IOFormat pyfmt = Eigen::IOFormat (Eigen::StreamPrecision, Eigen::DontAlignCols, ", ", ", ", "", "", "[", "]");
+
+const Vec3f UnitX = Vec3f(1, 0, 0);
+const Vec3f UnitY = Vec3f(0, 1, 0);
+const Vec3f UnitZ = Vec3f(0, 0, 1);
+
 FCL_REAL rand_interval(FCL_REAL rmin, FCL_REAL rmax)
 {
   FCL_REAL t = rand() / ((FCL_REAL)RAND_MAX + 1);
@@ -473,6 +479,15 @@ Quaternion3f makeQuat(FCL_REAL w, FCL_REAL x, FCL_REAL y, FCL_REAL z)
   return q;
 }
 
+std::ostream& operator<< (std::ostream& os, const Transform3f& tf)
+{
+  return os << "[ " <<
+    tf.getTranslation().format(vfmt)
+    << ", "
+    << tf.getQuatRotation().coeffs().format(vfmt)
+    << " ]" ;
+}
+
 }
 
 } // namespace hpp
diff --git a/test/utility.h b/test/utility.h
index 80bab48ebdc7a24282a2543fe98a4d9fe8ddff86..d724399db73e086684e7757e63243ec3a18f27fa 100644
--- a/test/utility.h
+++ b/test/utility.h
@@ -92,6 +92,12 @@ private:
 #endif
 };
 
+extern const Eigen::IOFormat vfmt;
+extern const Eigen::IOFormat pyfmt;
+typedef Eigen::AngleAxis<FCL_REAL> AngleAxis;
+extern const Vec3f UnitX;
+extern const Vec3f UnitY;
+extern const Vec3f UnitZ;
 
 /// @brief Load an obj mesh file
 void loadOBJFile(const char* filename, std::vector<Vec3f>& points, std::vector<Triangle>& triangles);
@@ -176,6 +182,8 @@ std::string getGJKSolverName(GJKSolverType solver_type);
 
 Quaternion3f makeQuat(FCL_REAL w, FCL_REAL x, FCL_REAL y, FCL_REAL z);
 
+std::ostream& operator<< (std::ostream& os, const Transform3f& tf);
+
 }
 
 } // namespace hpp