Merge pull request #107 from jmirabel/devel

Fix generateBVHModel + reduced execution time of unit-tests in debug mode.

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);
 }

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);
 

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>

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);

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);

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';

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);
 

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;
 

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

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