diff --git a/include/hpp/fcl/shape/geometric_shape_to_BVH_model.h b/include/hpp/fcl/shape/geometric_shape_to_BVH_model.h
index 5307014ad0762d4d83d8f6473a2ebfb0513a1dfc..60e8d979d7912fc00799b59ef676b4b2ec65a6ec 100644
--- a/include/hpp/fcl/shape/geometric_shape_to_BVH_model.h
+++ b/include/hpp/fcl/shape/geometric_shape_to_BVH_model.h
@@ -52,11 +52,11 @@ namespace fcl
 template<typename BV>
 void generateBVHModel(BVHModel<BV>& model, const Box& shape, const Transform3f& pose)
 {
-  double a = shape.halfSide[0];
-  double b = shape.halfSide[1];
-  double c = shape.halfSide[2];
+  FCL_REAL a = shape.halfSide[0];
+  FCL_REAL b = shape.halfSide[1];
+  FCL_REAL c = shape.halfSide[2];
   std::vector<Vec3f> points(8);
-  Triangle tri_indices[12];
+  std::vector<Triangle> tri_indices(12);
   points[0] = Vec3f ( a, -b,  c);
   points[1] = Vec3f ( a,  b,  c);
   points[2] = Vec3f (-a,  b,  c);
@@ -97,19 +97,19 @@ void generateBVHModel(BVHModel<BV>& model, const Sphere& shape, const Transform3
   std::vector<Vec3f> points;
   std::vector<Triangle> tri_indices;
 
-  double r = shape.radius;
-  double phi, phid;
-  const double pi = boost::math::constants::pi<double>();
+  FCL_REAL r = shape.radius;
+  FCL_REAL phi, phid;
+  const FCL_REAL pi = boost::math::constants::pi<FCL_REAL>();
   phid = pi * 2 / seg;
   phi = 0;
 
-  double theta, thetad;
+  FCL_REAL theta, thetad;
   thetad = pi / (ring + 1);
   theta = 0;
 
   for(unsigned int i = 0; i < ring; ++i)
   {
-    double theta_ = theta + thetad * (i + 1);
+    FCL_REAL theta_ = theta + thetad * (i + 1);
     for(unsigned int j = 0; j < seg; ++j)
     {
       points.push_back(Vec3f(r * sin(theta_) * cos(phi + j * phid), r * sin(theta_) * sin(phi + j * phid), r * cos(theta_)));
@@ -161,10 +161,10 @@ void generateBVHModel(BVHModel<BV>& model, const Sphere& shape, const Transform3
 template<typename BV>
 void generateBVHModel(BVHModel<BV>& model, const Sphere& shape, const Transform3f& pose, unsigned int n_faces_for_unit_sphere)
 {
-  double r = shape.radius;
-  double n_low_bound = sqrtf(n_faces_for_unit_sphere / 2.0) * r * r;
-  unsigned int ring = ceil(n_low_bound);
-  unsigned int seg = ceil(n_low_bound);
+  FCL_REAL r = shape.radius;
+  FCL_REAL n_low_bound = std::sqrt((FCL_REAL)n_faces_for_unit_sphere / FCL_REAL(2.)) * r * r;
+  unsigned int ring = (unsigned int) ceil(n_low_bound);
+  unsigned int seg = (unsigned int) ceil(n_low_bound);
 
   generateBVHModel(model, shape, pose, seg, ring);  
 }
@@ -177,31 +177,31 @@ void generateBVHModel(BVHModel<BV>& model, const Cylinder& shape, const Transfor
   std::vector<Vec3f> points;
   std::vector<Triangle> tri_indices;
 
-  double r = shape.radius;
-  double h = shape.lz;
-  double phi, phid;
-  const double pi = boost::math::constants::pi<double>();
+  FCL_REAL r = shape.radius;
+  FCL_REAL h = shape.halfLength;
+  FCL_REAL phi, phid;
+  const FCL_REAL pi = boost::math::constants::pi<FCL_REAL>();
   phid = pi * 2 / tot;
   phi = 0;
 
-  double hd = h / h_num;
+  FCL_REAL hd = 2 * h / h_num;
 
   for(unsigned int i = 0; i < tot; ++i)
-    points.push_back(Vec3f(r * cos(phi + phid * i), r * sin(phi + phid * i), h / 2));
+    points.push_back(Vec3f(r * cos(phi + phid * i), r * sin(phi + phid * i), h));
 
   for(unsigned int i = 0; i < h_num - 1; ++i)
   {
     for(unsigned int j = 0; j < tot; ++j)
     {
-      points.push_back(Vec3f(r * cos(phi + phid * j), r * sin(phi + phid * j), h / 2 - (i + 1) * hd));
+      points.push_back(Vec3f(r * cos(phi + phid * j), r * sin(phi + phid * j), h - (i + 1) * hd));
     }
   }
 
   for(unsigned int i = 0; i < tot; ++i)
-    points.push_back(Vec3f(r * cos(phi + phid * i), r * sin(phi + phid * i), - h / 2));
+    points.push_back(Vec3f(r * cos(phi + phid * i), r * sin(phi + phid * i), - h));
 
-  points.push_back(Vec3f(0, 0, h / 2));
-  points.push_back(Vec3f(0, 0, -h / 2));
+  points.push_back(Vec3f(0, 0, h));
+  points.push_back(Vec3f(0, 0, -h));
 
   for(unsigned int i = 0; i < tot; ++i)
   {
@@ -248,15 +248,15 @@ void generateBVHModel(BVHModel<BV>& model, const Cylinder& shape, const Transfor
 template<typename BV>
 void generateBVHModel(BVHModel<BV>& model, const Cylinder& shape, const Transform3f& pose, unsigned int tot_for_unit_cylinder)
 {
-  double r = shape.radius;
-  double h = shape.lz;
+  FCL_REAL r = shape.radius;
+  FCL_REAL h = 2 * shape.halfLength;
 
-  const double pi = boost::math::constants::pi<double>();
-  unsigned int tot = tot_for_unit_cylinder * r;
-  double phid = pi * 2 / tot;
+  const FCL_REAL pi = boost::math::constants::pi<FCL_REAL>();
+  unsigned int tot = (unsigned int)(tot_for_unit_cylinder * r);
+  FCL_REAL phid = pi * 2 / tot;
 
-  double circle_edge = phid * r;
-  unsigned int h_num = ceil(h / circle_edge);
+  FCL_REAL circle_edge = phid * r;
+  unsigned int h_num = (unsigned int)ceil(h / circle_edge);
   
   generateBVHModel(model, shape, pose, tot, h_num);
 }
@@ -269,20 +269,20 @@ void generateBVHModel(BVHModel<BV>& model, const Cone& shape, const Transform3f&
   std::vector<Vec3f> points;
   std::vector<Triangle> tri_indices;
 
-  double r = shape.radius;
-  double h = shape.lz;
+  FCL_REAL r = shape.radius;
+  FCL_REAL h = shape.halfLength;
 
-  double phi, phid;
-  const double pi = boost::math::constants::pi<double>();
+  FCL_REAL phi, phid;
+  const FCL_REAL pi = boost::math::constants::pi<FCL_REAL>();
   phid = pi * 2 / tot;
   phi = 0;
 
-  double hd = h / h_num;
+  FCL_REAL hd = 2 * h / h_num;
 
   for(unsigned int i = 0; i < h_num - 1; ++i)
   {
-    double h_i = h / 2 - (i + 1) * hd;
-    double rh = r * (0.5 - h_i / h);
+    FCL_REAL h_i = h - (i + 1) * hd;
+    FCL_REAL rh = r * (0.5 - h_i / h / 2);
     for(unsigned int j = 0; j < tot; ++j)
     {
       points.push_back(Vec3f(rh * cos(phi + phid * j), rh * sin(phi + phid * j), h_i));
@@ -290,10 +290,10 @@ void generateBVHModel(BVHModel<BV>& model, const Cone& shape, const Transform3f&
   }
 
   for(unsigned int i = 0; i < tot; ++i)
-    points.push_back(Vec3f(r * cos(phi + phid * i), r * sin(phi + phid * i), - h / 2));
+    points.push_back(Vec3f(r * cos(phi + phid * i), r * sin(phi + phid * i), - h));
 
-  points.push_back(Vec3f(0, 0, h / 2));
-  points.push_back(Vec3f(0, 0, -h / 2));
+  points.push_back(Vec3f(0, 0, h));
+  points.push_back(Vec3f(0, 0, -h));
 
   for(unsigned int i = 0; i < tot; ++i)
   {
@@ -340,15 +340,15 @@ void generateBVHModel(BVHModel<BV>& model, const Cone& shape, const Transform3f&
 template<typename BV>
 void generateBVHModel(BVHModel<BV>& model, const Cone& shape, const Transform3f& pose, unsigned int tot_for_unit_cone)
 {
-  double r = shape.radius;
-  double h = shape.lz;
+  FCL_REAL r = shape.radius;
+  FCL_REAL h = 2 * shape.halfLength;
 
-  const double pi = boost::math::constants::pi<double>();
-  unsigned int tot = tot_for_unit_cone * r;
-  double phid = pi * 2 / tot;
+  const FCL_REAL pi = boost::math::constants::pi<FCL_REAL>();
+  unsigned int tot = (unsigned int)(tot_for_unit_cone * r);
+  FCL_REAL phid = pi * 2 / tot;
 
-  double circle_edge = phid * r;
-  unsigned int h_num = ceil(h / circle_edge);
+  FCL_REAL circle_edge = phid * r;
+  unsigned int h_num = (unsigned int)ceil(h / circle_edge);
 
   generateBVHModel(model, shape, pose, tot, h_num);
 }
diff --git a/test/collision.cpp b/test/collision.cpp
index 655ab7908303ae489f7466125aeb2c0e9239c716..da4655f681ee2541dfb674958e9c5d6381d2fd66 100644
--- a/test/collision.cpp
+++ b/test/collision.cpp
@@ -63,6 +63,7 @@
 #include "fcl_resources/config.h"
 
 using namespace hpp::fcl;
+namespace utf = boost::unit_test::framework;
 
 int num_max_contacts = std::numeric_limits<int>::max();
 
@@ -587,7 +588,12 @@ BOOST_AUTO_TEST_CASE(mesh_mesh)
 {
   std::vector<Transform3f> transforms;
   FCL_REAL extents[] = {-3000, -3000, 0, 3000, 3000, 3000};
+#ifndef NDEBUG // if debug mode
+  std::size_t n = 1;
+#else
   std::size_t n = 10;
+#endif
+  n = getNbRun(utf::master_test_suite().argc, utf::master_test_suite().argv, n);
 
   generateRandomTransforms(extents, transforms, n);
 
@@ -609,7 +615,12 @@ BOOST_AUTO_TEST_CASE(mesh_mesh_benchmark)
 {
   std::vector<Transform3f> transforms;
   FCL_REAL extents[] = {-3000, -3000, 0, 3000, 3000, 3000};
+#ifndef NDEBUG
+  std::size_t n = 0;
+#else
   std::size_t n = 10;
+#endif
+  n = getNbRun(utf::master_test_suite().argc, utf::master_test_suite().argv, n);
 
   generateRandomTransforms(extents, transforms, n);
 
diff --git a/test/distance.cpp b/test/distance.cpp
index dc4c2489aa433d1591e7dda3b04b742b7f091c18..f575a49a0bf19fa579f6ed58d946cc077bb3997f 100644
--- a/test/distance.cpp
+++ b/test/distance.cpp
@@ -51,6 +51,7 @@
 #include "fcl_resources/config.h"
 
 using namespace hpp::fcl;
+namespace utf = boost::unit_test::framework;
 
 bool verbose = false;
 FCL_REAL DELTA = 0.001;
@@ -96,7 +97,12 @@ BOOST_AUTO_TEST_CASE(mesh_distance)
 
   std::vector<Transform3f> transforms; // t0
   FCL_REAL extents[] = {-3000, -3000, 0, 3000, 3000, 3000};
+#ifndef NDEBUG // if debug mode
+  std::size_t n = 1;
+#else
   std::size_t n = 10;
+#endif
+  n = getNbRun(utf::master_test_suite().argc, utf::master_test_suite().argv, n);
 
   generateRandomTransforms(extents, transforms, n);
 
@@ -323,8 +329,8 @@ BOOST_AUTO_TEST_CASE(mesh_distance)
 
   }
 
-  std::cout << "distance timing: " << dis_time << " sec" << std::endl;
-  std::cout << "collision timing: " << col_time << " sec" << std::endl;
+  BOOST_TEST_MESSAGE("distance timing: " << dis_time << " sec");
+  BOOST_TEST_MESSAGE("collision timing: " << col_time << " sec");
 }
 
 template<typename BV, typename TraversalNode>
diff --git a/test/frontlist.cpp b/test/frontlist.cpp
index 9762d69b272c0211a7cd12778dba22e6cc2b6fd7..6862d88350a6db48373ee4938f441f5e198d87ee 100644
--- a/test/frontlist.cpp
+++ b/test/frontlist.cpp
@@ -51,6 +51,7 @@
 #include <boost/filesystem.hpp>
 
 using namespace hpp::fcl;
+namespace utf = boost::unit_test::framework;
 
 template<typename BV>
 bool collide_front_list_Test(const Transform3f& tf1, const Transform3f& tf2,
@@ -84,11 +85,12 @@ BOOST_AUTO_TEST_CASE(front_list)
   std::vector<Transform3f> transforms2; // t1
   FCL_REAL extents[] = {-3000, -3000, 0, 3000, 3000, 3000};
   FCL_REAL delta_trans[] = {1, 1, 1};
-#ifdef NDEBUG
-  std::size_t n = 20;
+#ifndef NDEBUG // if debug mode
+  std::size_t n = 2;
 #else
-  std::size_t n = 5;
+  std::size_t n = 20;
 #endif
+  n = getNbRun(utf::master_test_suite().argc, utf::master_test_suite().argv, n);
   bool verbose = false;
 
   generateRandomTransforms(extents, delta_trans, 0.005 * 2 * 3.1415, transforms, transforms2, n);
diff --git a/test/geometric_shapes.cpp b/test/geometric_shapes.cpp
index 08b22fec339ab5d1e987fb9875ea38c82a9e066c..ea57e378e1e17b1f9efcf9164834e3d232d3c95c 100644
--- a/test/geometric_shapes.cpp
+++ b/test/geometric_shapes.cpp
@@ -47,6 +47,7 @@
 #include "utility.h"
 #include <iostream>
 #include <hpp/fcl/internal/tools.h>
+#include <hpp/fcl/shape/geometric_shape_to_BVH_model.h>
 
 using namespace hpp::fcl;
 
@@ -189,6 +190,37 @@ void testShapeIntersection(const S1& s1, const Transform3f& tf1,
   }
 }
 
+BOOST_AUTO_TEST_CASE (box_to_bvh)
+{
+  Box shape (1,1,1);
+  BVHModel<OBB> bvh;
+  generateBVHModel (bvh, shape, Transform3f());
+}
+
+BOOST_AUTO_TEST_CASE (sphere_to_bvh)
+{
+  Sphere shape (1);
+  BVHModel<OBB> bvh;
+  generateBVHModel (bvh, shape, Transform3f(), 10, 10);
+  generateBVHModel (bvh, shape, Transform3f(), 50);
+}
+
+BOOST_AUTO_TEST_CASE (cylinder_to_bvh)
+{
+  Cylinder shape (1,1);
+  BVHModel<OBB> bvh;
+  generateBVHModel (bvh, shape, Transform3f(), 10, 10);
+  generateBVHModel (bvh, shape, Transform3f(), 50);
+}
+
+BOOST_AUTO_TEST_CASE (cone_to_bvh)
+{
+  Cone shape (1,1);
+  BVHModel<OBB> bvh;
+  generateBVHModel (bvh, shape, Transform3f(), 10, 10);
+  generateBVHModel (bvh, shape, Transform3f(), 50);
+}
+
 BOOST_AUTO_TEST_CASE (shapeIntersection_cylinderbox)
 {
   Cylinder s1 (0.029, 0.1);
diff --git a/test/obb.cpp b/test/obb.cpp
index fb4bd9a59cb40a5c9d4489a41c6ef0930f437b56..9b27360a454d2a774575425f00f4ed58626e49e1 100644
--- a/test/obb.cpp
+++ b/test/obb.cpp
@@ -45,6 +45,7 @@
 
 #include "../src/BV/OBB.h"
 #include "../src/distance_func_matrix.h"
+#include "utility.h"
 
 using namespace hpp::fcl;
 
@@ -1331,9 +1332,15 @@ std::size_t obb_overlap_and_lower_bound_distance(std::ostream* output)
   Vec3f T;
   CollisionRequest request;
 
+#ifndef NDEBUG // if debug mode
+  static const size_t nbRandomOBB       = 10;
+  static const size_t nbTransformPerOBB = 10;
+  static const size_t nbRunForTimeMeas  = 10;
+#else
   static const size_t nbRandomOBB       = 100;
   static const size_t nbTransformPerOBB = 100;
   static const size_t nbRunForTimeMeas  = 1000;
+#endif
   static const FCL_REAL extentNorm = 1.;
 
   if (output != NULL) *output << BenchmarkResult::headers << '\n';
diff --git a/test/octree.cpp b/test/octree.cpp
index 5632a496190508769b5f5e174002d88299966b9c..135f19c097760543c658ca4239672a42c76bb85d 100644
--- a/test/octree.cpp
+++ b/test/octree.cpp
@@ -49,6 +49,8 @@
 #include "utility.h"
 #include "fcl_resources/config.h"
 
+namespace utf = boost::unit_test::framework;
+
 using hpp::fcl::Vec3f;
 using hpp::fcl::Triangle;
 using hpp::fcl::OBBRSS;
@@ -136,7 +138,12 @@ BOOST_AUTO_TEST_CASE (OCTREE)
 
   std::vector<Transform3f> transforms;
   FCL_REAL extents[] = {-2000, -2000, 0, 2000, 2000, 2000};
+#ifndef NDEBUG // if debug mode
+  std::size_t N = 100;
+#else
   std::size_t N = 10000;
+#endif
+  N = hpp::fcl::getNbRun(utf::master_test_suite().argc, utf::master_test_suite().argv, N);
 
   generateRandomTransforms(extents, transforms, 2*N);
 
diff --git a/test/profiling.cpp b/test/profiling.cpp
index 2e96e4c82abe0ece23c4d3c4398b91c708441db2..16623c79b369bbcf32105982a77164d60fb21109 100644
--- a/test/profiling.cpp
+++ b/test/profiling.cpp
@@ -120,7 +120,11 @@ void printResults (const Geometry& g1, const Geometry& g2, const Results& rs)
   std::cout << g1.type << sep << g2.type << sep << mean << sep << std::sqrt(var) << sep << rs.times.minCoeff() << sep << rs.times.maxCoeff() << std::endl;
 }
 
+#ifndef NDEBUG // if debug mode
+int Ntransform = 1;
+#else
 int Ntransform = 100;
+#endif
 FCL_REAL limit = 20;
 bool verbose = false;
 
diff --git a/test/utility.cpp b/test/utility.cpp
index a107faf800e65c71051f954a91e6124b10761609..ca0debf7f1d5a97ebb27dcd39f25b0cac01c1884 100644
--- a/test/utility.cpp
+++ b/test/utility.cpp
@@ -444,6 +444,15 @@ std::ostream& operator<< (std::ostream& os, const Transform3f& tf)
     << " ]" ;
 }
 
+std::size_t getNbRun (const int& argc, char const* const* argv, std::size_t defaultValue)
+{
+  for (int i = 0; i < argc; ++i)
+    if (strcmp(argv[i], "--nb-run") == 0)
+      if (i+1 != argc)
+        return (std::size_t)strtol(argv[i+1], NULL, 10);
+  return defaultValue;
+}
+
 }
 
 } // namespace hpp
diff --git a/test/utility.h b/test/utility.h
index dcefa68407e78a52928ba54432d8cc602445425d..97b3b3355c18ea7929569e361fc758e7d352edaf 100644
--- a/test/utility.h
+++ b/test/utility.h
@@ -178,6 +178,9 @@ Quaternion3f makeQuat(FCL_REAL w, FCL_REAL x, FCL_REAL y, FCL_REAL z);
 
 std::ostream& operator<< (std::ostream& os, const Transform3f& tf);
 
+/// Get the argument --nb-run from argv
+std::size_t getNbRun (const int& argc, char const* const* argv, std::size_t defaultValue);
+
 }
 
 } // namespace hpp