From d9cf0bedc92e577bc4e88492cbceaddeb5e0b259 Mon Sep 17 00:00:00 2001
From: Nico van Duijn <nico@auterion.com>
Date: Wed, 20 Jun 2018 16:05:59 +0200
Subject: [PATCH 01/24] Add GenerateBVHSubModel variants

Identical to the GenerateBVHModel, but without ending the model, such that
more geometric primitives can be added. This is useful if you want to
create a BVHModel containing multiple geometric primitives.
---
 .../geometric_shape_to_BVH_model-inl.h        | 131 ++++++++++++++++++
 .../geometry/geometric_shape_to_BVH_model.h   |   6 +
 2 files changed, 137 insertions(+)

diff --git a/include/fcl/geometry/geometric_shape_to_BVH_model-inl.h b/include/fcl/geometry/geometric_shape_to_BVH_model-inl.h
index bafdfea73..797d7cbb8 100644
--- a/include/fcl/geometry/geometric_shape_to_BVH_model-inl.h
+++ b/include/fcl/geometry/geometric_shape_to_BVH_model-inl.h
@@ -87,6 +87,51 @@ void generateBVHModel(BVHModel<BV>& model, const Box<typename BV::S>& shape, con
   model.computeLocalAABB();
 }
 
+//==============================================================================
+template<typename BV>
+void generateBVHSubModel(BVHModel<BV>& model, const Box<typename BV::S>& shape, const Transform3<typename BV::S>& pose)
+{
+  using S = typename BV::S;
+
+  S a = shape.side[0];
+  S b = shape.side[1];
+  S c = shape.side[2];
+  std::vector<Vector3<S>> points(8);
+  std::vector<Triangle> tri_indices(12);
+  points[0] << 0.5 * a, -0.5 * b, 0.5 * c;
+  points[1] << 0.5 * a, 0.5 * b, 0.5 * c;
+  points[2] << -0.5 * a, 0.5 * b, 0.5 * c;
+  points[3] << -0.5 * a, -0.5 * b, 0.5 * c;
+  points[4] << 0.5 * a, -0.5 * b, -0.5 * c;
+  points[5] << 0.5 * a, 0.5 * b, -0.5 * c;
+  points[6] << -0.5 * a, 0.5 * b, -0.5 * c;
+  points[7] << -0.5 * a, -0.5 * b, -0.5 * c;
+
+  tri_indices[0].set(0, 4, 1);
+  tri_indices[1].set(1, 4, 5);
+  tri_indices[2].set(2, 6, 3);
+  tri_indices[3].set(3, 6, 7);
+  tri_indices[4].set(3, 0, 2);
+  tri_indices[5].set(2, 0, 1);
+  tri_indices[6].set(6, 5, 7);
+  tri_indices[7].set(7, 5, 4);
+  tri_indices[8].set(1, 5, 2);
+  tri_indices[9].set(2, 5, 6);
+  tri_indices[10].set(3, 7, 0);
+  tri_indices[11].set(0, 7, 4);
+
+  for(unsigned int i = 0; i < points.size(); ++i)
+  {
+    points[i] = pose * points[i];
+  }
+
+  if(model.build_state == BVH_BUILD_STATE_EMPTY){
+    model.beginModel();
+  }
+
+  model.addSubModel(points, tri_indices);
+}
+
 //==============================================================================
 template<typename BV>
 void generateBVHModel(BVHModel<BV>& model, const Sphere<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int seg, unsigned int ring)
@@ -345,7 +390,93 @@ void generateBVHModel(BVHModel<BV>& model, const Cylinder<typename BV::S>& shape
 
   generateBVHModel(model, shape, pose, tot, h_num);
 }
+//==============================================================================
+template<typename BV>
+void generateBVHSubModel(BVHModel<BV>& model, const Cylinder<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int tot, unsigned int h_num)
+{
+  using S = typename BV::S;
 
+  std::vector<Vector3<S>> points;
+  std::vector<Triangle> tri_indices;
+
+  S r = shape.radius;
+  S h = shape.lz;
+  S phi, phid;
+  const S pi = constants<S>::pi();
+  phid = pi * 2 / tot;
+  phi = 0;
+
+  S hd = h / h_num;
+
+  for(unsigned int i = 0; i < tot; ++i)
+    points.emplace_back(r * cos(phi + phid * i), r * sin(phi + phid * i), h / 2);
+
+  for(unsigned int i = 0; i < h_num - 1; ++i)
+  {
+    for(unsigned int j = 0; j < tot; ++j)
+    {
+      points.emplace_back(r * cos(phi + phid * j), r * sin(phi + phid * j), h / 2 - (i + 1) * hd);
+    }
+  }
+
+  for(unsigned int i = 0; i < tot; ++i)
+    points.emplace_back(r * cos(phi + phid * i), r * sin(phi + phid * i), - h / 2);
+
+  points.emplace_back(0, 0, h / 2);
+  points.emplace_back(0, 0, -h / 2);
+
+  for(unsigned int i = 0; i < tot; ++i)
+    tri_indices.emplace_back((h_num + 1) * tot, i, ((i == tot - 1) ? 0 : (i + 1)));
+
+  for(unsigned int i = 0; i < tot; ++i)
+    tri_indices.emplace_back((h_num + 1) * tot + 1, h_num * tot + ((i == tot - 1) ? 0 : (i + 1)), h_num * tot + i);
+
+  for(unsigned int i = 0; i < h_num; ++i)
+  {
+    for(unsigned int j = 0; j < tot; ++j)
+    {
+      int a, b, c, d;
+      a = j;
+      b = (j == tot - 1) ? 0 : (j + 1);
+      c = j + tot;
+      d = (j == tot - 1) ? tot : (j + 1 + tot);
+
+      int start = i * tot;
+      tri_indices.emplace_back(start + b, start + a, start + c);
+      tri_indices.emplace_back(start + b, start + c, start + d);
+    }
+  }
+
+  for(unsigned int i = 0; i < points.size(); ++i)
+  {
+    points[i] = pose * points[i];
+  }
+
+  if(model.build_state == BVH_BUILD_STATE_EMPTY){
+    model.beginModel();
+  }
+  model.addSubModel(points, tri_indices);
+
+}
+
+//==============================================================================
+template<typename BV>
+void generateBVHSubModel(BVHModel<BV>& model, const Cylinder<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int tot_for_unit_cylinder)
+{
+  using S = typename BV::S;
+
+  S r = shape.radius;
+  S h = shape.lz;
+
+  const S pi = constants<S>::pi();
+  unsigned int tot = tot_for_unit_cylinder * r;
+  S phid = pi * 2 / tot;
+
+  S circle_edge = phid * r;
+  unsigned int h_num = ceil(h / circle_edge);
+
+  generateBVHSubModel(model, shape, pose, tot, h_num);
+}
 
 //==============================================================================
 template<typename BV>
diff --git a/include/fcl/geometry/geometric_shape_to_BVH_model.h b/include/fcl/geometry/geometric_shape_to_BVH_model.h
index c18825d07..f6ab42340 100644
--- a/include/fcl/geometry/geometric_shape_to_BVH_model.h
+++ b/include/fcl/geometry/geometric_shape_to_BVH_model.h
@@ -56,6 +56,8 @@ namespace fcl
 /// @brief Generate BVH model from box
 template<typename BV>
 void generateBVHModel(BVHModel<BV>& model, const Box<typename BV::S>& shape, const Transform3<typename BV::S>& pose);
+template<typename BV>
+void generateBVHSubModel(BVHModel<BV>& model, const Box<typename BV::S>& shape, const Transform3<typename BV::S>& pose);
 
 /// @brief Generate BVH model from sphere, given the number of segments along longitude and number of rings along latitude.
 template<typename BV>
@@ -81,12 +83,16 @@ void generateBVHModel(BVHModel<BV>& model, const Ellipsoid<typename BV::S>& shap
 /// @brief Generate BVH model from cylinder, given the number of segments along circle and the number of segments along axis.
 template<typename BV>
 void generateBVHModel(BVHModel<BV>& model, const Cylinder<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int tot, unsigned int h_num);
+template<typename BV>
+void generateBVHSubModel(BVHModel<BV>& model, const Cylinder<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int tot, unsigned int h_num);
 
 /// @brief Generate BVH model from cylinder
 /// Difference from generateBVHModel: is that it gives the circle split number tot for a cylinder with unit radius. For cylinder with
 /// larger radius, the number of circle split number is r * tot.
 template<typename BV>
 void generateBVHModel(BVHModel<BV>& model, const Cylinder<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int tot_for_unit_cylinder);
+template<typename BV>
+void generateBVHSubModel(BVHModel<BV>& model, const Cylinder<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int tot_for_unit_cylinder);
 
 /// @brief Generate BVH model from cone, given the number of segments along circle and the number of segments along axis.
 template<typename BV>

From 4753f473a88668ca1ac0c6e8af5b1ba52206c9dd Mon Sep 17 00:00:00 2001
From: Nico van Duijn <nico@auterion.com>
Date: Wed, 20 Jun 2018 16:48:14 +0200
Subject: [PATCH 02/24] Refactor to use boolean to choose submodel

This commit refactors the generateBVHModel() functions to use
an additional boolean argument to chose whether the geometric
primitive should be added to an existing model. This seems
a bit cleaner than the previously added "generateBVHSubModel"
variants.
---
 .../geometric_shape_to_BVH_model-inl.h        | 201 +++++-------------
 .../geometry/geometric_shape_to_BVH_model.h   |  25 +--
 test/test_fcl_broadphase_distance.cpp         |   8 +-
 test/test_fcl_shape_mesh_consistency.cpp      | 104 ++++-----
 test/test_fcl_utility.h                       |   4 +-
 5 files changed, 116 insertions(+), 226 deletions(-)

diff --git a/include/fcl/geometry/geometric_shape_to_BVH_model-inl.h b/include/fcl/geometry/geometric_shape_to_BVH_model-inl.h
index 797d7cbb8..7b46ba959 100644
--- a/include/fcl/geometry/geometric_shape_to_BVH_model-inl.h
+++ b/include/fcl/geometry/geometric_shape_to_BVH_model-inl.h
@@ -45,51 +45,7 @@ namespace fcl
 
 //==============================================================================
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Box<typename BV::S>& shape, const Transform3<typename BV::S>& pose)
-{
-  using S = typename BV::S;
-
-  S a = shape.side[0];
-  S b = shape.side[1];
-  S c = shape.side[2];
-  std::vector<Vector3<S>> points(8);
-  std::vector<Triangle> tri_indices(12);
-  points[0] << 0.5 * a, -0.5 * b, 0.5 * c;
-  points[1] << 0.5 * a, 0.5 * b, 0.5 * c;
-  points[2] << -0.5 * a, 0.5 * b, 0.5 * c;
-  points[3] << -0.5 * a, -0.5 * b, 0.5 * c;
-  points[4] << 0.5 * a, -0.5 * b, -0.5 * c;
-  points[5] << 0.5 * a, 0.5 * b, -0.5 * c;
-  points[6] << -0.5 * a, 0.5 * b, -0.5 * c;
-  points[7] << -0.5 * a, -0.5 * b, -0.5 * c;
-
-  tri_indices[0].set(0, 4, 1);
-  tri_indices[1].set(1, 4, 5);
-  tri_indices[2].set(2, 6, 3);
-  tri_indices[3].set(3, 6, 7);
-  tri_indices[4].set(3, 0, 2);
-  tri_indices[5].set(2, 0, 1);
-  tri_indices[6].set(6, 5, 7);
-  tri_indices[7].set(7, 5, 4);
-  tri_indices[8].set(1, 5, 2);
-  tri_indices[9].set(2, 5, 6);
-  tri_indices[10].set(3, 7, 0);
-  tri_indices[11].set(0, 7, 4);
-
-  for(unsigned int i = 0; i < points.size(); ++i)
-  {
-    points[i] = pose * points[i];
-  }
-
-  model.beginModel();
-  model.addSubModel(points, tri_indices);
-  model.endModel();
-  model.computeLocalAABB();
-}
-
-//==============================================================================
-template<typename BV>
-void generateBVHSubModel(BVHModel<BV>& model, const Box<typename BV::S>& shape, const Transform3<typename BV::S>& pose)
+void generateBVHModel(BVHModel<BV>& model, const Box<typename BV::S>& shape, const Transform3<typename BV::S>& pose, bool submodel)
 {
   using S = typename BV::S;
 
@@ -130,11 +86,16 @@ void generateBVHSubModel(BVHModel<BV>& model, const Box<typename BV::S>& shape,
   }
 
   model.addSubModel(points, tri_indices);
+  if(!submodel){
+    model.endModel();
+    model.computeLocalAABB();
+  }
 }
 
+
 //==============================================================================
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Sphere<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int seg, unsigned int ring)
+void generateBVHModel(BVHModel<BV>& model, const Sphere<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int seg, unsigned int ring, bool submodel)
 {
   using S = typename BV::S;
 
@@ -193,15 +154,20 @@ void generateBVHModel(BVHModel<BV>& model, const Sphere<typename BV::S>& shape,
     points[i] = pose * points[i];
   }
 
-  model.beginModel();
+  if(model.build_state == BVH_BUILD_STATE_EMPTY){
+    model.beginModel();
+  }
+
   model.addSubModel(points, tri_indices);
-  model.endModel();
-  model.computeLocalAABB();
+  if(!submodel){
+    model.endModel();
+    model.computeLocalAABB();
+  }
 }
 
 //==============================================================================
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Sphere<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int n_faces_for_unit_sphere)
+void generateBVHModel(BVHModel<BV>& model, const Sphere<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int n_faces_for_unit_sphere, bool submodel)
 {
   using S = typename BV::S;
 
@@ -210,12 +176,12 @@ void generateBVHModel(BVHModel<BV>& model, const Sphere<typename BV::S>& shape,
   unsigned int ring = ceil(n_low_bound);
   unsigned int seg = ceil(n_low_bound);
 
-  generateBVHModel(model, shape, pose, seg, ring);
+  generateBVHModel(model, shape, pose, seg, ring, submodel);
 }
 
 //==============================================================================
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Ellipsoid<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int seg, unsigned int ring)
+void generateBVHModel(BVHModel<BV>& model, const Ellipsoid<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int seg, unsigned int ring, bool submodel)
 {
   using S = typename BV::S;
 
@@ -277,15 +243,20 @@ void generateBVHModel(BVHModel<BV>& model, const Ellipsoid<typename BV::S>& shap
     points[i] = pose * points[i];
   }
 
-  model.beginModel();
+  if(model.build_state == BVH_BUILD_STATE_EMPTY){
+    model.beginModel();
+  }
+
   model.addSubModel(points, tri_indices);
-  model.endModel();
-  model.computeLocalAABB();
+  if(!submodel){
+    model.endModel();
+    model.computeLocalAABB();
+  }
 }
 
 //==============================================================================
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Ellipsoid<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int n_faces_for_unit_ellipsoid)
+void generateBVHModel(BVHModel<BV>& model, const Ellipsoid<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int n_faces_for_unit_ellipsoid, bool submodel)
 {
   using S = typename BV::S;
 
@@ -301,98 +272,12 @@ void generateBVHModel(BVHModel<BV>& model, const Ellipsoid<typename BV::S>& shap
   const unsigned int ring = std::ceil(n_low_bound);
   const unsigned int seg = std::ceil(n_low_bound);
 
-  generateBVHModel(model, shape, pose, seg, ring);
+  generateBVHModel(model, shape, pose, seg, ring, submodel);
 }
 
 //==============================================================================
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Cylinder<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int tot, unsigned int h_num)
-{
-  using S = typename BV::S;
-
-  std::vector<Vector3<S>> points;
-  std::vector<Triangle> tri_indices;
-
-  S r = shape.radius;
-  S h = shape.lz;
-  S phi, phid;
-  const S pi = constants<S>::pi();
-  phid = pi * 2 / tot;
-  phi = 0;
-
-  S hd = h / h_num;
-
-  for(unsigned int i = 0; i < tot; ++i)
-    points.emplace_back(r * cos(phi + phid * i), r * sin(phi + phid * i), h / 2);
-
-  for(unsigned int i = 0; i < h_num - 1; ++i)
-  {
-    for(unsigned int j = 0; j < tot; ++j)
-    {
-      points.emplace_back(r * cos(phi + phid * j), r * sin(phi + phid * j), h / 2 - (i + 1) * hd);
-    }
-  }
-
-  for(unsigned int i = 0; i < tot; ++i)
-    points.emplace_back(r * cos(phi + phid * i), r * sin(phi + phid * i), - h / 2);
-
-  points.emplace_back(0, 0, h / 2);
-  points.emplace_back(0, 0, -h / 2);
-
-  for(unsigned int i = 0; i < tot; ++i)
-    tri_indices.emplace_back((h_num + 1) * tot, i, ((i == tot - 1) ? 0 : (i + 1)));
-
-  for(unsigned int i = 0; i < tot; ++i)
-    tri_indices.emplace_back((h_num + 1) * tot + 1, h_num * tot + ((i == tot - 1) ? 0 : (i + 1)), h_num * tot + i);
-
-  for(unsigned int i = 0; i < h_num; ++i)
-  {
-    for(unsigned int j = 0; j < tot; ++j)
-    {
-      int a, b, c, d;
-      a = j;
-      b = (j == tot - 1) ? 0 : (j + 1);
-      c = j + tot;
-      d = (j == tot - 1) ? tot : (j + 1 + tot);
-
-      int start = i * tot;
-      tri_indices.emplace_back(start + b, start + a, start + c);
-      tri_indices.emplace_back(start + b, start + c, start + d);
-    }
-  }
-
-  for(unsigned int i = 0; i < points.size(); ++i)
-  {
-    points[i] = pose * points[i];
-  }
-
-  model.beginModel();
-  model.addSubModel(points, tri_indices);
-  model.endModel();
-  model.computeLocalAABB();
-}
-
-//==============================================================================
-template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Cylinder<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int tot_for_unit_cylinder)
-{
-  using S = typename BV::S;
-
-  S r = shape.radius;
-  S h = shape.lz;
-
-  const S pi = constants<S>::pi();
-  unsigned int tot = tot_for_unit_cylinder * r;
-  S phid = pi * 2 / tot;
-
-  S circle_edge = phid * r;
-  unsigned int h_num = ceil(h / circle_edge);
-
-  generateBVHModel(model, shape, pose, tot, h_num);
-}
-//==============================================================================
-template<typename BV>
-void generateBVHSubModel(BVHModel<BV>& model, const Cylinder<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int tot, unsigned int h_num)
+void generateBVHModel(BVHModel<BV>& model, const Cylinder<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int tot, unsigned int h_num, bool submodel)
 {
   using S = typename BV::S;
 
@@ -455,13 +340,17 @@ void generateBVHSubModel(BVHModel<BV>& model, const Cylinder<typename BV::S>& sh
   if(model.build_state == BVH_BUILD_STATE_EMPTY){
     model.beginModel();
   }
-  model.addSubModel(points, tri_indices);
 
+  model.addSubModel(points, tri_indices);
+  if(!submodel){
+    model.endModel();
+    model.computeLocalAABB();
+  }
 }
 
 //==============================================================================
 template<typename BV>
-void generateBVHSubModel(BVHModel<BV>& model, const Cylinder<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int tot_for_unit_cylinder)
+void generateBVHModel(BVHModel<BV>& model, const Cylinder<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int tot_for_unit_cylinder, bool submodel)
 {
   using S = typename BV::S;
 
@@ -475,12 +364,12 @@ void generateBVHSubModel(BVHModel<BV>& model, const Cylinder<typename BV::S>& sh
   S circle_edge = phid * r;
   unsigned int h_num = ceil(h / circle_edge);
 
-  generateBVHSubModel(model, shape, pose, tot, h_num);
+  generateBVHModel(model, shape, pose, tot, h_num, submodel);
 }
 
 //==============================================================================
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Cone<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int tot, unsigned int h_num)
+void generateBVHModel(BVHModel<BV>& model, const Cone<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int tot, unsigned int h_num, bool submodel)
 {
   using S = typename BV::S;
 
@@ -540,15 +429,20 @@ void generateBVHModel(BVHModel<BV>& model, const Cone<typename BV::S>& shape, co
     points[i] = pose * points[i];
   }
 
-  model.beginModel();
+  if(model.build_state == BVH_BUILD_STATE_EMPTY){
+    model.beginModel();
+  }
+
   model.addSubModel(points, tri_indices);
-  model.endModel();
-  model.computeLocalAABB();
+  if(!submodel){
+    model.endModel();
+    model.computeLocalAABB();
+  }
 }
 
 //==============================================================================
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Cone<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int tot_for_unit_cone)
+void generateBVHModel(BVHModel<BV>& model, const Cone<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int tot_for_unit_cone, bool submodel)
 {
   using S = typename BV::S;
 
@@ -562,9 +456,10 @@ void generateBVHModel(BVHModel<BV>& model, const Cone<typename BV::S>& shape, co
   S circle_edge = phid * r;
   unsigned int h_num = ceil(h / circle_edge);
 
-  generateBVHModel(model, shape, pose, tot, h_num);
+  generateBVHModel(model, shape, pose, tot, h_num, submodel);
 }
 
 } // namespace fcl
 
 #endif
+
diff --git a/include/fcl/geometry/geometric_shape_to_BVH_model.h b/include/fcl/geometry/geometric_shape_to_BVH_model.h
index f6ab42340..a35219337 100644
--- a/include/fcl/geometry/geometric_shape_to_BVH_model.h
+++ b/include/fcl/geometry/geometric_shape_to_BVH_model.h
@@ -55,54 +55,49 @@ namespace fcl
 
 /// @brief Generate BVH model from box
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Box<typename BV::S>& shape, const Transform3<typename BV::S>& pose);
-template<typename BV>
-void generateBVHSubModel(BVHModel<BV>& model, const Box<typename BV::S>& shape, const Transform3<typename BV::S>& pose);
+void generateBVHModel(BVHModel<BV>& model, const Box<typename BV::S>& shape, const Transform3<typename BV::S>& pose, bool submodel = false);
 
 /// @brief Generate BVH model from sphere, given the number of segments along longitude and number of rings along latitude.
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Sphere<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int seg, unsigned int ring);
+void generateBVHModel(BVHModel<BV>& model, const Sphere<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int seg, unsigned int ring, bool submodel = false);
 
 /// @brief Generate BVH model from sphere
 /// The difference between generateBVHModel is that it gives the number of triangles faces N for a sphere with unit radius. For sphere of radius r,
 /// then the number of triangles is r * r * N so that the area represented by a single triangle is approximately the same.s
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Sphere<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int n_faces_for_unit_sphere);
+void generateBVHModel(BVHModel<BV>& model, const Sphere<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int n_faces_for_unit_sphere, bool submodel = false);
 
 /// @brief Generate BVH model from ellipsoid, given the number of segments along longitude and number of rings along latitude.
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Ellipsoid<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int seg, unsigned int ring);
+void generateBVHModel(BVHModel<BV>& model, const Ellipsoid<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int seg, unsigned int ring, bool submodel = false);
 
 /// @brief Generate BVH model from ellipsoid
 /// The difference between generateBVHModel is that it gives the number of triangles faces N for an ellipsoid with unit radii (1, 1, 1). For ellipsoid of radii (a, b, c),
 /// then the number of triangles is ((a^p * b^p + b^p * c^p + c^p * a^p)/3)^(1/p) * N, where p is 1.6075, so that the area represented by a single triangle is approximately the same.
 /// Reference: https://en.wikipedia.org/wiki/Ellipsoid<S>#Approximate_formula
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Ellipsoid<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int n_faces_for_unit_ellipsoid);
+void generateBVHModel(BVHModel<BV>& model, const Ellipsoid<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int n_faces_for_unit_ellipsoid, bool submodel = false);
 
 /// @brief Generate BVH model from cylinder, given the number of segments along circle and the number of segments along axis.
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Cylinder<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int tot, unsigned int h_num);
-template<typename BV>
-void generateBVHSubModel(BVHModel<BV>& model, const Cylinder<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int tot, unsigned int h_num);
+void generateBVHModel(BVHModel<BV>& model, const Cylinder<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int tot, unsigned int h_num, bool submodel = false);
 
 /// @brief Generate BVH model from cylinder
 /// Difference from generateBVHModel: is that it gives the circle split number tot for a cylinder with unit radius. For cylinder with
 /// larger radius, the number of circle split number is r * tot.
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Cylinder<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int tot_for_unit_cylinder);
-template<typename BV>
-void generateBVHSubModel(BVHModel<BV>& model, const Cylinder<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int tot_for_unit_cylinder);
+void generateBVHModel(BVHModel<BV>& model, const Cylinder<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int tot_for_unit_cylinder, bool submodel = false);
+
 
 /// @brief Generate BVH model from cone, given the number of segments along circle and the number of segments along axis.
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Cone<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int tot, unsigned int h_num);
+void generateBVHModel(BVHModel<BV>& model, const Cone<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int tot, unsigned int h_num, bool submodel = false);
 
 /// @brief Generate BVH model from cone
 /// Difference from generateBVHModel: is that it gives the circle split number tot for a cylinder with unit radius. For cone with
 /// larger radius, the number of circle split number is r * tot.
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Cone<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int tot_for_unit_cone);
+void generateBVHModel(BVHModel<BV>& model, const Cone<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int tot_for_unit_cone, bool submodel = false);
 
 } // namespace fcl
 
diff --git a/test/test_fcl_broadphase_distance.cpp b/test/test_fcl_broadphase_distance.cpp
index 38acaa237..c450361c6 100644
--- a/test/test_fcl_broadphase_distance.cpp
+++ b/test/test_fcl_broadphase_distance.cpp
@@ -263,7 +263,7 @@ void generateSelfDistanceEnvironmentsMesh(std::vector<CollisionObject<S>*>& env,
 
     Sphere<S> sphere(single_size / 2);
     BVHModel<OBBRSS<S>>* model = new BVHModel<OBBRSS<S>>();
-    generateBVHModel(*model, sphere, Transform3<S>::Identity(), 16, 16);
+    generateBVHModel(*model, sphere, Transform3<S>::Identity(), 16, 16, false);
     env.push_back(new CollisionObject<S>(std::shared_ptr<CollisionGeometry<S>>(model),
                                       Transform3<S>(Translation3<S>(Vector3<S>(x * step_size + delta_size + 0.5 * single_size - env_scale,
                                                             y * step_size + delta_size + 0.5 * single_size - env_scale,
@@ -278,7 +278,7 @@ void generateSelfDistanceEnvironmentsMesh(std::vector<CollisionObject<S>*>& env,
 
     Ellipsoid<S> ellipsoid(single_size / 2, single_size / 2, single_size / 2);
     BVHModel<OBBRSS<S>>* model = new BVHModel<OBBRSS<S>>();
-    generateBVHModel(*model, ellipsoid, Transform3<S>::Identity(), 16, 16);
+    generateBVHModel(*model, ellipsoid, Transform3<S>::Identity(), 16, 16, false);
     env.push_back(new CollisionObject<S>(std::shared_ptr<CollisionGeometry<S>>(model),
                                       Transform3<S>(Translation3<S>(Vector3<S>(x * step_size + delta_size + 0.5 * single_size - env_scale,
                                                             y * step_size + delta_size + 0.5 * single_size - env_scale,
@@ -293,7 +293,7 @@ void generateSelfDistanceEnvironmentsMesh(std::vector<CollisionObject<S>*>& env,
 
     Cylinder<S> cylinder(single_size / 2, single_size);
     BVHModel<OBBRSS<S>>* model = new BVHModel<OBBRSS<S>>();
-    generateBVHModel(*model, cylinder, Transform3<S>::Identity(), 16, 16);
+    generateBVHModel(*model, cylinder, Transform3<S>::Identity(), 16, 16, false);
     env.push_back(new CollisionObject<S>(std::shared_ptr<CollisionGeometry<S>>(model),
                                       Transform3<S>(Translation3<S>(Vector3<S>(x * step_size + delta_size + 0.5 * single_size - env_scale,
                                                             y * step_size + delta_size + 0.5 * single_size - env_scale,
@@ -308,7 +308,7 @@ void generateSelfDistanceEnvironmentsMesh(std::vector<CollisionObject<S>*>& env,
 
     Cone<S> cone(single_size / 2, single_size);
     BVHModel<OBBRSS<S>>* model = new BVHModel<OBBRSS<S>>();
-    generateBVHModel(*model, cone, Transform3<S>::Identity(), 16, 16);
+    generateBVHModel(*model, cone, Transform3<S>::Identity(), 16, 16, false);
     env.push_back(new CollisionObject<S>(std::shared_ptr<CollisionGeometry<S>>(model),
                                       Transform3<S>(Translation3<S>(Vector3<S>(x * step_size + delta_size + 0.5 * single_size - env_scale,
                                                             y * step_size + delta_size + 0.5 * single_size - env_scale,
diff --git a/test/test_fcl_shape_mesh_consistency.cpp b/test/test_fcl_shape_mesh_consistency.cpp
index f86ea0df8..77f86ae7e 100644
--- a/test/test_fcl_shape_mesh_consistency.cpp
+++ b/test/test_fcl_shape_mesh_consistency.cpp
@@ -63,8 +63,8 @@ void test_consistency_distance_spheresphere_libccd()
   BVHModel<RSS<S>> s1_rss;
   BVHModel<RSS<S>> s2_rss;
 
-  generateBVHModel(s1_rss, s1, Transform3<S>::Identity(), 16, 16);
-  generateBVHModel(s2_rss, s2, Transform3<S>::Identity(), 16, 16);
+  generateBVHModel(s1_rss, s1, Transform3<S>::Identity(), 16, 16, false);
+  generateBVHModel(s2_rss, s2, Transform3<S>::Identity(), 16, 16, false);
 
   DistanceRequest<S> request;
   DistanceResult<S> res, res1;
@@ -161,8 +161,8 @@ void test_consistency_distance_ellipsoidellipsoid_libccd()
   BVHModel<RSS<S>> s1_rss;
   BVHModel<RSS<S>> s2_rss;
 
-  generateBVHModel(s1_rss, s1, Transform3<S>::Identity(), 16, 16);
-  generateBVHModel(s2_rss, s2, Transform3<S>::Identity(), 16, 16);
+  generateBVHModel(s1_rss, s1, Transform3<S>::Identity(), 16, 16, false);
+  generateBVHModel(s2_rss, s2, Transform3<S>::Identity(), 16, 16, false);
 
   DistanceRequest<S> request;
   DistanceResult<S> res, res1;
@@ -357,8 +357,8 @@ void test_consistency_distance_cylindercylinder_libccd()
   BVHModel<RSS<S>> s1_rss;
   BVHModel<RSS<S>> s2_rss;
 
-  generateBVHModel(s1_rss, s1, Transform3<S>::Identity(), 16, 16);
-  generateBVHModel(s2_rss, s2, Transform3<S>::Identity(), 16, 16);
+  generateBVHModel(s1_rss, s1, Transform3<S>::Identity(), 16, 16, false);
+  generateBVHModel(s2_rss, s2, Transform3<S>::Identity(), 16, 16, false);
 
   DistanceRequest<S> request;
   DistanceResult<S> res, res1;
@@ -455,8 +455,8 @@ void test_consistency_distance_conecone_libccd()
   BVHModel<RSS<S>> s1_rss;
   BVHModel<RSS<S>> s2_rss;
 
-  generateBVHModel(s1_rss, s1, Transform3<S>::Identity(), 16, 16);
-  generateBVHModel(s2_rss, s2, Transform3<S>::Identity(), 16, 16);
+  generateBVHModel(s1_rss, s1, Transform3<S>::Identity(), 16, 16, false);
+  generateBVHModel(s2_rss, s2, Transform3<S>::Identity(), 16, 16, false);
 
   DistanceRequest<S> request;
   DistanceResult<S> res, res1;
@@ -551,8 +551,8 @@ void test_consistency_distance_spheresphere_GJK()
   BVHModel<RSS<S>> s1_rss;
   BVHModel<RSS<S>> s2_rss;
 
-  generateBVHModel(s1_rss, s1, Transform3<S>::Identity(), 16, 16);
-  generateBVHModel(s2_rss, s2, Transform3<S>::Identity(), 16, 16);
+  generateBVHModel(s1_rss, s1, Transform3<S>::Identity(), 16, 16, false);
+  generateBVHModel(s2_rss, s2, Transform3<S>::Identity(), 16, 16, false);
 
   DistanceRequest<S> request;
   request.gjk_solver_type = GST_INDEP;
@@ -651,8 +651,8 @@ void test_consistency_distance_ellipsoidellipsoid_GJK()
   BVHModel<RSS<S>> s1_rss;
   BVHModel<RSS<S>> s2_rss;
 
-  generateBVHModel(s1_rss, s1, Transform3<S>::Identity(), 16, 16);
-  generateBVHModel(s2_rss, s2, Transform3<S>::Identity(), 16, 16);
+  generateBVHModel(s1_rss, s1, Transform3<S>::Identity(), 16, 16, false);
+  generateBVHModel(s2_rss, s2, Transform3<S>::Identity(), 16, 16, false);
 
   DistanceRequest<S> request;
   request.gjk_solver_type = GST_INDEP;
@@ -851,8 +851,8 @@ void test_consistency_distance_cylindercylinder_GJK()
   BVHModel<RSS<S>> s1_rss;
   BVHModel<RSS<S>> s2_rss;
 
-  generateBVHModel(s1_rss, s1, Transform3<S>::Identity(), 16, 16);
-  generateBVHModel(s2_rss, s2, Transform3<S>::Identity(), 16, 16);
+  generateBVHModel(s1_rss, s1, Transform3<S>::Identity(), 16, 16, false);
+  generateBVHModel(s2_rss, s2, Transform3<S>::Identity(), 16, 16, false);
 
   DistanceRequest<S> request;
   request.gjk_solver_type = GST_INDEP;
@@ -959,8 +959,8 @@ void test_consistency_distance_conecone_GJK()
   BVHModel<RSS<S>> s1_rss;
   BVHModel<RSS<S>> s2_rss;
 
-  generateBVHModel(s1_rss, s1, Transform3<S>::Identity(), 16, 16);
-  generateBVHModel(s2_rss, s2, Transform3<S>::Identity(), 16, 16);
+  generateBVHModel(s1_rss, s1, Transform3<S>::Identity(), 16, 16, false);
+  generateBVHModel(s2_rss, s2, Transform3<S>::Identity(), 16, 16, false);
 
   DistanceRequest<S> request;
   request.gjk_solver_type = GST_INDEP;
@@ -1059,10 +1059,10 @@ void test_consistency_collision_spheresphere_libccd()
   BVHModel<OBB<S>> s1_obb;
   BVHModel<OBB<S>> s2_obb;
 
-  generateBVHModel(s1_aabb, s1, Transform3<S>::Identity(), 16, 16);
-  generateBVHModel(s2_aabb, s2, Transform3<S>::Identity(), 16, 16);
-  generateBVHModel(s1_obb, s1, Transform3<S>::Identity(), 16, 16);
-  generateBVHModel(s2_obb, s2, Transform3<S>::Identity(), 16, 16);
+  generateBVHModel(s1_aabb, s1, Transform3<S>::Identity(), 16, 16, false);
+  generateBVHModel(s2_aabb, s2, Transform3<S>::Identity(), 16, 16, false);
+  generateBVHModel(s1_obb, s1, Transform3<S>::Identity(), 16, 16, false);
+  generateBVHModel(s2_obb, s2, Transform3<S>::Identity(), 16, 16, false);
 
   CollisionRequest<S> request;
   CollisionResult<S> result;
@@ -1285,10 +1285,10 @@ void test_consistency_collision_ellipsoidellipsoid_libccd()
   BVHModel<OBB<S>> s1_obb;
   BVHModel<OBB<S>> s2_obb;
 
-  generateBVHModel(s1_aabb, s1, Transform3<S>::Identity(), 16, 16);
-  generateBVHModel(s2_aabb, s2, Transform3<S>::Identity(), 16, 16);
-  generateBVHModel(s1_obb, s1, Transform3<S>::Identity(), 16, 16);
-  generateBVHModel(s2_obb, s2, Transform3<S>::Identity(), 16, 16);
+  generateBVHModel(s1_aabb, s1, Transform3<S>::Identity(), 16, 16, false);
+  generateBVHModel(s2_aabb, s2, Transform3<S>::Identity(), 16, 16, false);
+  generateBVHModel(s1_obb, s1, Transform3<S>::Identity(), 16, 16, false);
+  generateBVHModel(s2_obb, s2, Transform3<S>::Identity(), 16, 16, false);
 
   CollisionRequest<S> request;
   CollisionResult<S> result;
@@ -1643,9 +1643,9 @@ void test_consistency_collision_spherebox_libccd()
   BVHModel<OBB<S>> s1_obb;
   BVHModel<OBB<S>> s2_obb;
 
-  generateBVHModel(s1_aabb, s1, Transform3<S>::Identity(), 16, 16);
+  generateBVHModel(s1_aabb, s1, Transform3<S>::Identity(), 16, 16, false);
   generateBVHModel(s2_aabb, s2, Transform3<S>::Identity());
-  generateBVHModel(s1_obb, s1, Transform3<S>::Identity(), 16, 16);
+  generateBVHModel(s1_obb, s1, Transform3<S>::Identity(), 16, 16, false);
   generateBVHModel(s2_obb, s2, Transform3<S>::Identity());
 
   CollisionRequest<S> request;
@@ -1772,10 +1772,10 @@ void test_consistency_collision_cylindercylinder_libccd()
   BVHModel<OBB<S>> s1_obb;
   BVHModel<OBB<S>> s2_obb;
 
-  generateBVHModel(s1_aabb, s1, Transform3<S>::Identity(), 16, 16);
-  generateBVHModel(s2_aabb, s2, Transform3<S>::Identity(), 16, 16);
-  generateBVHModel(s1_obb, s1, Transform3<S>::Identity(), 16, 16);
-  generateBVHModel(s2_obb, s2, Transform3<S>::Identity(), 16, 16);
+  generateBVHModel(s1_aabb, s1, Transform3<S>::Identity(), 16, 16, false);
+  generateBVHModel(s2_aabb, s2, Transform3<S>::Identity(), 16, 16, false);
+  generateBVHModel(s1_obb, s1, Transform3<S>::Identity(), 16, 16, false);
+  generateBVHModel(s2_obb, s2, Transform3<S>::Identity(), 16, 16, false);
 
   CollisionRequest<S> request;
   CollisionResult<S> result;
@@ -1868,10 +1868,10 @@ void test_consistency_collision_conecone_libccd()
   BVHModel<OBB<S>> s1_obb;
   BVHModel<OBB<S>> s2_obb;
 
-  generateBVHModel(s1_aabb, s1, Transform3<S>::Identity(), 16, 16);
-  generateBVHModel(s2_aabb, s2, Transform3<S>::Identity(), 16, 16);
-  generateBVHModel(s1_obb, s1, Transform3<S>::Identity(), 16, 16);
-  generateBVHModel(s2_obb, s2, Transform3<S>::Identity(), 16, 16);
+  generateBVHModel(s1_aabb, s1, Transform3<S>::Identity(), 16, 16, false);
+  generateBVHModel(s2_aabb, s2, Transform3<S>::Identity(), 16, 16, false);
+  generateBVHModel(s1_obb, s1, Transform3<S>::Identity(), 16, 16, false);
+  generateBVHModel(s2_obb, s2, Transform3<S>::Identity(), 16, 16, false);
 
   CollisionRequest<S> request;
   CollisionResult<S> result;
@@ -2027,10 +2027,10 @@ void test_consistency_collision_spheresphere_GJK()
   BVHModel<OBB<S>> s1_obb;
   BVHModel<OBB<S>> s2_obb;
 
-  generateBVHModel(s1_aabb, s1, Transform3<S>::Identity(), 16, 16);
-  generateBVHModel(s2_aabb, s2, Transform3<S>::Identity(), 16, 16);
-  generateBVHModel(s1_obb, s1, Transform3<S>::Identity(), 16, 16);
-  generateBVHModel(s2_obb, s2, Transform3<S>::Identity(), 16, 16);
+  generateBVHModel(s1_aabb, s1, Transform3<S>::Identity(), 16, 16, false);
+  generateBVHModel(s2_aabb, s2, Transform3<S>::Identity(), 16, 16, false);
+  generateBVHModel(s1_obb, s1, Transform3<S>::Identity(), 16, 16, false);
+  generateBVHModel(s2_obb, s2, Transform3<S>::Identity(), 16, 16, false);
 
   CollisionRequest<S> request;
   request.gjk_solver_type = GST_INDEP;
@@ -2255,10 +2255,10 @@ void test_consistency_collision_ellipsoidellipsoid_GJK()
   BVHModel<OBB<S>> s1_obb;
   BVHModel<OBB<S>> s2_obb;
 
-  generateBVHModel(s1_aabb, s1, Transform3<S>::Identity(), 16, 16);
-  generateBVHModel(s2_aabb, s2, Transform3<S>::Identity(), 16, 16);
-  generateBVHModel(s1_obb, s1, Transform3<S>::Identity(), 16, 16);
-  generateBVHModel(s2_obb, s2, Transform3<S>::Identity(), 16, 16);
+  generateBVHModel(s1_aabb, s1, Transform3<S>::Identity(), 16, 16, false);
+  generateBVHModel(s2_aabb, s2, Transform3<S>::Identity(), 16, 16, false);
+  generateBVHModel(s1_obb, s1, Transform3<S>::Identity(), 16, 16, false);
+  generateBVHModel(s2_obb, s2, Transform3<S>::Identity(), 16, 16, false);
 
   CollisionRequest<S> request;
   request.gjk_solver_type = GST_INDEP;
@@ -2615,9 +2615,9 @@ void test_consistency_collision_spherebox_GJK()
   BVHModel<OBB<S>> s1_obb;
   BVHModel<OBB<S>> s2_obb;
 
-  generateBVHModel(s1_aabb, s1, Transform3<S>::Identity(), 16, 16);
+  generateBVHModel(s1_aabb, s1, Transform3<S>::Identity(), 16, 16, false);
   generateBVHModel(s2_aabb, s2, Transform3<S>::Identity());
-  generateBVHModel(s1_obb, s1, Transform3<S>::Identity(), 16, 16);
+  generateBVHModel(s1_obb, s1, Transform3<S>::Identity(), 16, 16, false);
   generateBVHModel(s2_obb, s2, Transform3<S>::Identity());
 
   CollisionRequest<S> request;
@@ -2746,10 +2746,10 @@ void test_consistency_collision_cylindercylinder_GJK()
   BVHModel<OBB<S>> s1_obb;
   BVHModel<OBB<S>> s2_obb;
 
-  generateBVHModel(s1_aabb, s1, Transform3<S>::Identity(), 16, 16);
-  generateBVHModel(s2_aabb, s2, Transform3<S>::Identity(), 16, 16);
-  generateBVHModel(s1_obb, s1, Transform3<S>::Identity(), 16, 16);
-  generateBVHModel(s2_obb, s2, Transform3<S>::Identity(), 16, 16);
+  generateBVHModel(s1_aabb, s1, Transform3<S>::Identity(), 16, 16, false);
+  generateBVHModel(s2_aabb, s2, Transform3<S>::Identity(), 16, 16, false);
+  generateBVHModel(s1_obb, s1, Transform3<S>::Identity(), 16, 16, false);
+  generateBVHModel(s2_obb, s2, Transform3<S>::Identity(), 16, 16, false);
 
   CollisionRequest<S> request;
   request.gjk_solver_type = GST_INDEP;
@@ -2844,10 +2844,10 @@ void test_consistency_collision_conecone_GJK()
   BVHModel<OBB<S>> s1_obb;
   BVHModel<OBB<S>> s2_obb;
 
-  generateBVHModel(s1_aabb, s1, Transform3<S>::Identity(), 16, 16);
-  generateBVHModel(s2_aabb, s2, Transform3<S>::Identity(), 16, 16);
-  generateBVHModel(s1_obb, s1, Transform3<S>::Identity(), 16, 16);
-  generateBVHModel(s2_obb, s2, Transform3<S>::Identity(), 16, 16);
+  generateBVHModel(s1_aabb, s1, Transform3<S>::Identity(), 16, 16, false);
+  generateBVHModel(s2_aabb, s2, Transform3<S>::Identity(), 16, 16, false);
+  generateBVHModel(s1_obb, s1, Transform3<S>::Identity(), 16, 16, false);
+  generateBVHModel(s2_obb, s2, Transform3<S>::Identity(), 16, 16, false);
 
   CollisionRequest<S> request;
   request.gjk_solver_type = GST_INDEP;
diff --git a/test/test_fcl_utility.h b/test/test_fcl_utility.h
index 937b100ef..3c3884047 100644
--- a/test/test_fcl_utility.h
+++ b/test/test_fcl_utility.h
@@ -594,7 +594,7 @@ void generateEnvironmentsMesh(std::vector<CollisionObject<S>*>& env, S env_scale
   for(std::size_t i = 0; i < n; ++i)
   {
     BVHModel<OBBRSS<S>>* model = new BVHModel<OBBRSS<S>>();
-    generateBVHModel(*model, sphere, Transform3<S>::Identity(), 16, 16);
+    generateBVHModel(*model, sphere, Transform3<S>::Identity(), 16, 16, false);
     env.push_back(new CollisionObject<S>(std::shared_ptr<CollisionGeometry<S>>(model), transforms[i]));
   }
 
@@ -603,7 +603,7 @@ void generateEnvironmentsMesh(std::vector<CollisionObject<S>*>& env, S env_scale
   for(std::size_t i = 0; i < n; ++i)
   {
     BVHModel<OBBRSS<S>>* model = new BVHModel<OBBRSS<S>>();
-    generateBVHModel(*model, cylinder, Transform3<S>::Identity(), 16, 16);
+    generateBVHModel(*model, cylinder, Transform3<S>::Identity(), 16, 16, false);
     env.push_back(new CollisionObject<S>(std::shared_ptr<CollisionGeometry<S>>(model), transforms[i]));
   }
 }

From cd683c2c59f9eb2d4fbf70b152809eb20b6e07f4 Mon Sep 17 00:00:00 2001
From: Nico van Duijn <nico@auterion.com>
Date: Wed, 4 Jul 2018 09:13:29 +0200
Subject: [PATCH 03/24] Add doxygen group comments and change boolean name

In response to Michael Sherman's comments. Should be more clear now
---
 .../geometric_shape_to_BVH_model-inl.h        | 36 +++++++++----------
 .../geometry/geometric_shape_to_BVH_model.h   | 30 +++++++++++-----
 2 files changed, 39 insertions(+), 27 deletions(-)

diff --git a/include/fcl/geometry/geometric_shape_to_BVH_model-inl.h b/include/fcl/geometry/geometric_shape_to_BVH_model-inl.h
index 7b46ba959..4328236f8 100644
--- a/include/fcl/geometry/geometric_shape_to_BVH_model-inl.h
+++ b/include/fcl/geometry/geometric_shape_to_BVH_model-inl.h
@@ -45,7 +45,7 @@ namespace fcl
 
 //==============================================================================
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Box<typename BV::S>& shape, const Transform3<typename BV::S>& pose, bool submodel)
+void generateBVHModel(BVHModel<BV>& model, const Box<typename BV::S>& shape, const Transform3<typename BV::S>& pose, bool finalize_model)
 {
   using S = typename BV::S;
 
@@ -86,7 +86,7 @@ void generateBVHModel(BVHModel<BV>& model, const Box<typename BV::S>& shape, con
   }
 
   model.addSubModel(points, tri_indices);
-  if(!submodel){
+  if(finalize_model){
     model.endModel();
     model.computeLocalAABB();
   }
@@ -95,7 +95,7 @@ void generateBVHModel(BVHModel<BV>& model, const Box<typename BV::S>& shape, con
 
 //==============================================================================
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Sphere<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int seg, unsigned int ring, bool submodel)
+void generateBVHModel(BVHModel<BV>& model, const Sphere<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int seg, unsigned int ring, bool finalize_model)
 {
   using S = typename BV::S;
 
@@ -159,7 +159,7 @@ void generateBVHModel(BVHModel<BV>& model, const Sphere<typename BV::S>& shape,
   }
 
   model.addSubModel(points, tri_indices);
-  if(!submodel){
+  if(finalize_model){
     model.endModel();
     model.computeLocalAABB();
   }
@@ -167,7 +167,7 @@ void generateBVHModel(BVHModel<BV>& model, const Sphere<typename BV::S>& shape,
 
 //==============================================================================
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Sphere<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int n_faces_for_unit_sphere, bool submodel)
+void generateBVHModel(BVHModel<BV>& model, const Sphere<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int n_faces_for_unit_sphere, bool finalize_model)
 {
   using S = typename BV::S;
 
@@ -176,12 +176,12 @@ void generateBVHModel(BVHModel<BV>& model, const Sphere<typename BV::S>& shape,
   unsigned int ring = ceil(n_low_bound);
   unsigned int seg = ceil(n_low_bound);
 
-  generateBVHModel(model, shape, pose, seg, ring, submodel);
+  generateBVHModel(model, shape, pose, seg, ring, finalize_model);
 }
 
 //==============================================================================
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Ellipsoid<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int seg, unsigned int ring, bool submodel)
+void generateBVHModel(BVHModel<BV>& model, const Ellipsoid<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int seg, unsigned int ring, bool finalize_model)
 {
   using S = typename BV::S;
 
@@ -248,7 +248,7 @@ void generateBVHModel(BVHModel<BV>& model, const Ellipsoid<typename BV::S>& shap
   }
 
   model.addSubModel(points, tri_indices);
-  if(!submodel){
+  if(finalize_model){
     model.endModel();
     model.computeLocalAABB();
   }
@@ -256,7 +256,7 @@ void generateBVHModel(BVHModel<BV>& model, const Ellipsoid<typename BV::S>& shap
 
 //==============================================================================
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Ellipsoid<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int n_faces_for_unit_ellipsoid, bool submodel)
+void generateBVHModel(BVHModel<BV>& model, const Ellipsoid<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int n_faces_for_unit_ellipsoid, bool finalize_model)
 {
   using S = typename BV::S;
 
@@ -272,12 +272,12 @@ void generateBVHModel(BVHModel<BV>& model, const Ellipsoid<typename BV::S>& shap
   const unsigned int ring = std::ceil(n_low_bound);
   const unsigned int seg = std::ceil(n_low_bound);
 
-  generateBVHModel(model, shape, pose, seg, ring, submodel);
+  generateBVHModel(model, shape, pose, seg, ring, finalize_model);
 }
 
 //==============================================================================
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Cylinder<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int tot, unsigned int h_num, bool submodel)
+void generateBVHModel(BVHModel<BV>& model, const Cylinder<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int tot, unsigned int h_num, bool finalize_model)
 {
   using S = typename BV::S;
 
@@ -342,7 +342,7 @@ void generateBVHModel(BVHModel<BV>& model, const Cylinder<typename BV::S>& shape
   }
 
   model.addSubModel(points, tri_indices);
-  if(!submodel){
+  if(finalize_model){
     model.endModel();
     model.computeLocalAABB();
   }
@@ -350,7 +350,7 @@ void generateBVHModel(BVHModel<BV>& model, const Cylinder<typename BV::S>& shape
 
 //==============================================================================
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Cylinder<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int tot_for_unit_cylinder, bool submodel)
+void generateBVHModel(BVHModel<BV>& model, const Cylinder<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int tot_for_unit_cylinder, bool finalize_model)
 {
   using S = typename BV::S;
 
@@ -364,12 +364,12 @@ void generateBVHModel(BVHModel<BV>& model, const Cylinder<typename BV::S>& shape
   S circle_edge = phid * r;
   unsigned int h_num = ceil(h / circle_edge);
 
-  generateBVHModel(model, shape, pose, tot, h_num, submodel);
+  generateBVHModel(model, shape, pose, tot, h_num, finalize_model);
 }
 
 //==============================================================================
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Cone<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int tot, unsigned int h_num, bool submodel)
+void generateBVHModel(BVHModel<BV>& model, const Cone<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int tot, unsigned int h_num, bool finalize_model)
 {
   using S = typename BV::S;
 
@@ -434,7 +434,7 @@ void generateBVHModel(BVHModel<BV>& model, const Cone<typename BV::S>& shape, co
   }
 
   model.addSubModel(points, tri_indices);
-  if(!submodel){
+  if(finalize_model){
     model.endModel();
     model.computeLocalAABB();
   }
@@ -442,7 +442,7 @@ void generateBVHModel(BVHModel<BV>& model, const Cone<typename BV::S>& shape, co
 
 //==============================================================================
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Cone<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int tot_for_unit_cone, bool submodel)
+void generateBVHModel(BVHModel<BV>& model, const Cone<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int tot_for_unit_cone, bool finalize_model)
 {
   using S = typename BV::S;
 
@@ -456,7 +456,7 @@ void generateBVHModel(BVHModel<BV>& model, const Cone<typename BV::S>& shape, co
   S circle_edge = phid * r;
   unsigned int h_num = ceil(h / circle_edge);
 
-  generateBVHModel(model, shape, pose, tot, h_num, submodel);
+  generateBVHModel(model, shape, pose, tot, h_num, finalize_model);
 }
 
 } // namespace fcl
diff --git a/include/fcl/geometry/geometric_shape_to_BVH_model.h b/include/fcl/geometry/geometric_shape_to_BVH_model.h
index a35219337..f58a4e987 100644
--- a/include/fcl/geometry/geometric_shape_to_BVH_model.h
+++ b/include/fcl/geometry/geometric_shape_to_BVH_model.h
@@ -52,55 +52,67 @@
 
 namespace fcl
 {
+/**
+* \defgroup generateBVHModel
+* @param[out]  model a reference to the BVHModel to be generated or appended to
+* @param[in]   shape a reference to the geometric object to be added to the BVHModel
+* @param[in]   pose a const reference to the pose of the geometric object
+* @param[in]   finalize_model a boolean indicating whether the model is final or more submodels can be added later
+* @{
+*/
 
 /// @brief Generate BVH model from box
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Box<typename BV::S>& shape, const Transform3<typename BV::S>& pose, bool submodel = false);
+void generateBVHModel(BVHModel<BV>& model, const Box<typename BV::S>& shape, const Transform3<typename BV::S>& pose, bool finalize_model = true);
 
 /// @brief Generate BVH model from sphere, given the number of segments along longitude and number of rings along latitude.
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Sphere<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int seg, unsigned int ring, bool submodel = false);
+void generateBVHModel(BVHModel<BV>& model, const Sphere<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int seg, unsigned int ring, bool finalize_model = true);
 
 /// @brief Generate BVH model from sphere
 /// The difference between generateBVHModel is that it gives the number of triangles faces N for a sphere with unit radius. For sphere of radius r,
 /// then the number of triangles is r * r * N so that the area represented by a single triangle is approximately the same.s
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Sphere<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int n_faces_for_unit_sphere, bool submodel = false);
+void generateBVHModel(BVHModel<BV>& model, const Sphere<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int n_faces_for_unit_sphere, bool finalize_model = true);
 
 /// @brief Generate BVH model from ellipsoid, given the number of segments along longitude and number of rings along latitude.
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Ellipsoid<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int seg, unsigned int ring, bool submodel = false);
+void generateBVHModel(BVHModel<BV>& model, const Ellipsoid<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int seg, unsigned int ring, bool finalize_model = true);
 
 /// @brief Generate BVH model from ellipsoid
 /// The difference between generateBVHModel is that it gives the number of triangles faces N for an ellipsoid with unit radii (1, 1, 1). For ellipsoid of radii (a, b, c),
 /// then the number of triangles is ((a^p * b^p + b^p * c^p + c^p * a^p)/3)^(1/p) * N, where p is 1.6075, so that the area represented by a single triangle is approximately the same.
 /// Reference: https://en.wikipedia.org/wiki/Ellipsoid<S>#Approximate_formula
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Ellipsoid<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int n_faces_for_unit_ellipsoid, bool submodel = false);
+void generateBVHModel(BVHModel<BV>& model, const Ellipsoid<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int n_faces_for_unit_ellipsoid, bool finalize_model = true);
 
 /// @brief Generate BVH model from cylinder, given the number of segments along circle and the number of segments along axis.
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Cylinder<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int tot, unsigned int h_num, bool submodel = false);
+void generateBVHModel(BVHModel<BV>& model, const Cylinder<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int tot, unsigned int h_num, bool finalize_model = true);
 
 /// @brief Generate BVH model from cylinder
 /// Difference from generateBVHModel: is that it gives the circle split number tot for a cylinder with unit radius. For cylinder with
 /// larger radius, the number of circle split number is r * tot.
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Cylinder<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int tot_for_unit_cylinder, bool submodel = false);
+void generateBVHModel(BVHModel<BV>& model, const Cylinder<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int tot_for_unit_cylinder, bool finalize_model = true);
 
 
 /// @brief Generate BVH model from cone, given the number of segments along circle and the number of segments along axis.
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Cone<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int tot, unsigned int h_num, bool submodel = false);
+void generateBVHModel(BVHModel<BV>& model, const Cone<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int tot, unsigned int h_num, bool finalize_model = true);
 
 /// @brief Generate BVH model from cone
 /// Difference from generateBVHModel: is that it gives the circle split number tot for a cylinder with unit radius. For cone with
 /// larger radius, the number of circle split number is r * tot.
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Cone<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int tot_for_unit_cone, bool submodel = false);
+void generateBVHModel(BVHModel<BV>& model, const Cone<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int tot_for_unit_cone, bool finalize_model = true);
+
+/**@} */ // end of doxygen group generateBVHModel
 
 } // namespace fcl
 
+
+
 #include "fcl/geometry/geometric_shape_to_BVH_model-inl.h"
 
 #endif

From 65aa330ca14c443cdf0e31c99f04721f3340a7c2 Mon Sep 17 00:00:00 2001
From: Nico van Duijn <nico@auterion.com>
Date: Wed, 4 Jul 2018 11:29:53 +0200
Subject: [PATCH 04/24] Use enum class instead of boolean

This is needed to avoid implicit casts from unsigned int that
would render the overload resolution impossible.
---
 .../geometric_shape_to_BVH_model-inl.h        |  28 ++---
 .../geometry/geometric_shape_to_BVH_model.h   |  31 ++++--
 test/test_fcl_broadphase_distance.cpp         |   8 +-
 test/test_fcl_shape_mesh_consistency.cpp      | 104 +++++++++---------
 test/test_fcl_utility.h                       |   4 +-
 5 files changed, 92 insertions(+), 83 deletions(-)

diff --git a/include/fcl/geometry/geometric_shape_to_BVH_model-inl.h b/include/fcl/geometry/geometric_shape_to_BVH_model-inl.h
index 4328236f8..701bab962 100644
--- a/include/fcl/geometry/geometric_shape_to_BVH_model-inl.h
+++ b/include/fcl/geometry/geometric_shape_to_BVH_model-inl.h
@@ -45,7 +45,7 @@ namespace fcl
 
 //==============================================================================
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Box<typename BV::S>& shape, const Transform3<typename BV::S>& pose, bool finalize_model)
+void generateBVHModel(BVHModel<BV>& model, const Box<typename BV::S>& shape, const Transform3<typename BV::S>& pose, FinalizeModel finalize_model)
 {
   using S = typename BV::S;
 
@@ -86,7 +86,7 @@ void generateBVHModel(BVHModel<BV>& model, const Box<typename BV::S>& shape, con
   }
 
   model.addSubModel(points, tri_indices);
-  if(finalize_model){
+  if(finalize_model == FinalizeModel::DO_FINALIZE){
     model.endModel();
     model.computeLocalAABB();
   }
@@ -95,7 +95,7 @@ void generateBVHModel(BVHModel<BV>& model, const Box<typename BV::S>& shape, con
 
 //==============================================================================
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Sphere<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int seg, unsigned int ring, bool finalize_model)
+void generateBVHModel(BVHModel<BV>& model, const Sphere<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int seg, unsigned int ring, FinalizeModel finalize_model)
 {
   using S = typename BV::S;
 
@@ -159,7 +159,7 @@ void generateBVHModel(BVHModel<BV>& model, const Sphere<typename BV::S>& shape,
   }
 
   model.addSubModel(points, tri_indices);
-  if(finalize_model){
+  if(finalize_model == FinalizeModel::DO_FINALIZE){
     model.endModel();
     model.computeLocalAABB();
   }
@@ -167,7 +167,7 @@ void generateBVHModel(BVHModel<BV>& model, const Sphere<typename BV::S>& shape,
 
 //==============================================================================
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Sphere<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int n_faces_for_unit_sphere, bool finalize_model)
+void generateBVHModel(BVHModel<BV>& model, const Sphere<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int n_faces_for_unit_sphere, FinalizeModel finalize_model)
 {
   using S = typename BV::S;
 
@@ -181,7 +181,7 @@ void generateBVHModel(BVHModel<BV>& model, const Sphere<typename BV::S>& shape,
 
 //==============================================================================
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Ellipsoid<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int seg, unsigned int ring, bool finalize_model)
+void generateBVHModel(BVHModel<BV>& model, const Ellipsoid<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int seg, unsigned int ring, FinalizeModel finalize_model)
 {
   using S = typename BV::S;
 
@@ -248,7 +248,7 @@ void generateBVHModel(BVHModel<BV>& model, const Ellipsoid<typename BV::S>& shap
   }
 
   model.addSubModel(points, tri_indices);
-  if(finalize_model){
+  if(finalize_model == FinalizeModel::DO_FINALIZE){
     model.endModel();
     model.computeLocalAABB();
   }
@@ -256,7 +256,7 @@ void generateBVHModel(BVHModel<BV>& model, const Ellipsoid<typename BV::S>& shap
 
 //==============================================================================
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Ellipsoid<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int n_faces_for_unit_ellipsoid, bool finalize_model)
+void generateBVHModel(BVHModel<BV>& model, const Ellipsoid<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int n_faces_for_unit_ellipsoid, FinalizeModel finalize_model)
 {
   using S = typename BV::S;
 
@@ -277,7 +277,7 @@ void generateBVHModel(BVHModel<BV>& model, const Ellipsoid<typename BV::S>& shap
 
 //==============================================================================
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Cylinder<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int tot, unsigned int h_num, bool finalize_model)
+void generateBVHModel(BVHModel<BV>& model, const Cylinder<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int tot, unsigned int h_num, FinalizeModel finalize_model)
 {
   using S = typename BV::S;
 
@@ -342,7 +342,7 @@ void generateBVHModel(BVHModel<BV>& model, const Cylinder<typename BV::S>& shape
   }
 
   model.addSubModel(points, tri_indices);
-  if(finalize_model){
+  if(finalize_model == FinalizeModel::DO_FINALIZE){
     model.endModel();
     model.computeLocalAABB();
   }
@@ -350,7 +350,7 @@ void generateBVHModel(BVHModel<BV>& model, const Cylinder<typename BV::S>& shape
 
 //==============================================================================
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Cylinder<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int tot_for_unit_cylinder, bool finalize_model)
+void generateBVHModel(BVHModel<BV>& model, const Cylinder<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int tot_for_unit_cylinder, FinalizeModel finalize_model)
 {
   using S = typename BV::S;
 
@@ -369,7 +369,7 @@ void generateBVHModel(BVHModel<BV>& model, const Cylinder<typename BV::S>& shape
 
 //==============================================================================
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Cone<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int tot, unsigned int h_num, bool finalize_model)
+void generateBVHModel(BVHModel<BV>& model, const Cone<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int tot, unsigned int h_num, FinalizeModel finalize_model)
 {
   using S = typename BV::S;
 
@@ -434,7 +434,7 @@ void generateBVHModel(BVHModel<BV>& model, const Cone<typename BV::S>& shape, co
   }
 
   model.addSubModel(points, tri_indices);
-  if(finalize_model){
+  if(finalize_model == FinalizeModel::DO_FINALIZE){
     model.endModel();
     model.computeLocalAABB();
   }
@@ -442,7 +442,7 @@ void generateBVHModel(BVHModel<BV>& model, const Cone<typename BV::S>& shape, co
 
 //==============================================================================
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Cone<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int tot_for_unit_cone, bool finalize_model)
+void generateBVHModel(BVHModel<BV>& model, const Cone<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int tot_for_unit_cone, FinalizeModel finalize_model)
 {
   using S = typename BV::S;
 
diff --git a/include/fcl/geometry/geometric_shape_to_BVH_model.h b/include/fcl/geometry/geometric_shape_to_BVH_model.h
index f58a4e987..bf977ee20 100644
--- a/include/fcl/geometry/geometric_shape_to_BVH_model.h
+++ b/include/fcl/geometry/geometric_shape_to_BVH_model.h
@@ -54,58 +54,67 @@ namespace fcl
 {
 /**
 * \defgroup generateBVHModel
-* @param[out]  model a reference to the BVHModel to be generated or appended to
+* @param[out]  model a reference to the BVHModel to be generated or added to
 * @param[in]   shape a reference to the geometric object to be added to the BVHModel
 * @param[in]   pose a const reference to the pose of the geometric object
-* @param[in]   finalize_model a boolean indicating whether the model is final or more submodels can be added later
+* @param[in]   finalize_model a FinalizeModel enum indicating whether the model is final or more submodels can be added later
 * @{
 */
 
+/**
+* @brief    enum class used to indicate whether we simply want to add more primitives to the model
+*           or finalize it.
+*/
+enum class FinalizeModel{
+	DO_FINALIZE,
+	DONT_FINALIZE
+};
+
 /// @brief Generate BVH model from box
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Box<typename BV::S>& shape, const Transform3<typename BV::S>& pose, bool finalize_model = true);
+void generateBVHModel(BVHModel<BV>& model, const Box<typename BV::S>& shape, const Transform3<typename BV::S>& pose, FinalizeModel finalize_model = FinalizeModel::DO_FINALIZE);
 
 /// @brief Generate BVH model from sphere, given the number of segments along longitude and number of rings along latitude.
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Sphere<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int seg, unsigned int ring, bool finalize_model = true);
+void generateBVHModel(BVHModel<BV>& model, const Sphere<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int seg, unsigned int ring, FinalizeModel finalize_model = FinalizeModel::DO_FINALIZE);
 
 /// @brief Generate BVH model from sphere
 /// The difference between generateBVHModel is that it gives the number of triangles faces N for a sphere with unit radius. For sphere of radius r,
 /// then the number of triangles is r * r * N so that the area represented by a single triangle is approximately the same.s
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Sphere<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int n_faces_for_unit_sphere, bool finalize_model = true);
+void generateBVHModel(BVHModel<BV>& model, const Sphere<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int n_faces_for_unit_sphere, FinalizeModel finalize_model = FinalizeModel::DO_FINALIZE);
 
 /// @brief Generate BVH model from ellipsoid, given the number of segments along longitude and number of rings along latitude.
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Ellipsoid<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int seg, unsigned int ring, bool finalize_model = true);
+void generateBVHModel(BVHModel<BV>& model, const Ellipsoid<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int seg, unsigned int ring, FinalizeModel finalize_model = FinalizeModel::DO_FINALIZE);
 
 /// @brief Generate BVH model from ellipsoid
 /// The difference between generateBVHModel is that it gives the number of triangles faces N for an ellipsoid with unit radii (1, 1, 1). For ellipsoid of radii (a, b, c),
 /// then the number of triangles is ((a^p * b^p + b^p * c^p + c^p * a^p)/3)^(1/p) * N, where p is 1.6075, so that the area represented by a single triangle is approximately the same.
 /// Reference: https://en.wikipedia.org/wiki/Ellipsoid<S>#Approximate_formula
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Ellipsoid<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int n_faces_for_unit_ellipsoid, bool finalize_model = true);
+void generateBVHModel(BVHModel<BV>& model, const Ellipsoid<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int n_faces_for_unit_ellipsoid, FinalizeModel finalize_model = FinalizeModel::DO_FINALIZE);
 
 /// @brief Generate BVH model from cylinder, given the number of segments along circle and the number of segments along axis.
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Cylinder<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int tot, unsigned int h_num, bool finalize_model = true);
+void generateBVHModel(BVHModel<BV>& model, const Cylinder<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int tot, unsigned int h_num, FinalizeModel finalize_model = FinalizeModel::DO_FINALIZE);
 
 /// @brief Generate BVH model from cylinder
 /// Difference from generateBVHModel: is that it gives the circle split number tot for a cylinder with unit radius. For cylinder with
 /// larger radius, the number of circle split number is r * tot.
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Cylinder<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int tot_for_unit_cylinder, bool finalize_model = true);
+void generateBVHModel(BVHModel<BV>& model, const Cylinder<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int tot_for_unit_cylinder, FinalizeModel finalize_model = FinalizeModel::DO_FINALIZE);
 
 
 /// @brief Generate BVH model from cone, given the number of segments along circle and the number of segments along axis.
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Cone<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int tot, unsigned int h_num, bool finalize_model = true);
+void generateBVHModel(BVHModel<BV>& model, const Cone<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int tot, unsigned int h_num, FinalizeModel finalize_model = FinalizeModel::DO_FINALIZE);
 
 /// @brief Generate BVH model from cone
 /// Difference from generateBVHModel: is that it gives the circle split number tot for a cylinder with unit radius. For cone with
 /// larger radius, the number of circle split number is r * tot.
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Cone<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int tot_for_unit_cone, bool finalize_model = true);
+void generateBVHModel(BVHModel<BV>& model, const Cone<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int tot_for_unit_cone, FinalizeModel finalize_model = FinalizeModel::DO_FINALIZE);
 
 /**@} */ // end of doxygen group generateBVHModel
 
diff --git a/test/test_fcl_broadphase_distance.cpp b/test/test_fcl_broadphase_distance.cpp
index c450361c6..38acaa237 100644
--- a/test/test_fcl_broadphase_distance.cpp
+++ b/test/test_fcl_broadphase_distance.cpp
@@ -263,7 +263,7 @@ void generateSelfDistanceEnvironmentsMesh(std::vector<CollisionObject<S>*>& env,
 
     Sphere<S> sphere(single_size / 2);
     BVHModel<OBBRSS<S>>* model = new BVHModel<OBBRSS<S>>();
-    generateBVHModel(*model, sphere, Transform3<S>::Identity(), 16, 16, false);
+    generateBVHModel(*model, sphere, Transform3<S>::Identity(), 16, 16);
     env.push_back(new CollisionObject<S>(std::shared_ptr<CollisionGeometry<S>>(model),
                                       Transform3<S>(Translation3<S>(Vector3<S>(x * step_size + delta_size + 0.5 * single_size - env_scale,
                                                             y * step_size + delta_size + 0.5 * single_size - env_scale,
@@ -278,7 +278,7 @@ void generateSelfDistanceEnvironmentsMesh(std::vector<CollisionObject<S>*>& env,
 
     Ellipsoid<S> ellipsoid(single_size / 2, single_size / 2, single_size / 2);
     BVHModel<OBBRSS<S>>* model = new BVHModel<OBBRSS<S>>();
-    generateBVHModel(*model, ellipsoid, Transform3<S>::Identity(), 16, 16, false);
+    generateBVHModel(*model, ellipsoid, Transform3<S>::Identity(), 16, 16);
     env.push_back(new CollisionObject<S>(std::shared_ptr<CollisionGeometry<S>>(model),
                                       Transform3<S>(Translation3<S>(Vector3<S>(x * step_size + delta_size + 0.5 * single_size - env_scale,
                                                             y * step_size + delta_size + 0.5 * single_size - env_scale,
@@ -293,7 +293,7 @@ void generateSelfDistanceEnvironmentsMesh(std::vector<CollisionObject<S>*>& env,
 
     Cylinder<S> cylinder(single_size / 2, single_size);
     BVHModel<OBBRSS<S>>* model = new BVHModel<OBBRSS<S>>();
-    generateBVHModel(*model, cylinder, Transform3<S>::Identity(), 16, 16, false);
+    generateBVHModel(*model, cylinder, Transform3<S>::Identity(), 16, 16);
     env.push_back(new CollisionObject<S>(std::shared_ptr<CollisionGeometry<S>>(model),
                                       Transform3<S>(Translation3<S>(Vector3<S>(x * step_size + delta_size + 0.5 * single_size - env_scale,
                                                             y * step_size + delta_size + 0.5 * single_size - env_scale,
@@ -308,7 +308,7 @@ void generateSelfDistanceEnvironmentsMesh(std::vector<CollisionObject<S>*>& env,
 
     Cone<S> cone(single_size / 2, single_size);
     BVHModel<OBBRSS<S>>* model = new BVHModel<OBBRSS<S>>();
-    generateBVHModel(*model, cone, Transform3<S>::Identity(), 16, 16, false);
+    generateBVHModel(*model, cone, Transform3<S>::Identity(), 16, 16);
     env.push_back(new CollisionObject<S>(std::shared_ptr<CollisionGeometry<S>>(model),
                                       Transform3<S>(Translation3<S>(Vector3<S>(x * step_size + delta_size + 0.5 * single_size - env_scale,
                                                             y * step_size + delta_size + 0.5 * single_size - env_scale,
diff --git a/test/test_fcl_shape_mesh_consistency.cpp b/test/test_fcl_shape_mesh_consistency.cpp
index 77f86ae7e..f86ea0df8 100644
--- a/test/test_fcl_shape_mesh_consistency.cpp
+++ b/test/test_fcl_shape_mesh_consistency.cpp
@@ -63,8 +63,8 @@ void test_consistency_distance_spheresphere_libccd()
   BVHModel<RSS<S>> s1_rss;
   BVHModel<RSS<S>> s2_rss;
 
-  generateBVHModel(s1_rss, s1, Transform3<S>::Identity(), 16, 16, false);
-  generateBVHModel(s2_rss, s2, Transform3<S>::Identity(), 16, 16, false);
+  generateBVHModel(s1_rss, s1, Transform3<S>::Identity(), 16, 16);
+  generateBVHModel(s2_rss, s2, Transform3<S>::Identity(), 16, 16);
 
   DistanceRequest<S> request;
   DistanceResult<S> res, res1;
@@ -161,8 +161,8 @@ void test_consistency_distance_ellipsoidellipsoid_libccd()
   BVHModel<RSS<S>> s1_rss;
   BVHModel<RSS<S>> s2_rss;
 
-  generateBVHModel(s1_rss, s1, Transform3<S>::Identity(), 16, 16, false);
-  generateBVHModel(s2_rss, s2, Transform3<S>::Identity(), 16, 16, false);
+  generateBVHModel(s1_rss, s1, Transform3<S>::Identity(), 16, 16);
+  generateBVHModel(s2_rss, s2, Transform3<S>::Identity(), 16, 16);
 
   DistanceRequest<S> request;
   DistanceResult<S> res, res1;
@@ -357,8 +357,8 @@ void test_consistency_distance_cylindercylinder_libccd()
   BVHModel<RSS<S>> s1_rss;
   BVHModel<RSS<S>> s2_rss;
 
-  generateBVHModel(s1_rss, s1, Transform3<S>::Identity(), 16, 16, false);
-  generateBVHModel(s2_rss, s2, Transform3<S>::Identity(), 16, 16, false);
+  generateBVHModel(s1_rss, s1, Transform3<S>::Identity(), 16, 16);
+  generateBVHModel(s2_rss, s2, Transform3<S>::Identity(), 16, 16);
 
   DistanceRequest<S> request;
   DistanceResult<S> res, res1;
@@ -455,8 +455,8 @@ void test_consistency_distance_conecone_libccd()
   BVHModel<RSS<S>> s1_rss;
   BVHModel<RSS<S>> s2_rss;
 
-  generateBVHModel(s1_rss, s1, Transform3<S>::Identity(), 16, 16, false);
-  generateBVHModel(s2_rss, s2, Transform3<S>::Identity(), 16, 16, false);
+  generateBVHModel(s1_rss, s1, Transform3<S>::Identity(), 16, 16);
+  generateBVHModel(s2_rss, s2, Transform3<S>::Identity(), 16, 16);
 
   DistanceRequest<S> request;
   DistanceResult<S> res, res1;
@@ -551,8 +551,8 @@ void test_consistency_distance_spheresphere_GJK()
   BVHModel<RSS<S>> s1_rss;
   BVHModel<RSS<S>> s2_rss;
 
-  generateBVHModel(s1_rss, s1, Transform3<S>::Identity(), 16, 16, false);
-  generateBVHModel(s2_rss, s2, Transform3<S>::Identity(), 16, 16, false);
+  generateBVHModel(s1_rss, s1, Transform3<S>::Identity(), 16, 16);
+  generateBVHModel(s2_rss, s2, Transform3<S>::Identity(), 16, 16);
 
   DistanceRequest<S> request;
   request.gjk_solver_type = GST_INDEP;
@@ -651,8 +651,8 @@ void test_consistency_distance_ellipsoidellipsoid_GJK()
   BVHModel<RSS<S>> s1_rss;
   BVHModel<RSS<S>> s2_rss;
 
-  generateBVHModel(s1_rss, s1, Transform3<S>::Identity(), 16, 16, false);
-  generateBVHModel(s2_rss, s2, Transform3<S>::Identity(), 16, 16, false);
+  generateBVHModel(s1_rss, s1, Transform3<S>::Identity(), 16, 16);
+  generateBVHModel(s2_rss, s2, Transform3<S>::Identity(), 16, 16);
 
   DistanceRequest<S> request;
   request.gjk_solver_type = GST_INDEP;
@@ -851,8 +851,8 @@ void test_consistency_distance_cylindercylinder_GJK()
   BVHModel<RSS<S>> s1_rss;
   BVHModel<RSS<S>> s2_rss;
 
-  generateBVHModel(s1_rss, s1, Transform3<S>::Identity(), 16, 16, false);
-  generateBVHModel(s2_rss, s2, Transform3<S>::Identity(), 16, 16, false);
+  generateBVHModel(s1_rss, s1, Transform3<S>::Identity(), 16, 16);
+  generateBVHModel(s2_rss, s2, Transform3<S>::Identity(), 16, 16);
 
   DistanceRequest<S> request;
   request.gjk_solver_type = GST_INDEP;
@@ -959,8 +959,8 @@ void test_consistency_distance_conecone_GJK()
   BVHModel<RSS<S>> s1_rss;
   BVHModel<RSS<S>> s2_rss;
 
-  generateBVHModel(s1_rss, s1, Transform3<S>::Identity(), 16, 16, false);
-  generateBVHModel(s2_rss, s2, Transform3<S>::Identity(), 16, 16, false);
+  generateBVHModel(s1_rss, s1, Transform3<S>::Identity(), 16, 16);
+  generateBVHModel(s2_rss, s2, Transform3<S>::Identity(), 16, 16);
 
   DistanceRequest<S> request;
   request.gjk_solver_type = GST_INDEP;
@@ -1059,10 +1059,10 @@ void test_consistency_collision_spheresphere_libccd()
   BVHModel<OBB<S>> s1_obb;
   BVHModel<OBB<S>> s2_obb;
 
-  generateBVHModel(s1_aabb, s1, Transform3<S>::Identity(), 16, 16, false);
-  generateBVHModel(s2_aabb, s2, Transform3<S>::Identity(), 16, 16, false);
-  generateBVHModel(s1_obb, s1, Transform3<S>::Identity(), 16, 16, false);
-  generateBVHModel(s2_obb, s2, Transform3<S>::Identity(), 16, 16, false);
+  generateBVHModel(s1_aabb, s1, Transform3<S>::Identity(), 16, 16);
+  generateBVHModel(s2_aabb, s2, Transform3<S>::Identity(), 16, 16);
+  generateBVHModel(s1_obb, s1, Transform3<S>::Identity(), 16, 16);
+  generateBVHModel(s2_obb, s2, Transform3<S>::Identity(), 16, 16);
 
   CollisionRequest<S> request;
   CollisionResult<S> result;
@@ -1285,10 +1285,10 @@ void test_consistency_collision_ellipsoidellipsoid_libccd()
   BVHModel<OBB<S>> s1_obb;
   BVHModel<OBB<S>> s2_obb;
 
-  generateBVHModel(s1_aabb, s1, Transform3<S>::Identity(), 16, 16, false);
-  generateBVHModel(s2_aabb, s2, Transform3<S>::Identity(), 16, 16, false);
-  generateBVHModel(s1_obb, s1, Transform3<S>::Identity(), 16, 16, false);
-  generateBVHModel(s2_obb, s2, Transform3<S>::Identity(), 16, 16, false);
+  generateBVHModel(s1_aabb, s1, Transform3<S>::Identity(), 16, 16);
+  generateBVHModel(s2_aabb, s2, Transform3<S>::Identity(), 16, 16);
+  generateBVHModel(s1_obb, s1, Transform3<S>::Identity(), 16, 16);
+  generateBVHModel(s2_obb, s2, Transform3<S>::Identity(), 16, 16);
 
   CollisionRequest<S> request;
   CollisionResult<S> result;
@@ -1643,9 +1643,9 @@ void test_consistency_collision_spherebox_libccd()
   BVHModel<OBB<S>> s1_obb;
   BVHModel<OBB<S>> s2_obb;
 
-  generateBVHModel(s1_aabb, s1, Transform3<S>::Identity(), 16, 16, false);
+  generateBVHModel(s1_aabb, s1, Transform3<S>::Identity(), 16, 16);
   generateBVHModel(s2_aabb, s2, Transform3<S>::Identity());
-  generateBVHModel(s1_obb, s1, Transform3<S>::Identity(), 16, 16, false);
+  generateBVHModel(s1_obb, s1, Transform3<S>::Identity(), 16, 16);
   generateBVHModel(s2_obb, s2, Transform3<S>::Identity());
 
   CollisionRequest<S> request;
@@ -1772,10 +1772,10 @@ void test_consistency_collision_cylindercylinder_libccd()
   BVHModel<OBB<S>> s1_obb;
   BVHModel<OBB<S>> s2_obb;
 
-  generateBVHModel(s1_aabb, s1, Transform3<S>::Identity(), 16, 16, false);
-  generateBVHModel(s2_aabb, s2, Transform3<S>::Identity(), 16, 16, false);
-  generateBVHModel(s1_obb, s1, Transform3<S>::Identity(), 16, 16, false);
-  generateBVHModel(s2_obb, s2, Transform3<S>::Identity(), 16, 16, false);
+  generateBVHModel(s1_aabb, s1, Transform3<S>::Identity(), 16, 16);
+  generateBVHModel(s2_aabb, s2, Transform3<S>::Identity(), 16, 16);
+  generateBVHModel(s1_obb, s1, Transform3<S>::Identity(), 16, 16);
+  generateBVHModel(s2_obb, s2, Transform3<S>::Identity(), 16, 16);
 
   CollisionRequest<S> request;
   CollisionResult<S> result;
@@ -1868,10 +1868,10 @@ void test_consistency_collision_conecone_libccd()
   BVHModel<OBB<S>> s1_obb;
   BVHModel<OBB<S>> s2_obb;
 
-  generateBVHModel(s1_aabb, s1, Transform3<S>::Identity(), 16, 16, false);
-  generateBVHModel(s2_aabb, s2, Transform3<S>::Identity(), 16, 16, false);
-  generateBVHModel(s1_obb, s1, Transform3<S>::Identity(), 16, 16, false);
-  generateBVHModel(s2_obb, s2, Transform3<S>::Identity(), 16, 16, false);
+  generateBVHModel(s1_aabb, s1, Transform3<S>::Identity(), 16, 16);
+  generateBVHModel(s2_aabb, s2, Transform3<S>::Identity(), 16, 16);
+  generateBVHModel(s1_obb, s1, Transform3<S>::Identity(), 16, 16);
+  generateBVHModel(s2_obb, s2, Transform3<S>::Identity(), 16, 16);
 
   CollisionRequest<S> request;
   CollisionResult<S> result;
@@ -2027,10 +2027,10 @@ void test_consistency_collision_spheresphere_GJK()
   BVHModel<OBB<S>> s1_obb;
   BVHModel<OBB<S>> s2_obb;
 
-  generateBVHModel(s1_aabb, s1, Transform3<S>::Identity(), 16, 16, false);
-  generateBVHModel(s2_aabb, s2, Transform3<S>::Identity(), 16, 16, false);
-  generateBVHModel(s1_obb, s1, Transform3<S>::Identity(), 16, 16, false);
-  generateBVHModel(s2_obb, s2, Transform3<S>::Identity(), 16, 16, false);
+  generateBVHModel(s1_aabb, s1, Transform3<S>::Identity(), 16, 16);
+  generateBVHModel(s2_aabb, s2, Transform3<S>::Identity(), 16, 16);
+  generateBVHModel(s1_obb, s1, Transform3<S>::Identity(), 16, 16);
+  generateBVHModel(s2_obb, s2, Transform3<S>::Identity(), 16, 16);
 
   CollisionRequest<S> request;
   request.gjk_solver_type = GST_INDEP;
@@ -2255,10 +2255,10 @@ void test_consistency_collision_ellipsoidellipsoid_GJK()
   BVHModel<OBB<S>> s1_obb;
   BVHModel<OBB<S>> s2_obb;
 
-  generateBVHModel(s1_aabb, s1, Transform3<S>::Identity(), 16, 16, false);
-  generateBVHModel(s2_aabb, s2, Transform3<S>::Identity(), 16, 16, false);
-  generateBVHModel(s1_obb, s1, Transform3<S>::Identity(), 16, 16, false);
-  generateBVHModel(s2_obb, s2, Transform3<S>::Identity(), 16, 16, false);
+  generateBVHModel(s1_aabb, s1, Transform3<S>::Identity(), 16, 16);
+  generateBVHModel(s2_aabb, s2, Transform3<S>::Identity(), 16, 16);
+  generateBVHModel(s1_obb, s1, Transform3<S>::Identity(), 16, 16);
+  generateBVHModel(s2_obb, s2, Transform3<S>::Identity(), 16, 16);
 
   CollisionRequest<S> request;
   request.gjk_solver_type = GST_INDEP;
@@ -2615,9 +2615,9 @@ void test_consistency_collision_spherebox_GJK()
   BVHModel<OBB<S>> s1_obb;
   BVHModel<OBB<S>> s2_obb;
 
-  generateBVHModel(s1_aabb, s1, Transform3<S>::Identity(), 16, 16, false);
+  generateBVHModel(s1_aabb, s1, Transform3<S>::Identity(), 16, 16);
   generateBVHModel(s2_aabb, s2, Transform3<S>::Identity());
-  generateBVHModel(s1_obb, s1, Transform3<S>::Identity(), 16, 16, false);
+  generateBVHModel(s1_obb, s1, Transform3<S>::Identity(), 16, 16);
   generateBVHModel(s2_obb, s2, Transform3<S>::Identity());
 
   CollisionRequest<S> request;
@@ -2746,10 +2746,10 @@ void test_consistency_collision_cylindercylinder_GJK()
   BVHModel<OBB<S>> s1_obb;
   BVHModel<OBB<S>> s2_obb;
 
-  generateBVHModel(s1_aabb, s1, Transform3<S>::Identity(), 16, 16, false);
-  generateBVHModel(s2_aabb, s2, Transform3<S>::Identity(), 16, 16, false);
-  generateBVHModel(s1_obb, s1, Transform3<S>::Identity(), 16, 16, false);
-  generateBVHModel(s2_obb, s2, Transform3<S>::Identity(), 16, 16, false);
+  generateBVHModel(s1_aabb, s1, Transform3<S>::Identity(), 16, 16);
+  generateBVHModel(s2_aabb, s2, Transform3<S>::Identity(), 16, 16);
+  generateBVHModel(s1_obb, s1, Transform3<S>::Identity(), 16, 16);
+  generateBVHModel(s2_obb, s2, Transform3<S>::Identity(), 16, 16);
 
   CollisionRequest<S> request;
   request.gjk_solver_type = GST_INDEP;
@@ -2844,10 +2844,10 @@ void test_consistency_collision_conecone_GJK()
   BVHModel<OBB<S>> s1_obb;
   BVHModel<OBB<S>> s2_obb;
 
-  generateBVHModel(s1_aabb, s1, Transform3<S>::Identity(), 16, 16, false);
-  generateBVHModel(s2_aabb, s2, Transform3<S>::Identity(), 16, 16, false);
-  generateBVHModel(s1_obb, s1, Transform3<S>::Identity(), 16, 16, false);
-  generateBVHModel(s2_obb, s2, Transform3<S>::Identity(), 16, 16, false);
+  generateBVHModel(s1_aabb, s1, Transform3<S>::Identity(), 16, 16);
+  generateBVHModel(s2_aabb, s2, Transform3<S>::Identity(), 16, 16);
+  generateBVHModel(s1_obb, s1, Transform3<S>::Identity(), 16, 16);
+  generateBVHModel(s2_obb, s2, Transform3<S>::Identity(), 16, 16);
 
   CollisionRequest<S> request;
   request.gjk_solver_type = GST_INDEP;
diff --git a/test/test_fcl_utility.h b/test/test_fcl_utility.h
index 3c3884047..937b100ef 100644
--- a/test/test_fcl_utility.h
+++ b/test/test_fcl_utility.h
@@ -594,7 +594,7 @@ void generateEnvironmentsMesh(std::vector<CollisionObject<S>*>& env, S env_scale
   for(std::size_t i = 0; i < n; ++i)
   {
     BVHModel<OBBRSS<S>>* model = new BVHModel<OBBRSS<S>>();
-    generateBVHModel(*model, sphere, Transform3<S>::Identity(), 16, 16, false);
+    generateBVHModel(*model, sphere, Transform3<S>::Identity(), 16, 16);
     env.push_back(new CollisionObject<S>(std::shared_ptr<CollisionGeometry<S>>(model), transforms[i]));
   }
 
@@ -603,7 +603,7 @@ void generateEnvironmentsMesh(std::vector<CollisionObject<S>*>& env, S env_scale
   for(std::size_t i = 0; i < n; ++i)
   {
     BVHModel<OBBRSS<S>>* model = new BVHModel<OBBRSS<S>>();
-    generateBVHModel(*model, cylinder, Transform3<S>::Identity(), 16, 16, false);
+    generateBVHModel(*model, cylinder, Transform3<S>::Identity(), 16, 16);
     env.push_back(new CollisionObject<S>(std::shared_ptr<CollisionGeometry<S>>(model), transforms[i]));
   }
 }

From de29f8f4514fe3464a1374a3b26e24d595df4b72 Mon Sep 17 00:00:00 2001
From: Nico van Duijn <nico@auterion.com>
Date: Wed, 4 Jul 2018 14:41:33 +0200
Subject: [PATCH 05/24] Clearer naming of total triangle variable

In generateBVHModel for a cylinder and cone, total actually represents
the total number of triangles of the bottom or top plate, not
the total triangles of the entire primitive (as opposed to the other
generateBVHModel variants)
---
 .../geometric_shape_to_BVH_model-inl.h        | 70 +++++++++----------
 .../geometry/geometric_shape_to_BVH_model.h   |  8 +--
 2 files changed, 39 insertions(+), 39 deletions(-)

diff --git a/include/fcl/geometry/geometric_shape_to_BVH_model-inl.h b/include/fcl/geometry/geometric_shape_to_BVH_model-inl.h
index 701bab962..7852fdc79 100644
--- a/include/fcl/geometry/geometric_shape_to_BVH_model-inl.h
+++ b/include/fcl/geometry/geometric_shape_to_BVH_model-inl.h
@@ -277,7 +277,7 @@ void generateBVHModel(BVHModel<BV>& model, const Ellipsoid<typename BV::S>& shap
 
 //==============================================================================
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Cylinder<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int tot, unsigned int h_num, FinalizeModel finalize_model)
+void generateBVHModel(BVHModel<BV>& model, const Cylinder<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int circle_split_tot, unsigned int h_num, FinalizeModel finalize_model)
 {
   using S = typename BV::S;
 
@@ -288,45 +288,45 @@ void generateBVHModel(BVHModel<BV>& model, const Cylinder<typename BV::S>& shape
   S h = shape.lz;
   S phi, phid;
   const S pi = constants<S>::pi();
-  phid = pi * 2 / tot;
+  phid = pi * 2 / circle_split_tot;
   phi = 0;
 
   S hd = h / h_num;
 
-  for(unsigned int i = 0; i < tot; ++i)
+  for(unsigned int i = 0; i < circle_split_tot; ++i)
     points.emplace_back(r * cos(phi + phid * i), r * sin(phi + phid * i), h / 2);
 
   for(unsigned int i = 0; i < h_num - 1; ++i)
   {
-    for(unsigned int j = 0; j < tot; ++j)
+    for(unsigned int j = 0; j < circle_split_tot; ++j)
     {
       points.emplace_back(r * cos(phi + phid * j), r * sin(phi + phid * j), h / 2 - (i + 1) * hd);
     }
   }
 
-  for(unsigned int i = 0; i < tot; ++i)
+  for(unsigned int i = 0; i < circle_split_tot; ++i)
     points.emplace_back(r * cos(phi + phid * i), r * sin(phi + phid * i), - h / 2);
 
   points.emplace_back(0, 0, h / 2);
   points.emplace_back(0, 0, -h / 2);
 
-  for(unsigned int i = 0; i < tot; ++i)
-    tri_indices.emplace_back((h_num + 1) * tot, i, ((i == tot - 1) ? 0 : (i + 1)));
+  for(unsigned int i = 0; i < circle_split_tot; ++i)
+    tri_indices.emplace_back((h_num + 1) * circle_split_tot, i, ((i == circle_split_tot - 1) ? 0 : (i + 1)));
 
-  for(unsigned int i = 0; i < tot; ++i)
-    tri_indices.emplace_back((h_num + 1) * tot + 1, h_num * tot + ((i == tot - 1) ? 0 : (i + 1)), h_num * tot + i);
+  for(unsigned int i = 0; i < circle_split_tot; ++i)
+    tri_indices.emplace_back((h_num + 1) * circle_split_tot + 1, h_num * circle_split_tot + ((i == circle_split_tot - 1) ? 0 : (i + 1)), h_num * circle_split_tot + i);
 
   for(unsigned int i = 0; i < h_num; ++i)
   {
-    for(unsigned int j = 0; j < tot; ++j)
+    for(unsigned int j = 0; j < circle_split_tot; ++j)
     {
       int a, b, c, d;
       a = j;
-      b = (j == tot - 1) ? 0 : (j + 1);
-      c = j + tot;
-      d = (j == tot - 1) ? tot : (j + 1 + tot);
+      b = (j == circle_split_tot - 1) ? 0 : (j + 1);
+      c = j + circle_split_tot;
+      d = (j == circle_split_tot - 1) ? circle_split_tot : (j + 1 + circle_split_tot);
 
-      int start = i * tot;
+      int start = i * circle_split_tot;
       tri_indices.emplace_back(start + b, start + a, start + c);
       tri_indices.emplace_back(start + b, start + c, start + d);
     }
@@ -350,7 +350,7 @@ void generateBVHModel(BVHModel<BV>& model, const Cylinder<typename BV::S>& shape
 
 //==============================================================================
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Cylinder<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int tot_for_unit_cylinder, FinalizeModel finalize_model)
+void generateBVHModel(BVHModel<BV>& model, const Cylinder<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int circle_split_tot_for_unit_cylinder, FinalizeModel finalize_model)
 {
   using S = typename BV::S;
 
@@ -358,18 +358,18 @@ void generateBVHModel(BVHModel<BV>& model, const Cylinder<typename BV::S>& shape
   S h = shape.lz;
 
   const S pi = constants<S>::pi();
-  unsigned int tot = tot_for_unit_cylinder * r;
-  S phid = pi * 2 / tot;
+  unsigned int circle_split_tot = circle_split_tot_for_unit_cylinder * r;
+  S phid = pi * 2 / circle_split_tot;
 
   S circle_edge = phid * r;
   unsigned int h_num = ceil(h / circle_edge);
 
-  generateBVHModel(model, shape, pose, tot, h_num, finalize_model);
+  generateBVHModel(model, shape, pose, circle_split_tot, h_num, finalize_model);
 }
 
 //==============================================================================
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Cone<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int tot, unsigned int h_num, FinalizeModel finalize_model)
+void generateBVHModel(BVHModel<BV>& model, const Cone<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int circle_split_tot, unsigned int h_num, FinalizeModel finalize_model)
 {
   using S = typename BV::S;
 
@@ -381,7 +381,7 @@ void generateBVHModel(BVHModel<BV>& model, const Cone<typename BV::S>& shape, co
 
   S phi, phid;
   const S pi = constants<S>::pi();
-  phid = pi * 2 / tot;
+  phid = pi * 2 / circle_split_tot;
   phi = 0;
 
   S hd = h / h_num;
@@ -390,35 +390,35 @@ void generateBVHModel(BVHModel<BV>& model, const Cone<typename BV::S>& shape, co
   {
     S h_i = h / 2 - (i + 1) * hd;
     S rh = r * (0.5 - h_i / h);
-    for(unsigned int j = 0; j < tot; ++j)
+    for(unsigned int j = 0; j < circle_split_tot; ++j)
     {
       points.emplace_back(rh * cos(phi + phid * j), rh * sin(phi + phid * j), h_i);
     }
   }
 
-  for(unsigned int i = 0; i < tot; ++i)
+  for(unsigned int i = 0; i < circle_split_tot; ++i)
     points.emplace_back(r * cos(phi + phid * i), r * sin(phi + phid * i), - h / 2);
 
   points.emplace_back(0, 0, h / 2);
   points.emplace_back(0, 0, -h / 2);
 
-  for(unsigned int i = 0; i < tot; ++i)
-    tri_indices.emplace_back(h_num * tot, i, (i == tot - 1) ? 0 : (i + 1));
+  for(unsigned int i = 0; i < circle_split_tot; ++i)
+    tri_indices.emplace_back(h_num * circle_split_tot, i, (i == circle_split_tot - 1) ? 0 : (i + 1));
 
-  for(unsigned int i = 0; i < tot; ++i)
-    tri_indices.emplace_back(h_num * tot + 1, (h_num - 1) * tot + ((i == tot - 1) ? 0 : (i + 1)), (h_num - 1) * tot + i);
+  for(unsigned int i = 0; i < circle_split_tot; ++i)
+    tri_indices.emplace_back(h_num * circle_split_tot + 1, (h_num - 1) * circle_split_tot + ((i == circle_split_tot - 1) ? 0 : (i + 1)), (h_num - 1) * circle_split_tot + i);
 
   for(unsigned int i = 0; i < h_num - 1; ++i)
   {
-    for(unsigned int j = 0; j < tot; ++j)
+    for(unsigned int j = 0; j < circle_split_tot; ++j)
     {
       int a, b, c, d;
       a = j;
-      b = (j == tot - 1) ? 0 : (j + 1);
-      c = j + tot;
-      d = (j == tot - 1) ? tot : (j + 1 + tot);
+      b = (j == circle_split_tot - 1) ? 0 : (j + 1);
+      c = j + circle_split_tot;
+      d = (j == circle_split_tot - 1) ? circle_split_tot : (j + 1 + circle_split_tot);
 
-      int start = i * tot;
+      int start = i * circle_split_tot;
       tri_indices.emplace_back(start + b, start + a, start + c);
       tri_indices.emplace_back(start + b, start + c, start + d);
     }
@@ -442,7 +442,7 @@ void generateBVHModel(BVHModel<BV>& model, const Cone<typename BV::S>& shape, co
 
 //==============================================================================
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Cone<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int tot_for_unit_cone, FinalizeModel finalize_model)
+void generateBVHModel(BVHModel<BV>& model, const Cone<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int circle_split_tot_for_unit_cylinder, FinalizeModel finalize_model)
 {
   using S = typename BV::S;
 
@@ -450,13 +450,13 @@ void generateBVHModel(BVHModel<BV>& model, const Cone<typename BV::S>& shape, co
   S h = shape.lz;
 
   const S pi = constants<S>::pi();
-  unsigned int tot = tot_for_unit_cone * r;
-  S phid = pi * 2 / tot;
+  unsigned int circle_split_tot = circle_split_tot_for_unit_cylinder * r;
+  S phid = pi * 2 / circle_split_tot;
 
   S circle_edge = phid * r;
   unsigned int h_num = ceil(h / circle_edge);
 
-  generateBVHModel(model, shape, pose, tot, h_num, finalize_model);
+  generateBVHModel(model, shape, pose, circle_split_tot, h_num, finalize_model);
 }
 
 } // namespace fcl
diff --git a/include/fcl/geometry/geometric_shape_to_BVH_model.h b/include/fcl/geometry/geometric_shape_to_BVH_model.h
index bf977ee20..30683a060 100644
--- a/include/fcl/geometry/geometric_shape_to_BVH_model.h
+++ b/include/fcl/geometry/geometric_shape_to_BVH_model.h
@@ -97,24 +97,24 @@ void generateBVHModel(BVHModel<BV>& model, const Ellipsoid<typename BV::S>& shap
 
 /// @brief Generate BVH model from cylinder, given the number of segments along circle and the number of segments along axis.
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Cylinder<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int tot, unsigned int h_num, FinalizeModel finalize_model = FinalizeModel::DO_FINALIZE);
+void generateBVHModel(BVHModel<BV>& model, const Cylinder<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int circle_split_tot, unsigned int h_num, FinalizeModel finalize_model = FinalizeModel::DO_FINALIZE);
 
 /// @brief Generate BVH model from cylinder
 /// Difference from generateBVHModel: is that it gives the circle split number tot for a cylinder with unit radius. For cylinder with
 /// larger radius, the number of circle split number is r * tot.
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Cylinder<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int tot_for_unit_cylinder, FinalizeModel finalize_model = FinalizeModel::DO_FINALIZE);
+void generateBVHModel(BVHModel<BV>& model, const Cylinder<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int circle_split_tot_for_unit_cylinder, FinalizeModel finalize_model = FinalizeModel::DO_FINALIZE);
 
 
 /// @brief Generate BVH model from cone, given the number of segments along circle and the number of segments along axis.
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Cone<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int tot, unsigned int h_num, FinalizeModel finalize_model = FinalizeModel::DO_FINALIZE);
+void generateBVHModel(BVHModel<BV>& model, const Cone<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int circle_split_tot, unsigned int h_num, FinalizeModel finalize_model = FinalizeModel::DO_FINALIZE);
 
 /// @brief Generate BVH model from cone
 /// Difference from generateBVHModel: is that it gives the circle split number tot for a cylinder with unit radius. For cone with
 /// larger radius, the number of circle split number is r * tot.
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Cone<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int tot_for_unit_cone, FinalizeModel finalize_model = FinalizeModel::DO_FINALIZE);
+void generateBVHModel(BVHModel<BV>& model, const Cone<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int circle_split_tot_for_unit_cylinder, FinalizeModel finalize_model = FinalizeModel::DO_FINALIZE);
 
 /**@} */ // end of doxygen group generateBVHModel
 

From 5184591ae72a3e642315222576a7c7ec7b48b8ce Mon Sep 17 00:00:00 2001
From: Nico van Duijn <nico@auterion.com>
Date: Wed, 4 Jul 2018 16:15:43 +0200
Subject: [PATCH 06/24] Add unit tests for generateBVHModel

---
 test/CMakeLists.txt                  |   1 +
 test/test_fcl_generate_bvh_model.cpp | 190 +++++++++++++++++++++++++++
 2 files changed, 191 insertions(+)
 create mode 100644 test/test_fcl_generate_bvh_model.cpp

diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index cc599b137..549ab2a4a 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -52,6 +52,7 @@ set(tests
     test_fcl_distance.cpp
     test_fcl_frontlist.cpp
     test_fcl_general.cpp
+    test_fcl_generate_bvh_model.cpp
     test_fcl_geometric_shapes.cpp
     test_fcl_math.cpp
     test_fcl_profiler.cpp
diff --git a/test/test_fcl_generate_bvh_model.cpp b/test/test_fcl_generate_bvh_model.cpp
new file mode 100644
index 000000000..747bb8388
--- /dev/null
+++ b/test/test_fcl_generate_bvh_model.cpp
@@ -0,0 +1,190 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2016, Open Source Robotics Foundation
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Open Source Robotics Foundation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** @author Nico van Duijn */
+
+#include <gtest/gtest.h>
+
+#include "fcl/config.h"
+#include "fcl/geometry/geometric_shape_to_BVH_model.h"
+#include "test_fcl_utility.h"
+#include <iostream>
+
+using namespace fcl;
+
+template<typename BV>
+void testBVHModelFromBox()
+{
+  using S = typename BV::S;
+
+  std::shared_ptr<BVHModel<BV> > model(new BVHModel<BV>);
+  Box<S> box(1.0, 1.0, 1.0);
+
+  generateBVHModel(*model, box, Transform3<S>::Identity(), FinalizeModel::DONT_FINALIZE);
+  EXPECT_EQ(model->num_vertices, 8);
+  EXPECT_EQ(model->num_tris, 12);
+  EXPECT_EQ(model->build_state, BVH_BUILD_STATE_BEGUN);
+
+  generateBVHModel(*model, box, Transform3<S>(Translation3<S>(Vector3<S>(2.0, 2.0, 2.0))));
+  EXPECT_EQ(model->num_vertices, 16);
+  EXPECT_EQ(model->num_tris, 24);
+  EXPECT_EQ(model->build_state, BVH_BUILD_STATE_PROCESSED);
+
+}
+
+template<typename BV>
+void testBVHModelFromSphere()
+{
+  using S = typename BV::S;
+
+  std::shared_ptr<BVHModel<BV> > model(new BVHModel<BV>);
+  Sphere<S> sphere(1.0);
+
+  // Testing the overload with num_faces defined ends up in a call to both
+  generateBVHModel(*model, sphere, Transform3<S>::Identity(), 32, FinalizeModel::DONT_FINALIZE);
+  EXPECT_EQ(model->num_vertices, 18);
+  EXPECT_EQ(model->num_tris, 32);
+  EXPECT_EQ(model->build_state, BVH_BUILD_STATE_BEGUN);
+
+  generateBVHModel(*model, sphere, Transform3<S>(Translation3<S>(Vector3<S>(2.0, 2.0, 2.0))), 32);
+  EXPECT_EQ(model->num_vertices, 36);
+  EXPECT_EQ(model->num_tris, 64);
+  EXPECT_EQ(model->build_state, BVH_BUILD_STATE_PROCESSED);
+
+}
+
+template<typename BV>
+void testBVHModelFromEllipsoid()
+{
+  using S = typename BV::S;
+
+  std::shared_ptr<BVHModel<BV> > model(new BVHModel<BV>);
+  Ellipsoid<S> ellipsoid(1.0, 1.0, 1.0);
+
+  // Testing the overload with num_faces defined ends up in a call to both
+  generateBVHModel(*model, ellipsoid, Transform3<S>::Identity(), 32, FinalizeModel::DONT_FINALIZE);
+  EXPECT_EQ(model->num_vertices, 18);
+  EXPECT_EQ(model->num_tris, 32);
+  EXPECT_EQ(model->build_state, BVH_BUILD_STATE_BEGUN);
+
+  generateBVHModel(*model, ellipsoid, Transform3<S>(Translation3<S>(Vector3<S>(2.0, 2.0, 2.0))), 32);
+  EXPECT_EQ(model->num_vertices, 36);
+  EXPECT_EQ(model->num_tris, 64);
+  EXPECT_EQ(model->build_state, BVH_BUILD_STATE_PROCESSED);
+
+}
+
+template<typename BV>
+void testBVHModelFromCylinder()
+{
+  using S = typename BV::S;
+
+  std::shared_ptr<BVHModel<BV> > model(new BVHModel<BV>);
+  Cylinder<S> cylinder(1.0, 1.0);
+
+  // Testing the overload with num_faces defined ends up in a call to both
+  generateBVHModel(*model, cylinder, Transform3<S>::Identity(), 8, FinalizeModel::DONT_FINALIZE);
+
+  EXPECT_EQ(model->num_vertices, 26);
+  EXPECT_EQ(model->num_tris, 48);
+  EXPECT_EQ(model->build_state, BVH_BUILD_STATE_BEGUN);
+
+  generateBVHModel(*model, cylinder, Transform3<S>(Translation3<S>(Vector3<S>(2.0, 2.0, 2.0))), 8);
+  EXPECT_EQ(model->num_vertices, 52);
+  EXPECT_EQ(model->num_tris, 96);
+  EXPECT_EQ(model->build_state, BVH_BUILD_STATE_PROCESSED);
+
+}
+
+template<typename BV>
+void testBVHModelFromCone()
+{
+
+  using S = typename BV::S;
+  const S pi = constants<S>::pi();
+
+  // Try various resolutions, radii and heights
+  for(uint8_t n = 4; n <= 32; n+=3){
+    for(S r = 0.5; r <= 5.0; r+=0.8){
+      for(S h = 0.5; h <= 5.0; h+=0.8){
+        unsigned int n_tot = n * r;
+        unsigned int h_num = ceil(h / ((pi * 2 / n_tot) * r));
+
+        std::shared_ptr<BVHModel<BV> > model(new BVHModel<BV>);
+        Cone<S> cone(r,h);
+
+        // Testing the overload with num_faces defined ends up in a call to both
+        generateBVHModel(*model, cone, Transform3<S>::Identity(), n, FinalizeModel::DONT_FINALIZE);
+        EXPECT_EQ(model->num_vertices, 2+n_tot*h_num);
+        EXPECT_EQ(model->num_tris, 2*n_tot*h_num); 
+        EXPECT_EQ(model->build_state, BVH_BUILD_STATE_BEGUN);
+ 
+        generateBVHModel(*model, cone, Transform3<S>(Translation3<S>(Vector3<S>(2.0, 2.0, 2.0))), n);
+        EXPECT_EQ(model->num_vertices, 2*(2+n_tot*h_num));
+        EXPECT_EQ(model->num_tris, 4*n_tot*h_num);
+        EXPECT_EQ(model->build_state, BVH_BUILD_STATE_PROCESSED);
+      }
+    }
+  }
+}
+
+template<typename BV>
+void testBVHModelFromPrimitives()
+{
+  testBVHModelFromBox<BV>();
+  testBVHModelFromSphere<BV>();
+  testBVHModelFromEllipsoid<BV>();
+  testBVHModelFromCylinder<BV>();
+  testBVHModelFromCone<BV>();
+}
+
+GTEST_TEST(FCL_GENERATE_BVH_MODELS, generating_bvh_models_from_primitives)
+{
+
+  testBVHModelFromPrimitives<AABB<double>>();
+  testBVHModelFromPrimitives<OBB<double>>();
+  testBVHModelFromPrimitives<RSS<double>>();
+  testBVHModelFromPrimitives<kIOS<double>>();
+  testBVHModelFromPrimitives<OBBRSS<double>>();
+  testBVHModelFromPrimitives<KDOP<double, 16> >();
+  testBVHModelFromPrimitives<KDOP<double, 18> >();
+  testBVHModelFromPrimitives<KDOP<double, 24> >();
+}
+
+//==============================================================================
+int main(int argc, char* argv[])
+{
+  ::testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}

From 72a7dd69bc35c0dec771a8dfd324d46c1eed1847 Mon Sep 17 00:00:00 2001
From: Nico van Duijn <nico@auterion.com>
Date: Wed, 4 Jul 2018 16:27:22 +0200
Subject: [PATCH 07/24] Remove some hardcoding in unit test

---
 test/CMakeLists.txt                  |  2 +-
 test/test_fcl_generate_bvh_model.cpp | 33 ++++++++++++++++++----------
 2 files changed, 23 insertions(+), 12 deletions(-)

diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 549ab2a4a..e99b46080 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -37,6 +37,7 @@ target_link_libraries(test_fcl_utility fcl)
 
 # test file list
 set(tests
+    test_fcl_generate_bvh_model.cpp
     test_fcl_auto_diff.cpp
     test_fcl_box_box.cpp
     test_fcl_broadphase_collision_1.cpp
@@ -52,7 +53,6 @@ set(tests
     test_fcl_distance.cpp
     test_fcl_frontlist.cpp
     test_fcl_general.cpp
-    test_fcl_generate_bvh_model.cpp
     test_fcl_geometric_shapes.cpp
     test_fcl_math.cpp
     test_fcl_profiler.cpp
diff --git a/test/test_fcl_generate_bvh_model.cpp b/test/test_fcl_generate_bvh_model.cpp
index 747bb8388..6eb09e8e3 100644
--- a/test/test_fcl_generate_bvh_model.cpp
+++ b/test/test_fcl_generate_bvh_model.cpp
@@ -109,21 +109,32 @@ template<typename BV>
 void testBVHModelFromCylinder()
 {
   using S = typename BV::S;
+  const S pi = constants<S>::pi();
 
-  std::shared_ptr<BVHModel<BV> > model(new BVHModel<BV>);
-  Cylinder<S> cylinder(1.0, 1.0);
+  // Try various resolutions, radii and heights
+  for(uint8_t n = 4; n <= 32; n+=3){
+    for(S r = 0.5; r <= 5.0; r+=0.8){
+      for(S h = 0.5; h <= 5.0; h+=0.8){
+        unsigned int n_tot = n * r;
+        unsigned int h_num = ceil(h / ((pi * 2 / n_tot) * r));
 
-  // Testing the overload with num_faces defined ends up in a call to both
-  generateBVHModel(*model, cylinder, Transform3<S>::Identity(), 8, FinalizeModel::DONT_FINALIZE);
+        std::shared_ptr<BVHModel<BV> > model(new BVHModel<BV>);
+        Cylinder<S> cylinder(r, h);
 
-  EXPECT_EQ(model->num_vertices, 26);
-  EXPECT_EQ(model->num_tris, 48);
-  EXPECT_EQ(model->build_state, BVH_BUILD_STATE_BEGUN);
+        // Testing the overload with num_faces defined ends up in a call to both
+        generateBVHModel(*model, cylinder, Transform3<S>::Identity(), n, FinalizeModel::DONT_FINALIZE);
 
-  generateBVHModel(*model, cylinder, Transform3<S>(Translation3<S>(Vector3<S>(2.0, 2.0, 2.0))), 8);
-  EXPECT_EQ(model->num_vertices, 52);
-  EXPECT_EQ(model->num_tris, 96);
-  EXPECT_EQ(model->build_state, BVH_BUILD_STATE_PROCESSED);
+     //   EXPECT_EQ(model->num_vertices, 2+n_tot*h_num);
+        EXPECT_EQ(model->num_tris, (2*h_num+2)*n_tot);
+     //   EXPECT_EQ(model->build_state, BVH_BUILD_STATE_BEGUN);
+
+        generateBVHModel(*model, cylinder, Transform3<S>(Translation3<S>(Vector3<S>(2.0, 2.0, 2.0))), 8);
+     //   EXPECT_EQ(model->num_vertices, 52);
+     //   EXPECT_EQ(model->num_tris, 96);
+     //   EXPECT_EQ(model->build_state, BVH_BUILD_STATE_PROCESSED);
+      }
+    }
+  }
 
 }
 

From 0ba78151c5bf339b9cd78914050f4b984530d092 Mon Sep 17 00:00:00 2001
From: Nico van Duijn <nico@auterion.com>
Date: Thu, 5 Jul 2018 08:28:12 +0200
Subject: [PATCH 08/24] Remove hardcoding in testEllipsoid

---
 test/test_fcl_generate_bvh_model.cpp | 69 ++++++++++++++++++----------
 1 file changed, 45 insertions(+), 24 deletions(-)

diff --git a/test/test_fcl_generate_bvh_model.cpp b/test/test_fcl_generate_bvh_model.cpp
index 6eb09e8e3..ceb8e5600 100644
--- a/test/test_fcl_generate_bvh_model.cpp
+++ b/test/test_fcl_generate_bvh_model.cpp
@@ -88,20 +88,41 @@ template<typename BV>
 void testBVHModelFromEllipsoid()
 {
   using S = typename BV::S;
+  const S p = 1.6075;
+
+// Test various radii
+for(S a = 0.5; a <= 2.1; a+=0.8){
+  for(S b = 0.5; b <= 2.1; b+=0.8){
+    for(S c = 0.5; c <= 2.1; c+=0.8){
+      Ellipsoid<S> ellipsoid(a, b, c);
+      const S& ap = std::pow(a, p);
+      const S& bp = std::pow(b, p);
+      const S& cp = std::pow(c, p);
+      const S ratio = std::pow((ap * bp + bp * cp + cp * ap) / 3.0, 1.0 / p);
+
+      // Test various resolutions
+      for(uint8_t n = 4; n <= 32; n+=8){
+        const S n_low_bound = std::sqrt(n / 2.0) * ratio;
+        const unsigned int ring = std::ceil(n_low_bound);
+        std::shared_ptr<BVHModel<BV> > model(new BVHModel<BV>);
 
-  std::shared_ptr<BVHModel<BV> > model(new BVHModel<BV>);
-  Ellipsoid<S> ellipsoid(1.0, 1.0, 1.0);
-
-  // Testing the overload with num_faces defined ends up in a call to both
-  generateBVHModel(*model, ellipsoid, Transform3<S>::Identity(), 32, FinalizeModel::DONT_FINALIZE);
-  EXPECT_EQ(model->num_vertices, 18);
-  EXPECT_EQ(model->num_tris, 32);
-  EXPECT_EQ(model->build_state, BVH_BUILD_STATE_BEGUN);
+        // Testing the overload with num_faces defined ends up in a call to both
+        generateBVHModel(*model, ellipsoid, Transform3<S>::Identity(), n, FinalizeModel::DONT_FINALIZE);
+        EXPECT_EQ(model->num_vertices, 2+ring*ring);
+        EXPECT_EQ(model->num_tris, 2*ring*ring);
+        EXPECT_EQ(model->build_state, BVH_BUILD_STATE_BEGUN);
 
-  generateBVHModel(*model, ellipsoid, Transform3<S>(Translation3<S>(Vector3<S>(2.0, 2.0, 2.0))), 32);
-  EXPECT_EQ(model->num_vertices, 36);
-  EXPECT_EQ(model->num_tris, 64);
-  EXPECT_EQ(model->build_state, BVH_BUILD_STATE_PROCESSED);
+        // Make sure we can add another ellipsoid to the model
+        generateBVHModel(*model, ellipsoid, Transform3<S>(Translation3<S>(Vector3<S>(2.0, 2.0, 2.0))), n);
+        EXPECT_EQ(model->num_vertices, 2*(2+ring*ring));
+        EXPECT_EQ(model->num_tris, 2*(2*ring*ring));
+        EXPECT_EQ(model->build_state, BVH_BUILD_STATE_PROCESSED);
+      }
+    }
+  }
+}
+  
+  
 
 }
 
@@ -112,9 +133,9 @@ void testBVHModelFromCylinder()
   const S pi = constants<S>::pi();
 
   // Try various resolutions, radii and heights
-  for(uint8_t n = 4; n <= 32; n+=3){
-    for(S r = 0.5; r <= 5.0; r+=0.8){
-      for(S h = 0.5; h <= 5.0; h+=0.8){
+  for(uint8_t n = 4; n <= 32; n+=8){
+    for(S r = 0.5; r <= 2.1; r+=0.8){
+      for(S h = 0.5; h <= 2.1; h+=0.8){
         unsigned int n_tot = n * r;
         unsigned int h_num = ceil(h / ((pi * 2 / n_tot) * r));
 
@@ -124,14 +145,14 @@ void testBVHModelFromCylinder()
         // Testing the overload with num_faces defined ends up in a call to both
         generateBVHModel(*model, cylinder, Transform3<S>::Identity(), n, FinalizeModel::DONT_FINALIZE);
 
-     //   EXPECT_EQ(model->num_vertices, 2+n_tot*h_num);
+        EXPECT_EQ(model->num_vertices, 2+n_tot*(h_num+1));
         EXPECT_EQ(model->num_tris, (2*h_num+2)*n_tot);
-     //   EXPECT_EQ(model->build_state, BVH_BUILD_STATE_BEGUN);
+        EXPECT_EQ(model->build_state, BVH_BUILD_STATE_BEGUN);
 
-        generateBVHModel(*model, cylinder, Transform3<S>(Translation3<S>(Vector3<S>(2.0, 2.0, 2.0))), 8);
-     //   EXPECT_EQ(model->num_vertices, 52);
-     //   EXPECT_EQ(model->num_tris, 96);
-     //   EXPECT_EQ(model->build_state, BVH_BUILD_STATE_PROCESSED);
+        generateBVHModel(*model, cylinder, Transform3<S>(Translation3<S>(Vector3<S>(2.0, 2.0, 2.0))), n);
+        EXPECT_EQ(model->num_vertices, 2*( 2+n_tot*(h_num+1)));
+        EXPECT_EQ(model->num_tris, 2*((2*h_num+2)*n_tot));
+        EXPECT_EQ(model->build_state, BVH_BUILD_STATE_PROCESSED);
       }
     }
   }
@@ -146,9 +167,9 @@ void testBVHModelFromCone()
   const S pi = constants<S>::pi();
 
   // Try various resolutions, radii and heights
-  for(uint8_t n = 4; n <= 32; n+=3){
-    for(S r = 0.5; r <= 5.0; r+=0.8){
-      for(S h = 0.5; h <= 5.0; h+=0.8){
+  for(uint8_t n = 4; n <= 32; n+=8){
+    for(S r = 0.5; r <= 2.1; r+=0.8){
+      for(S h = 0.5; h <= 2.1; h+=0.8){
         unsigned int n_tot = n * r;
         unsigned int h_num = ceil(h / ((pi * 2 / n_tot) * r));
 

From 5f407e90f21f160b159fadf7dd2f0c93e6d6992a Mon Sep 17 00:00:00 2001
From: Nico van Duijn <nico@auterion.com>
Date: Thu, 5 Jul 2018 08:42:15 +0200
Subject: [PATCH 09/24] Remove hardcoding in testSphere

---
 test/test_fcl_generate_bvh_model.cpp | 78 ++++++++++++++++------------
 1 file changed, 44 insertions(+), 34 deletions(-)

diff --git a/test/test_fcl_generate_bvh_model.cpp b/test/test_fcl_generate_bvh_model.cpp
index ceb8e5600..8239626a1 100644
--- a/test/test_fcl_generate_bvh_model.cpp
+++ b/test/test_fcl_generate_bvh_model.cpp
@@ -68,20 +68,30 @@ void testBVHModelFromSphere()
 {
   using S = typename BV::S;
 
-  std::shared_ptr<BVHModel<BV> > model(new BVHModel<BV>);
-  Sphere<S> sphere(1.0);
-
-  // Testing the overload with num_faces defined ends up in a call to both
-  generateBVHModel(*model, sphere, Transform3<S>::Identity(), 32, FinalizeModel::DONT_FINALIZE);
-  EXPECT_EQ(model->num_vertices, 18);
-  EXPECT_EQ(model->num_tris, 32);
-  EXPECT_EQ(model->build_state, BVH_BUILD_STATE_BEGUN);
+  // Test various radii
+  for(S r = 0.5; r <= 2.1; r += 0.8){
+    Sphere<S> sphere(r);
 
-  generateBVHModel(*model, sphere, Transform3<S>(Translation3<S>(Vector3<S>(2.0, 2.0, 2.0))), 32);
-  EXPECT_EQ(model->num_vertices, 36);
-  EXPECT_EQ(model->num_tris, 64);
-  EXPECT_EQ(model->build_state, BVH_BUILD_STATE_PROCESSED);
+    // Test various resolutions
+    for(uint8_t n = 4; n <= 32; n += 8){
+      S n_low_bound = sqrtf(n / 2.0) * r * r;
+      unsigned int ring = ceil(n_low_bound);
 
+      std::shared_ptr<BVHModel<BV> > model(new BVHModel<BV>);
+  
+      // Testing the overload with num_faces defined ends up in a call to both
+      generateBVHModel(*model, sphere, Transform3<S>::Identity(), n, FinalizeModel::DONT_FINALIZE);
+      EXPECT_EQ(model->num_vertices, 2 + ring * ring);
+      EXPECT_EQ(model->num_tris, 2 * ring * ring);
+      EXPECT_EQ(model->build_state, BVH_BUILD_STATE_BEGUN);
+
+      // Test that we can add another sphere to the model
+      generateBVHModel(*model, sphere, Transform3<S>(Translation3<S>(Vector3<S>(2.0, 2.0, 2.0))), n);
+      EXPECT_EQ(model->num_vertices, 2 * ( 2 + ring * ring));
+      EXPECT_EQ(model->num_tris, 2 * (2 * ring * ring));
+      EXPECT_EQ(model->build_state, BVH_BUILD_STATE_PROCESSED);
+    }
+  }
 }
 
 template<typename BV>
@@ -91,9 +101,9 @@ void testBVHModelFromEllipsoid()
   const S p = 1.6075;
 
 // Test various radii
-for(S a = 0.5; a <= 2.1; a+=0.8){
-  for(S b = 0.5; b <= 2.1; b+=0.8){
-    for(S c = 0.5; c <= 2.1; c+=0.8){
+for(S a = 0.5; a <= 2.1; a += 0.8){
+  for(S b = 0.5; b <= 2.1; b += 0.8){
+    for(S c = 0.5; c <= 2.1; c += 0.8){
       Ellipsoid<S> ellipsoid(a, b, c);
       const S& ap = std::pow(a, p);
       const S& bp = std::pow(b, p);
@@ -101,21 +111,21 @@ for(S a = 0.5; a <= 2.1; a+=0.8){
       const S ratio = std::pow((ap * bp + bp * cp + cp * ap) / 3.0, 1.0 / p);
 
       // Test various resolutions
-      for(uint8_t n = 4; n <= 32; n+=8){
+      for(uint8_t n = 4; n <= 32; n += 8){
         const S n_low_bound = std::sqrt(n / 2.0) * ratio;
         const unsigned int ring = std::ceil(n_low_bound);
         std::shared_ptr<BVHModel<BV> > model(new BVHModel<BV>);
 
         // Testing the overload with num_faces defined ends up in a call to both
         generateBVHModel(*model, ellipsoid, Transform3<S>::Identity(), n, FinalizeModel::DONT_FINALIZE);
-        EXPECT_EQ(model->num_vertices, 2+ring*ring);
-        EXPECT_EQ(model->num_tris, 2*ring*ring);
+        EXPECT_EQ(model->num_vertices, 2 + ring * ring);
+        EXPECT_EQ(model->num_tris, 2 * ring * ring);
         EXPECT_EQ(model->build_state, BVH_BUILD_STATE_BEGUN);
 
         // Make sure we can add another ellipsoid to the model
         generateBVHModel(*model, ellipsoid, Transform3<S>(Translation3<S>(Vector3<S>(2.0, 2.0, 2.0))), n);
-        EXPECT_EQ(model->num_vertices, 2*(2+ring*ring));
-        EXPECT_EQ(model->num_tris, 2*(2*ring*ring));
+        EXPECT_EQ(model->num_vertices, 2 * ( 2 + ring * ring));
+        EXPECT_EQ(model->num_tris, 2 * (2 * ring * ring));
         EXPECT_EQ(model->build_state, BVH_BUILD_STATE_PROCESSED);
       }
     }
@@ -133,9 +143,9 @@ void testBVHModelFromCylinder()
   const S pi = constants<S>::pi();
 
   // Try various resolutions, radii and heights
-  for(uint8_t n = 4; n <= 32; n+=8){
-    for(S r = 0.5; r <= 2.1; r+=0.8){
-      for(S h = 0.5; h <= 2.1; h+=0.8){
+  for(uint8_t n = 4; n <= 32; n += 8){
+    for(S r = 0.5; r <= 2.1; r += 0.8){
+      for(S h = 0.5; h <= 2.1; h += 0.8){
         unsigned int n_tot = n * r;
         unsigned int h_num = ceil(h / ((pi * 2 / n_tot) * r));
 
@@ -145,13 +155,13 @@ void testBVHModelFromCylinder()
         // Testing the overload with num_faces defined ends up in a call to both
         generateBVHModel(*model, cylinder, Transform3<S>::Identity(), n, FinalizeModel::DONT_FINALIZE);
 
-        EXPECT_EQ(model->num_vertices, 2+n_tot*(h_num+1));
-        EXPECT_EQ(model->num_tris, (2*h_num+2)*n_tot);
+        EXPECT_EQ(model->num_vertices, 2 + n_tot * (h_num + 1));
+        EXPECT_EQ(model->num_tris, (2 * h_num + 2) * n_tot);
         EXPECT_EQ(model->build_state, BVH_BUILD_STATE_BEGUN);
 
         generateBVHModel(*model, cylinder, Transform3<S>(Translation3<S>(Vector3<S>(2.0, 2.0, 2.0))), n);
-        EXPECT_EQ(model->num_vertices, 2*( 2+n_tot*(h_num+1)));
-        EXPECT_EQ(model->num_tris, 2*((2*h_num+2)*n_tot));
+        EXPECT_EQ(model->num_vertices, 2 * (2 + n_tot * (h_num + 1)));
+        EXPECT_EQ(model->num_tris, 2 * ((2 * h_num + 2) * n_tot));
         EXPECT_EQ(model->build_state, BVH_BUILD_STATE_PROCESSED);
       }
     }
@@ -167,9 +177,9 @@ void testBVHModelFromCone()
   const S pi = constants<S>::pi();
 
   // Try various resolutions, radii and heights
-  for(uint8_t n = 4; n <= 32; n+=8){
-    for(S r = 0.5; r <= 2.1; r+=0.8){
-      for(S h = 0.5; h <= 2.1; h+=0.8){
+  for(uint8_t n = 4; n <= 32; n += 8){
+    for(S r = 0.5; r <= 2.1; r += 0.8){
+      for(S h = 0.5; h <= 2.1; h += 0.8){
         unsigned int n_tot = n * r;
         unsigned int h_num = ceil(h / ((pi * 2 / n_tot) * r));
 
@@ -178,13 +188,13 @@ void testBVHModelFromCone()
 
         // Testing the overload with num_faces defined ends up in a call to both
         generateBVHModel(*model, cone, Transform3<S>::Identity(), n, FinalizeModel::DONT_FINALIZE);
-        EXPECT_EQ(model->num_vertices, 2+n_tot*h_num);
-        EXPECT_EQ(model->num_tris, 2*n_tot*h_num); 
+        EXPECT_EQ(model->num_vertices, 2 + n_tot * h_num);
+        EXPECT_EQ(model->num_tris, 2 * n_tot * h_num); 
         EXPECT_EQ(model->build_state, BVH_BUILD_STATE_BEGUN);
  
         generateBVHModel(*model, cone, Transform3<S>(Translation3<S>(Vector3<S>(2.0, 2.0, 2.0))), n);
-        EXPECT_EQ(model->num_vertices, 2*(2+n_tot*h_num));
-        EXPECT_EQ(model->num_tris, 4*n_tot*h_num);
+        EXPECT_EQ(model->num_vertices, 2 * (2 + n_tot * h_num));
+        EXPECT_EQ(model->num_tris, 4 * n_tot * h_num);
         EXPECT_EQ(model->build_state, BVH_BUILD_STATE_PROCESSED);
       }
     }

From e15642fdc3ca0e7944ae1b762cfa290faf886213 Mon Sep 17 00:00:00 2001
From: Nico van Duijn <nico@auterion.com>
Date: Thu, 5 Jul 2018 08:46:59 +0200
Subject: [PATCH 10/24] Improve TestBox

---
 test/test_fcl_generate_bvh_model.cpp | 28 +++++++++++++++++-----------
 1 file changed, 17 insertions(+), 11 deletions(-)

diff --git a/test/test_fcl_generate_bvh_model.cpp b/test/test_fcl_generate_bvh_model.cpp
index 8239626a1..28e580149 100644
--- a/test/test_fcl_generate_bvh_model.cpp
+++ b/test/test_fcl_generate_bvh_model.cpp
@@ -48,19 +48,26 @@ void testBVHModelFromBox()
 {
   using S = typename BV::S;
 
-  std::shared_ptr<BVHModel<BV> > model(new BVHModel<BV>);
-  Box<S> box(1.0, 1.0, 1.0);
+  // Test various box sizes
+  for(S a = 0.5; a <= 2.1; a += 0.8){
+    for(S b = 0.5; b <= 2.1; b += 0.8){
+      for(S c = 0.5; c <= 2.1; c += 0.8){
 
-  generateBVHModel(*model, box, Transform3<S>::Identity(), FinalizeModel::DONT_FINALIZE);
-  EXPECT_EQ(model->num_vertices, 8);
-  EXPECT_EQ(model->num_tris, 12);
-  EXPECT_EQ(model->build_state, BVH_BUILD_STATE_BEGUN);
+        std::shared_ptr<BVHModel<BV> > model(new BVHModel<BV>);
+        Box<S> box(a, b, c);
 
-  generateBVHModel(*model, box, Transform3<S>(Translation3<S>(Vector3<S>(2.0, 2.0, 2.0))));
-  EXPECT_EQ(model->num_vertices, 16);
-  EXPECT_EQ(model->num_tris, 24);
-  EXPECT_EQ(model->build_state, BVH_BUILD_STATE_PROCESSED);
+        generateBVHModel(*model, box, Transform3<S>::Identity(), FinalizeModel::DONT_FINALIZE);
+        EXPECT_EQ(model->num_vertices, 8);
+        EXPECT_EQ(model->num_tris, 12);
+        EXPECT_EQ(model->build_state, BVH_BUILD_STATE_BEGUN);
 
+        generateBVHModel(*model, box, Transform3<S>(Translation3<S>(Vector3<S>(2.0, 2.0, 2.0))));
+        EXPECT_EQ(model->num_vertices, 16);
+        EXPECT_EQ(model->num_tris, 24);
+        EXPECT_EQ(model->build_state, BVH_BUILD_STATE_PROCESSED);
+      }
+    }
+  }
 }
 
 template<typename BV>
@@ -213,7 +220,6 @@ void testBVHModelFromPrimitives()
 
 GTEST_TEST(FCL_GENERATE_BVH_MODELS, generating_bvh_models_from_primitives)
 {
-
   testBVHModelFromPrimitives<AABB<double>>();
   testBVHModelFromPrimitives<OBB<double>>();
   testBVHModelFromPrimitives<RSS<double>>();

From 685b917ad644f959c5eaad0b31c909a313061245 Mon Sep 17 00:00:00 2001
From: Nico van Duijn <nico@auterion.com>
Date: Thu, 5 Jul 2018 08:48:44 +0200
Subject: [PATCH 11/24] Change loop ordering in for performance

So we can pull out a loop invariant
---
 test/test_fcl_generate_bvh_model.cpp | 18 ++++++++++--------
 1 file changed, 10 insertions(+), 8 deletions(-)

diff --git a/test/test_fcl_generate_bvh_model.cpp b/test/test_fcl_generate_bvh_model.cpp
index 28e580149..8a113c4e9 100644
--- a/test/test_fcl_generate_bvh_model.cpp
+++ b/test/test_fcl_generate_bvh_model.cpp
@@ -150,14 +150,15 @@ void testBVHModelFromCylinder()
   const S pi = constants<S>::pi();
 
   // Try various resolutions, radii and heights
-  for(uint8_t n = 4; n <= 32; n += 8){
-    for(S r = 0.5; r <= 2.1; r += 0.8){
-      for(S h = 0.5; h <= 2.1; h += 0.8){
+  for(S r = 0.5; r <= 2.1; r += 0.8){
+    for(S h = 0.5; h <= 2.1; h += 0.8){
+      Cylinder<S> cylinder(r, h);
+
+      for(uint8_t n = 4; n <= 32; n += 8){
         unsigned int n_tot = n * r;
         unsigned int h_num = ceil(h / ((pi * 2 / n_tot) * r));
 
         std::shared_ptr<BVHModel<BV> > model(new BVHModel<BV>);
-        Cylinder<S> cylinder(r, h);
 
         // Testing the overload with num_faces defined ends up in a call to both
         generateBVHModel(*model, cylinder, Transform3<S>::Identity(), n, FinalizeModel::DONT_FINALIZE);
@@ -184,14 +185,15 @@ void testBVHModelFromCone()
   const S pi = constants<S>::pi();
 
   // Try various resolutions, radii and heights
-  for(uint8_t n = 4; n <= 32; n += 8){
-    for(S r = 0.5; r <= 2.1; r += 0.8){
-      for(S h = 0.5; h <= 2.1; h += 0.8){
+  for(S r = 0.5; r <= 2.1; r += 0.8){
+    for(S h = 0.5; h <= 2.1; h += 0.8){
+      Cone<S> cone(r,h);
+  
+      for(uint8_t n = 4; n <= 32; n += 8){
         unsigned int n_tot = n * r;
         unsigned int h_num = ceil(h / ((pi * 2 / n_tot) * r));
 
         std::shared_ptr<BVHModel<BV> > model(new BVHModel<BV>);
-        Cone<S> cone(r,h);
 
         // Testing the overload with num_faces defined ends up in a call to both
         generateBVHModel(*model, cone, Transform3<S>::Identity(), n, FinalizeModel::DONT_FINALIZE);

From 2a348394decc84c1b0bd5e2e3c6e902e9622d6e7 Mon Sep 17 00:00:00 2001
From: Nico van Duijn <nico@auterion.com>
Date: Thu, 5 Jul 2018 09:19:25 +0200
Subject: [PATCH 12/24] Add casts to remove compiler warnings

---
 test/CMakeLists.txt                  |  2 +-
 test/test_fcl_generate_bvh_model.cpp | 32 ++++++++++++++--------------
 2 files changed, 17 insertions(+), 17 deletions(-)

diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index e99b46080..549ab2a4a 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -37,7 +37,6 @@ target_link_libraries(test_fcl_utility fcl)
 
 # test file list
 set(tests
-    test_fcl_generate_bvh_model.cpp
     test_fcl_auto_diff.cpp
     test_fcl_box_box.cpp
     test_fcl_broadphase_collision_1.cpp
@@ -53,6 +52,7 @@ set(tests
     test_fcl_distance.cpp
     test_fcl_frontlist.cpp
     test_fcl_general.cpp
+    test_fcl_generate_bvh_model.cpp
     test_fcl_geometric_shapes.cpp
     test_fcl_math.cpp
     test_fcl_profiler.cpp
diff --git a/test/test_fcl_generate_bvh_model.cpp b/test/test_fcl_generate_bvh_model.cpp
index 8a113c4e9..51e6e1674 100644
--- a/test/test_fcl_generate_bvh_model.cpp
+++ b/test/test_fcl_generate_bvh_model.cpp
@@ -88,14 +88,14 @@ void testBVHModelFromSphere()
   
       // Testing the overload with num_faces defined ends up in a call to both
       generateBVHModel(*model, sphere, Transform3<S>::Identity(), n, FinalizeModel::DONT_FINALIZE);
-      EXPECT_EQ(model->num_vertices, 2 + ring * ring);
-      EXPECT_EQ(model->num_tris, 2 * ring * ring);
+      EXPECT_EQ(model->num_vertices, static_cast<int>(2 + ring * ring));
+      EXPECT_EQ(model->num_tris, static_cast<int>(2 * ring * ring));
       EXPECT_EQ(model->build_state, BVH_BUILD_STATE_BEGUN);
 
       // Test that we can add another sphere to the model
       generateBVHModel(*model, sphere, Transform3<S>(Translation3<S>(Vector3<S>(2.0, 2.0, 2.0))), n);
-      EXPECT_EQ(model->num_vertices, 2 * ( 2 + ring * ring));
-      EXPECT_EQ(model->num_tris, 2 * (2 * ring * ring));
+      EXPECT_EQ(model->num_vertices, static_cast<int>(2 * ( 2 + ring * ring)));
+      EXPECT_EQ(model->num_tris, static_cast<int>(2 * (2 * ring * ring)));
       EXPECT_EQ(model->build_state, BVH_BUILD_STATE_PROCESSED);
     }
   }
@@ -125,14 +125,14 @@ for(S a = 0.5; a <= 2.1; a += 0.8){
 
         // Testing the overload with num_faces defined ends up in a call to both
         generateBVHModel(*model, ellipsoid, Transform3<S>::Identity(), n, FinalizeModel::DONT_FINALIZE);
-        EXPECT_EQ(model->num_vertices, 2 + ring * ring);
-        EXPECT_EQ(model->num_tris, 2 * ring * ring);
+        EXPECT_EQ(model->num_vertices, static_cast<int>(2 + ring * ring));
+        EXPECT_EQ(model->num_tris, static_cast<int>(2 * ring * ring));
         EXPECT_EQ(model->build_state, BVH_BUILD_STATE_BEGUN);
 
         // Make sure we can add another ellipsoid to the model
         generateBVHModel(*model, ellipsoid, Transform3<S>(Translation3<S>(Vector3<S>(2.0, 2.0, 2.0))), n);
-        EXPECT_EQ(model->num_vertices, 2 * ( 2 + ring * ring));
-        EXPECT_EQ(model->num_tris, 2 * (2 * ring * ring));
+        EXPECT_EQ(model->num_vertices, static_cast<int>(2 * ( 2 + ring * ring)));
+        EXPECT_EQ(model->num_tris, static_cast<int>(2 * (2 * ring * ring)));
         EXPECT_EQ(model->build_state, BVH_BUILD_STATE_PROCESSED);
       }
     }
@@ -163,13 +163,13 @@ void testBVHModelFromCylinder()
         // Testing the overload with num_faces defined ends up in a call to both
         generateBVHModel(*model, cylinder, Transform3<S>::Identity(), n, FinalizeModel::DONT_FINALIZE);
 
-        EXPECT_EQ(model->num_vertices, 2 + n_tot * (h_num + 1));
-        EXPECT_EQ(model->num_tris, (2 * h_num + 2) * n_tot);
+        EXPECT_EQ(model->num_vertices, static_cast<int>(2 + n_tot * (h_num + 1)));
+        EXPECT_EQ(model->num_tris, static_cast<int>((2 * h_num + 2) * n_tot));
         EXPECT_EQ(model->build_state, BVH_BUILD_STATE_BEGUN);
 
         generateBVHModel(*model, cylinder, Transform3<S>(Translation3<S>(Vector3<S>(2.0, 2.0, 2.0))), n);
-        EXPECT_EQ(model->num_vertices, 2 * (2 + n_tot * (h_num + 1)));
-        EXPECT_EQ(model->num_tris, 2 * ((2 * h_num + 2) * n_tot));
+        EXPECT_EQ(model->num_vertices, static_cast<int>(2 * (2 + n_tot * (h_num + 1))));
+        EXPECT_EQ(model->num_tris, static_cast<int>(2 * ((2 * h_num + 2) * n_tot)));
         EXPECT_EQ(model->build_state, BVH_BUILD_STATE_PROCESSED);
       }
     }
@@ -197,13 +197,13 @@ void testBVHModelFromCone()
 
         // Testing the overload with num_faces defined ends up in a call to both
         generateBVHModel(*model, cone, Transform3<S>::Identity(), n, FinalizeModel::DONT_FINALIZE);
-        EXPECT_EQ(model->num_vertices, 2 + n_tot * h_num);
-        EXPECT_EQ(model->num_tris, 2 * n_tot * h_num); 
+        EXPECT_EQ(model->num_vertices, static_cast<int>(2 + n_tot * h_num));
+        EXPECT_EQ(model->num_tris, static_cast<int>(2 * n_tot * h_num)); 
         EXPECT_EQ(model->build_state, BVH_BUILD_STATE_BEGUN);
  
         generateBVHModel(*model, cone, Transform3<S>(Translation3<S>(Vector3<S>(2.0, 2.0, 2.0))), n);
-        EXPECT_EQ(model->num_vertices, 2 * (2 + n_tot * h_num));
-        EXPECT_EQ(model->num_tris, 4 * n_tot * h_num);
+        EXPECT_EQ(model->num_vertices, static_cast<int>(2 * (2 + n_tot * h_num)));
+        EXPECT_EQ(model->num_tris, static_cast<int>(4 * n_tot * h_num));
         EXPECT_EQ(model->build_state, BVH_BUILD_STATE_PROCESSED);
       }
     }

From 72131a99b0c1a8518ea878d28d1feb4513b9dc73 Mon Sep 17 00:00:00 2001
From: Nico van Duijn <nico@auterion.com>
Date: Thu, 5 Jul 2018 10:06:56 +0200
Subject: [PATCH 13/24] Formatting and whitespace

---
 .../geometric_shape_to_BVH_model-inl.h        |  3 +-
 .../geometry/geometric_shape_to_BVH_model.h   |  4 +-
 test/test_fcl_generate_bvh_model.cpp          | 60 +++++++++----------
 3 files changed, 30 insertions(+), 37 deletions(-)

diff --git a/include/fcl/geometry/geometric_shape_to_BVH_model-inl.h b/include/fcl/geometry/geometric_shape_to_BVH_model-inl.h
index 7852fdc79..03fbe2f25 100644
--- a/include/fcl/geometry/geometric_shape_to_BVH_model-inl.h
+++ b/include/fcl/geometry/geometric_shape_to_BVH_model-inl.h
@@ -461,5 +461,4 @@ void generateBVHModel(BVHModel<BV>& model, const Cone<typename BV::S>& shape, co
 
 } // namespace fcl
 
-#endif
-
+#endif
\ No newline at end of file
diff --git a/include/fcl/geometry/geometric_shape_to_BVH_model.h b/include/fcl/geometry/geometric_shape_to_BVH_model.h
index 30683a060..17e9800ed 100644
--- a/include/fcl/geometry/geometric_shape_to_BVH_model.h
+++ b/include/fcl/geometry/geometric_shape_to_BVH_model.h
@@ -120,8 +120,6 @@ void generateBVHModel(BVHModel<BV>& model, const Cone<typename BV::S>& shape, co
 
 } // namespace fcl
 
-
-
 #include "fcl/geometry/geometric_shape_to_BVH_model-inl.h"
 
-#endif
+#endif
\ No newline at end of file
diff --git a/test/test_fcl_generate_bvh_model.cpp b/test/test_fcl_generate_bvh_model.cpp
index 51e6e1674..c78b27d18 100644
--- a/test/test_fcl_generate_bvh_model.cpp
+++ b/test/test_fcl_generate_bvh_model.cpp
@@ -107,41 +107,38 @@ void testBVHModelFromEllipsoid()
   using S = typename BV::S;
   const S p = 1.6075;
 
-// Test various radii
-for(S a = 0.5; a <= 2.1; a += 0.8){
-  for(S b = 0.5; b <= 2.1; b += 0.8){
-    for(S c = 0.5; c <= 2.1; c += 0.8){
-      Ellipsoid<S> ellipsoid(a, b, c);
-      const S& ap = std::pow(a, p);
-      const S& bp = std::pow(b, p);
-      const S& cp = std::pow(c, p);
-      const S ratio = std::pow((ap * bp + bp * cp + cp * ap) / 3.0, 1.0 / p);
-
-      // Test various resolutions
-      for(uint8_t n = 4; n <= 32; n += 8){
-        const S n_low_bound = std::sqrt(n / 2.0) * ratio;
-        const unsigned int ring = std::ceil(n_low_bound);
-        std::shared_ptr<BVHModel<BV> > model(new BVHModel<BV>);
-
-        // Testing the overload with num_faces defined ends up in a call to both
-        generateBVHModel(*model, ellipsoid, Transform3<S>::Identity(), n, FinalizeModel::DONT_FINALIZE);
-        EXPECT_EQ(model->num_vertices, static_cast<int>(2 + ring * ring));
-        EXPECT_EQ(model->num_tris, static_cast<int>(2 * ring * ring));
-        EXPECT_EQ(model->build_state, BVH_BUILD_STATE_BEGUN);
-
-        // Make sure we can add another ellipsoid to the model
-        generateBVHModel(*model, ellipsoid, Transform3<S>(Translation3<S>(Vector3<S>(2.0, 2.0, 2.0))), n);
-        EXPECT_EQ(model->num_vertices, static_cast<int>(2 * ( 2 + ring * ring)));
-        EXPECT_EQ(model->num_tris, static_cast<int>(2 * (2 * ring * ring)));
-        EXPECT_EQ(model->build_state, BVH_BUILD_STATE_PROCESSED);
+  // Test various radii
+  for(S a = 0.5; a <= 2.1; a += 0.8){
+    for(S b = 0.5; b <= 2.1; b += 0.8){
+      for(S c = 0.5; c <= 2.1; c += 0.8){
+        Ellipsoid<S> ellipsoid(a, b, c);
+        const S& ap = std::pow(a, p);
+        const S& bp = std::pow(b, p);
+        const S& cp = std::pow(c, p);
+        const S ratio = std::pow((ap * bp + bp * cp + cp * ap) / 3.0, 1.0 / p);
+
+        // Test various resolutions
+        for(uint8_t n = 4; n <= 32; n += 8){
+          const S n_low_bound = std::sqrt(n / 2.0) * ratio;
+          const unsigned int ring = std::ceil(n_low_bound);
+          std::shared_ptr<BVHModel<BV> > model(new BVHModel<BV>);
+
+          // Testing the overload with num_faces defined ends up in a call to both
+          generateBVHModel(*model, ellipsoid, Transform3<S>::Identity(), n, FinalizeModel::DONT_FINALIZE);
+          EXPECT_EQ(model->num_vertices, static_cast<int>(2 + ring * ring));
+          EXPECT_EQ(model->num_tris, static_cast<int>(2 * ring * ring));
+          EXPECT_EQ(model->build_state, BVH_BUILD_STATE_BEGUN);
+
+          // Make sure we can add another ellipsoid to the model
+          generateBVHModel(*model, ellipsoid, Transform3<S>(Translation3<S>(Vector3<S>(2.0, 2.0, 2.0))), n);
+          EXPECT_EQ(model->num_vertices, static_cast<int>(2 * ( 2 + ring * ring)));
+          EXPECT_EQ(model->num_tris, static_cast<int>(2 * (2 * ring * ring)));
+          EXPECT_EQ(model->build_state, BVH_BUILD_STATE_PROCESSED);
+        }
       }
     }
   }
 }
-  
-  
-
-}
 
 template<typename BV>
 void testBVHModelFromCylinder()
@@ -174,7 +171,6 @@ void testBVHModelFromCylinder()
       }
     }
   }
-
 }
 
 template<typename BV>

From eeab6b80886e367847e535779ef979058b50b803 Mon Sep 17 00:00:00 2001
From: Nico van Duijn <nico@auterion.com>
Date: Fri, 6 Jul 2018 16:07:52 +0200
Subject: [PATCH 14/24] More elaborate comments

---
 .../geometric_shape_to_BVH_model-inl.h        |  4 +-
 .../geometry/geometric_shape_to_BVH_model.h   | 80 +++++++++++++------
 2 files changed, 56 insertions(+), 28 deletions(-)

diff --git a/include/fcl/geometry/geometric_shape_to_BVH_model-inl.h b/include/fcl/geometry/geometric_shape_to_BVH_model-inl.h
index 03fbe2f25..ce7425b8f 100644
--- a/include/fcl/geometry/geometric_shape_to_BVH_model-inl.h
+++ b/include/fcl/geometry/geometric_shape_to_BVH_model-inl.h
@@ -442,7 +442,7 @@ void generateBVHModel(BVHModel<BV>& model, const Cone<typename BV::S>& shape, co
 
 //==============================================================================
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Cone<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int circle_split_tot_for_unit_cylinder, FinalizeModel finalize_model)
+void generateBVHModel(BVHModel<BV>& model, const Cone<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int circle_split_tot_for_unit_cone, FinalizeModel finalize_model)
 {
   using S = typename BV::S;
 
@@ -450,7 +450,7 @@ void generateBVHModel(BVHModel<BV>& model, const Cone<typename BV::S>& shape, co
   S h = shape.lz;
 
   const S pi = constants<S>::pi();
-  unsigned int circle_split_tot = circle_split_tot_for_unit_cylinder * r;
+  unsigned int circle_split_tot = circle_split_tot_for_unit_cone * r;
   S phid = pi * 2 / circle_split_tot;
 
   S circle_edge = phid * r;
diff --git a/include/fcl/geometry/geometric_shape_to_BVH_model.h b/include/fcl/geometry/geometric_shape_to_BVH_model.h
index 17e9800ed..56a6377f2 100644
--- a/include/fcl/geometry/geometric_shape_to_BVH_model.h
+++ b/include/fcl/geometry/geometric_shape_to_BVH_model.h
@@ -53,17 +53,17 @@
 namespace fcl
 {
 /**
-* \defgroup generateBVHModel
-* @param[out]  model a reference to the BVHModel to be generated or added to
-* @param[in]   shape a reference to the geometric object to be added to the BVHModel
-* @param[in]   pose a const reference to the pose of the geometric object
-* @param[in]   finalize_model a FinalizeModel enum indicating whether the model is final or more submodels can be added later
-* @{
+@defgroup generateBVHModel
+@param[out]  model a reference to the BVHModel to be generated or added to
+@param[in]   shape a reference to the geometric object to be added to the BVHModel
+@param[in]   pose a const reference to the pose of the geometric object
+@param[in]   finalize_model a FinalizeModel enum indicating whether the model is final or more submodels can be added later
+@{
 */
 
 /**
-* @brief    enum class used to indicate whether we simply want to add more primitives to the model
-*           or finalize it.
+@brief    enum class used to indicate whether we simply want to add more primitives to the model
+          or finalize it.
 */
 enum class FinalizeModel{
 	DO_FINALIZE,
@@ -74,47 +74,75 @@ enum class FinalizeModel{
 template<typename BV>
 void generateBVHModel(BVHModel<BV>& model, const Box<typename BV::S>& shape, const Transform3<typename BV::S>& pose, FinalizeModel finalize_model = FinalizeModel::DO_FINALIZE);
 
-/// @brief Generate BVH model from sphere, given the number of segments along longitude and number of rings along latitude.
+/**
+@brief Generate BVH model from sphere
+@param[in]   seg an unsigned integer defining the number of segments along longitude
+@param[in]   ring an unsigned integer defining the number of rings along latitude
+**/
 template<typename BV>
 void generateBVHModel(BVHModel<BV>& model, const Sphere<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int seg, unsigned int ring, FinalizeModel finalize_model = FinalizeModel::DO_FINALIZE);
 
-/// @brief Generate BVH model from sphere
-/// The difference between generateBVHModel is that it gives the number of triangles faces N for a sphere with unit radius. For sphere of radius r,
-/// then the number of triangles is r * r * N so that the area represented by a single triangle is approximately the same.s
+/**
+@brief     Generate BVH model from sphere
+@details   The difference between generateBVHModel is that it gives the number of triangles faces N for a sphere with unit radius. For sphere of radius r,
+           then the number of triangles is r * r * N so that the area represented by a single triangle is approximately the same.
+@param[in] n_faces_for_unit_sphere an unsigned integer defining the number of triangles for a unit-sized sphere
+**/
 template<typename BV>
 void generateBVHModel(BVHModel<BV>& model, const Sphere<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int n_faces_for_unit_sphere, FinalizeModel finalize_model = FinalizeModel::DO_FINALIZE);
 
-/// @brief Generate BVH model from ellipsoid, given the number of segments along longitude and number of rings along latitude.
+/**
+@brief      Generate BVH model from ellipsoid
+@param[in]  seg an unsigned integer defining the number of segments along longitude
+@param[in]  ring an unsigned integer defining the number of rings along latitude
+**/
 template<typename BV>
 void generateBVHModel(BVHModel<BV>& model, const Ellipsoid<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int seg, unsigned int ring, FinalizeModel finalize_model = FinalizeModel::DO_FINALIZE);
 
-/// @brief Generate BVH model from ellipsoid
-/// The difference between generateBVHModel is that it gives the number of triangles faces N for an ellipsoid with unit radii (1, 1, 1). For ellipsoid of radii (a, b, c),
-/// then the number of triangles is ((a^p * b^p + b^p * c^p + c^p * a^p)/3)^(1/p) * N, where p is 1.6075, so that the area represented by a single triangle is approximately the same.
-/// Reference: https://en.wikipedia.org/wiki/Ellipsoid<S>#Approximate_formula
+/**
+@brief     Generate BVH model from ellipsoid
+@details   The difference between generateBVHModel is that it gives the number of triangles faces N for an ellipsoid with unit radii (1, 1, 1). For ellipsoid of radii (a, b, c),
+           then the number of triangles is ((a^p * b^p + b^p * c^p + c^p * a^p)/3)^(1/p) * N, where p is 1.6075, so that the area represented by a single triangle is approximately the same.
+           Reference: https://en.wikipedia.org/wiki/Ellipsoid<S>#Approximate_formula
+@param[in] n_faces_for_unit_ellipsoid an unsigned integer defining the number of faces a unit ellipsoid would have
+**/
 template<typename BV>
 void generateBVHModel(BVHModel<BV>& model, const Ellipsoid<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int n_faces_for_unit_ellipsoid, FinalizeModel finalize_model = FinalizeModel::DO_FINALIZE);
 
-/// @brief Generate BVH model from cylinder, given the number of segments along circle and the number of segments along axis.
+/**
+@brief     Generate BVH model from cylinder, given the number of segments along circle and the number of segments along axis.
+@param[in] circle_split_tot an unsigned integer defining into how many segments the bottom plate of the cylinder is split into
+@param[in] h_num an unsigned integer defining into how many segments along the axis the cylinder is split into
+**/
 template<typename BV>
 void generateBVHModel(BVHModel<BV>& model, const Cylinder<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int circle_split_tot, unsigned int h_num, FinalizeModel finalize_model = FinalizeModel::DO_FINALIZE);
 
-/// @brief Generate BVH model from cylinder
-/// Difference from generateBVHModel: is that it gives the circle split number tot for a cylinder with unit radius. For cylinder with
-/// larger radius, the number of circle split number is r * tot.
+/**
+@brief     Generate BVH model from cylinder
+@details   Difference from generateBVHModel: is that it gives the circle split number tot for a cylinder with unit radius. For cylinder with
+           larger radius, the number of circle split number is r * tot.
+@param[in] circle_split_tot_for_unit_cylinder an unsigned integer defining into how many segments the bottom plate of an equivalent unit-sized cylinder would be split into
+**/
 template<typename BV>
 void generateBVHModel(BVHModel<BV>& model, const Cylinder<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int circle_split_tot_for_unit_cylinder, FinalizeModel finalize_model = FinalizeModel::DO_FINALIZE);
 
 
-/// @brief Generate BVH model from cone, given the number of segments along circle and the number of segments along axis.
+/**
+@brief     Generate BVH model from cone
+@param[in] circle_split_tot an unsigned integer defining how many segments the bottom plate is split into
+@param[in] h_num an unsigned integer defining how many segments along the axis the cone is split into
+**/
 template<typename BV>
 void generateBVHModel(BVHModel<BV>& model, const Cone<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int circle_split_tot, unsigned int h_num, FinalizeModel finalize_model = FinalizeModel::DO_FINALIZE);
 
-/// @brief Generate BVH model from cone
-/// Difference from generateBVHModel: is that it gives the circle split number tot for a cylinder with unit radius. For cone with
-/// larger radius, the number of circle split number is r * tot.
+/**
+@brief     Generate BVH model from cone
+@details   Difference from generateBVHModel: is that it gives the circle split number tot for a cylinder with unit radius. For cone with
+           larger radius, the number of circle split number is r * tot.
+@param[in] circle_split_tot_for_unit_cone an unsigned integer defining into how many segments the bottom plate of an equivalent unit-sized cone would be split into
+**/
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Cone<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int circle_split_tot_for_unit_cylinder, FinalizeModel finalize_model = FinalizeModel::DO_FINALIZE);
+void generateBVHModel(BVHModel<BV>& model, const Cone<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int circle_split_tot_for_unit_cone, FinalizeModel finalize_model = FinalizeModel::DO_FINALIZE);
 
 /**@} */ // end of doxygen group generateBVHModel
 

From 7361dfa3f85146589d9bf963aead5b98d364711f Mon Sep 17 00:00:00 2001
From: Nico van Duijn <nico@auterion.com>
Date: Mon, 9 Jul 2018 09:02:37 +0200
Subject: [PATCH 15/24] Better comments and addTriangles() function

---
 .../geometric_shape_to_BVH_model-inl.h        | 67 ++++++----------
 .../geometry/geometric_shape_to_BVH_model.h   | 79 +++++++++++--------
 2 files changed, 69 insertions(+), 77 deletions(-)

diff --git a/include/fcl/geometry/geometric_shape_to_BVH_model-inl.h b/include/fcl/geometry/geometric_shape_to_BVH_model-inl.h
index ce7425b8f..ee99b1211 100644
--- a/include/fcl/geometry/geometric_shape_to_BVH_model-inl.h
+++ b/include/fcl/geometry/geometric_shape_to_BVH_model-inl.h
@@ -81,15 +81,7 @@ void generateBVHModel(BVHModel<BV>& model, const Box<typename BV::S>& shape, con
     points[i] = pose * points[i];
   }
 
-  if(model.build_state == BVH_BUILD_STATE_EMPTY){
-    model.beginModel();
-  }
-
-  model.addSubModel(points, tri_indices);
-  if(finalize_model == FinalizeModel::DO_FINALIZE){
-    model.endModel();
-    model.computeLocalAABB();
-  }
+  addTriangles(model, points, tri_indices, finalize_model);
 }
 
 
@@ -154,15 +146,7 @@ void generateBVHModel(BVHModel<BV>& model, const Sphere<typename BV::S>& shape,
     points[i] = pose * points[i];
   }
 
-  if(model.build_state == BVH_BUILD_STATE_EMPTY){
-    model.beginModel();
-  }
-
-  model.addSubModel(points, tri_indices);
-  if(finalize_model == FinalizeModel::DO_FINALIZE){
-    model.endModel();
-    model.computeLocalAABB();
-  }
+  addTriangles(model, points, tri_indices, finalize_model);
 }
 
 //==============================================================================
@@ -243,15 +227,7 @@ void generateBVHModel(BVHModel<BV>& model, const Ellipsoid<typename BV::S>& shap
     points[i] = pose * points[i];
   }
 
-  if(model.build_state == BVH_BUILD_STATE_EMPTY){
-    model.beginModel();
-  }
-
-  model.addSubModel(points, tri_indices);
-  if(finalize_model == FinalizeModel::DO_FINALIZE){
-    model.endModel();
-    model.computeLocalAABB();
-  }
+  addTriangles(model, points, tri_indices, finalize_model);
 }
 
 //==============================================================================
@@ -337,15 +313,7 @@ void generateBVHModel(BVHModel<BV>& model, const Cylinder<typename BV::S>& shape
     points[i] = pose * points[i];
   }
 
-  if(model.build_state == BVH_BUILD_STATE_EMPTY){
-    model.beginModel();
-  }
-
-  model.addSubModel(points, tri_indices);
-  if(finalize_model == FinalizeModel::DO_FINALIZE){
-    model.endModel();
-    model.computeLocalAABB();
-  }
+  addTriangles(model, points, tri_indices, finalize_model);
 }
 
 //==============================================================================
@@ -429,15 +397,8 @@ void generateBVHModel(BVHModel<BV>& model, const Cone<typename BV::S>& shape, co
     points[i] = pose * points[i];
   }
 
-  if(model.build_state == BVH_BUILD_STATE_EMPTY){
-    model.beginModel();
-  }
-
-  model.addSubModel(points, tri_indices);
-  if(finalize_model == FinalizeModel::DO_FINALIZE){
-    model.endModel();
-    model.computeLocalAABB();
-  }
+  addTriangles(model, points, tri_indices, finalize_model);
+  
 }
 
 //==============================================================================
@@ -459,6 +420,22 @@ void generateBVHModel(BVHModel<BV>& model, const Cone<typename BV::S>& shape, co
   generateBVHModel(model, shape, pose, circle_split_tot, h_num, finalize_model);
 }
 
+//==============================================================================
+template<typename BV>
+void addTriangles(BVHModel<BV>& model, const std::vector<Vector3<typename BV::S>>& points, const std::vector<Triangle>& tri_indices, FinalizeModel finalize_model)
+{
+  if(model.build_state == BVH_BUILD_STATE_EMPTY){
+    model.beginModel();
+  }
+
+  model.addSubModel(points, tri_indices);
+  if(finalize_model == FinalizeModel::DO){
+    model.endModel();
+    model.computeLocalAABB();
+  }
+}
+
+
 } // namespace fcl
 
 #endif
\ No newline at end of file
diff --git a/include/fcl/geometry/geometric_shape_to_BVH_model.h b/include/fcl/geometry/geometric_shape_to_BVH_model.h
index 56a6377f2..9eff9de77 100644
--- a/include/fcl/geometry/geometric_shape_to_BVH_model.h
+++ b/include/fcl/geometry/geometric_shape_to_BVH_model.h
@@ -52,97 +52,112 @@
 
 namespace fcl
 {
-/**
-@defgroup generateBVHModel
-@param[out]  model a reference to the BVHModel to be generated or added to
-@param[in]   shape a reference to the geometric object to be added to the BVHModel
-@param[in]   pose a const reference to the pose of the geometric object
-@param[in]   finalize_model a FinalizeModel enum indicating whether the model is final or more submodels can be added later
-@{
-*/
-
 /**
 @brief    enum class used to indicate whether we simply want to add more primitives to the model
           or finalize it.
 */
 enum class FinalizeModel{
-	DO_FINALIZE,
-	DONT_FINALIZE
+	DO,
+	DONT
 };
 
+
+/**
+@defgroup generateBVHModel
+@brief       Create a BVHModel using geometric primitives
+@details    The functions in this group can be used to add geometric primitives (Box, Sphere, Ellipsoid, Cylinder, Cone)
+             to a BVHModel. It can either close off the model or leave it unfinalized in order to add more primitives later.
+
+@param[out]  model The BVHModel to be generated or added to
+@param[in]   shape The geometric object to be added to the BVHModel
+@param[in]   pose The pose of the geometric object
+@param[in]   finalize_model an enum indicating whether the model is final or more submodels can be added later
+@{
+*/
+
 /// @brief Generate BVH model from box
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Box<typename BV::S>& shape, const Transform3<typename BV::S>& pose, FinalizeModel finalize_model = FinalizeModel::DO_FINALIZE);
+void generateBVHModel(BVHModel<BV>& model, const Box<typename BV::S>& shape, const Transform3<typename BV::S>& pose, FinalizeModel finalize_model = FinalizeModel::DO);
 
 /**
 @brief Generate BVH model from sphere
-@param[in]   seg an unsigned integer defining the number of segments along longitude
-@param[in]   ring an unsigned integer defining the number of rings along latitude
+@param[in]   seg The number of segments along longitude
+@param[in]   ring The number of rings along latitude
 **/
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Sphere<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int seg, unsigned int ring, FinalizeModel finalize_model = FinalizeModel::DO_FINALIZE);
+void generateBVHModel(BVHModel<BV>& model, const Sphere<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int seg, unsigned int ring, FinalizeModel finalize_model = FinalizeModel::DO);
 
 /**
 @brief     Generate BVH model from sphere
 @details   The difference between generateBVHModel is that it gives the number of triangles faces N for a sphere with unit radius. For sphere of radius r,
            then the number of triangles is r * r * N so that the area represented by a single triangle is approximately the same.
-@param[in] n_faces_for_unit_sphere an unsigned integer defining the number of triangles for a unit-sized sphere
+@param[in] n_faces_for_unit_sphere The number of triangles for a unit-sized sphere
 **/
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Sphere<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int n_faces_for_unit_sphere, FinalizeModel finalize_model = FinalizeModel::DO_FINALIZE);
+void generateBVHModel(BVHModel<BV>& model, const Sphere<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int n_faces_for_unit_sphere, FinalizeModel finalize_model = FinalizeModel::DO);
 
 /**
 @brief      Generate BVH model from ellipsoid
-@param[in]  seg an unsigned integer defining the number of segments along longitude
-@param[in]  ring an unsigned integer defining the number of rings along latitude
+@param[in]  seg The number of segments along longitude
+@param[in]  ring The number of rings along latitude
 **/
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Ellipsoid<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int seg, unsigned int ring, FinalizeModel finalize_model = FinalizeModel::DO_FINALIZE);
+void generateBVHModel(BVHModel<BV>& model, const Ellipsoid<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int seg, unsigned int ring, FinalizeModel finalize_model = FinalizeModel::DO);
 
 /**
 @brief     Generate BVH model from ellipsoid
 @details   The difference between generateBVHModel is that it gives the number of triangles faces N for an ellipsoid with unit radii (1, 1, 1). For ellipsoid of radii (a, b, c),
            then the number of triangles is ((a^p * b^p + b^p * c^p + c^p * a^p)/3)^(1/p) * N, where p is 1.6075, so that the area represented by a single triangle is approximately the same.
            Reference: https://en.wikipedia.org/wiki/Ellipsoid<S>#Approximate_formula
-@param[in] n_faces_for_unit_ellipsoid an unsigned integer defining the number of faces a unit ellipsoid would have
+@param[in] n_faces_for_unit_ellipsoid The number of faces a unit ellipsoid would have
 **/
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Ellipsoid<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int n_faces_for_unit_ellipsoid, FinalizeModel finalize_model = FinalizeModel::DO_FINALIZE);
+void generateBVHModel(BVHModel<BV>& model, const Ellipsoid<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int n_faces_for_unit_ellipsoid, FinalizeModel finalize_model = FinalizeModel::DO);
 
 /**
 @brief     Generate BVH model from cylinder, given the number of segments along circle and the number of segments along axis.
-@param[in] circle_split_tot an unsigned integer defining into how many segments the bottom plate of the cylinder is split into
-@param[in] h_num an unsigned integer defining into how many segments along the axis the cylinder is split into
+@param[in] circle_split_tot The number of segments the bottom plate of the cylinder is split into
+@param[in] h_num The number of segments along the axis the cylinder is split into
 **/
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Cylinder<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int circle_split_tot, unsigned int h_num, FinalizeModel finalize_model = FinalizeModel::DO_FINALIZE);
+void generateBVHModel(BVHModel<BV>& model, const Cylinder<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int circle_split_tot, unsigned int h_num, FinalizeModel finalize_model = FinalizeModel::DO);
 
 /**
 @brief     Generate BVH model from cylinder
 @details   Difference from generateBVHModel: is that it gives the circle split number tot for a cylinder with unit radius. For cylinder with
            larger radius, the number of circle split number is r * tot.
-@param[in] circle_split_tot_for_unit_cylinder an unsigned integer defining into how many segments the bottom plate of an equivalent unit-sized cylinder would be split into
+@param[in] circle_split_tot_for_unit_cylinder The number of segments the bottom plate of an equivalent unit-sized cylinder would be split into
 **/
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Cylinder<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int circle_split_tot_for_unit_cylinder, FinalizeModel finalize_model = FinalizeModel::DO_FINALIZE);
+void generateBVHModel(BVHModel<BV>& model, const Cylinder<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int circle_split_tot_for_unit_cylinder, FinalizeModel finalize_model = FinalizeModel::DO);
 
 
 /**
 @brief     Generate BVH model from cone
-@param[in] circle_split_tot an unsigned integer defining how many segments the bottom plate is split into
-@param[in] h_num an unsigned integer defining how many segments along the axis the cone is split into
+@param[in] circle_split_tot The number of segments the bottom plate is split into
+@param[in] h_num an The number of segments along the axis the cone is split into
 **/
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Cone<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int circle_split_tot, unsigned int h_num, FinalizeModel finalize_model = FinalizeModel::DO_FINALIZE);
+void generateBVHModel(BVHModel<BV>& model, const Cone<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int circle_split_tot, unsigned int h_num, FinalizeModel finalize_model = FinalizeModel::DO);
 
 /**
 @brief     Generate BVH model from cone
 @details   Difference from generateBVHModel: is that it gives the circle split number tot for a cylinder with unit radius. For cone with
            larger radius, the number of circle split number is r * tot.
-@param[in] circle_split_tot_for_unit_cone an unsigned integer defining into how many segments the bottom plate of an equivalent unit-sized cone would be split into
+@param[in] circle_split_tot_for_unit_cone The number of segments the bottom plate of an equivalent unit-sized cone would be split into
+**/
+template<typename BV>
+void generateBVHModel(BVHModel<BV>& model, const Cone<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int circle_split_tot_for_unit_cone, FinalizeModel finalize_model = FinalizeModel::DO);
+
+/**
+@brief          AddTriangles to a BVHModel
+@param[in, out] model The BVHModel
+@param[in]      points The points to add
+@param[in]      tri_indices The triangles to add
+@param[in]      finalize_model An enum indicating whether to close off the model afterwards
 **/
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Cone<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int circle_split_tot_for_unit_cone, FinalizeModel finalize_model = FinalizeModel::DO_FINALIZE);
+void addTriangles(BVHModel<BV>& model, const std::vector<Vector3<typename BV::S>>& points, const std::vector<Triangle>& tri_indices, FinalizeModel finalize_model);
 
 /**@} */ // end of doxygen group generateBVHModel
 

From 45647be5bfd88a84a2a15181d436ac21f3a5afc5 Mon Sep 17 00:00:00 2001
From: Nico van Duijn <nico@auterion.com>
Date: Mon, 9 Jul 2018 09:58:18 +0200
Subject: [PATCH 16/24] Refactor test to use checkNumVerticesAndTris

---
 test/test_fcl_generate_bvh_model.cpp | 104 ++++++++++++---------------
 1 file changed, 45 insertions(+), 59 deletions(-)

diff --git a/test/test_fcl_generate_bvh_model.cpp b/test/test_fcl_generate_bvh_model.cpp
index c78b27d18..de006124d 100644
--- a/test/test_fcl_generate_bvh_model.cpp
+++ b/test/test_fcl_generate_bvh_model.cpp
@@ -43,6 +43,39 @@
 
 using namespace fcl;
 
+template<typename BV, typename ShapeType>
+void checkNumVerticesAndTris(BVHModel<BV>& model, const ShapeType& shape, uint8_t n, int vertices, int tris)
+{  
+  using S = typename BV::S;
+
+  // Add the shape to the model and count vertices and triangles to make sure it has been created
+  generateBVHModel(model, shape, Transform3<S>::Identity(), n, FinalizeModel::DONT);
+  EXPECT_EQ(model.num_vertices, vertices);
+  EXPECT_EQ(model.num_tris, tris); 
+  EXPECT_EQ(model.build_state, BVH_BUILD_STATE_BEGUN);
+
+  // Add another instance of the shape and make sure it was added to the model by counting vertices and tris
+  generateBVHModel(model, shape, Transform3<S>(Translation3<S>(Vector3<S>(2.0, 2.0, 2.0))), n);
+  EXPECT_EQ(model.num_vertices, 2*vertices);
+  EXPECT_EQ(model.num_tris, 2*tris);
+  EXPECT_EQ(model.build_state, BVH_BUILD_STATE_PROCESSED);
+}
+
+// Slightly different for boxes, cannot define numFaces
+template<typename BV>
+void checkNumVerticesAndTris(BVHModel<BV>& model, const Box<typename BV::S>& shape, int vertices, int tris)
+{
+  using S = typename BV::S;
+  generateBVHModel(model, shape, Transform3<S>::Identity(), FinalizeModel::DONT);
+  EXPECT_EQ(model.num_vertices, vertices);
+  EXPECT_EQ(model.num_tris, tris); 
+  EXPECT_EQ(model.build_state, BVH_BUILD_STATE_BEGUN);
+  generateBVHModel(model, shape, Transform3<S>(Translation3<S>(Vector3<S>(2.0, 2.0, 2.0))));
+  EXPECT_EQ(model.num_vertices, 2*vertices);
+  EXPECT_EQ(model.num_tris, 2*tris);
+  EXPECT_EQ(model.build_state, BVH_BUILD_STATE_PROCESSED);
+}
+
 template<typename BV>
 void testBVHModelFromBox()
 {
@@ -53,18 +86,10 @@ void testBVHModelFromBox()
     for(S b = 0.5; b <= 2.1; b += 0.8){
       for(S c = 0.5; c <= 2.1; c += 0.8){
 
-        std::shared_ptr<BVHModel<BV> > model(new BVHModel<BV>);
+        std::shared_ptr<BVHModel<BV>> model(new BVHModel<BV>);
         Box<S> box(a, b, c);
 
-        generateBVHModel(*model, box, Transform3<S>::Identity(), FinalizeModel::DONT_FINALIZE);
-        EXPECT_EQ(model->num_vertices, 8);
-        EXPECT_EQ(model->num_tris, 12);
-        EXPECT_EQ(model->build_state, BVH_BUILD_STATE_BEGUN);
-
-        generateBVHModel(*model, box, Transform3<S>(Translation3<S>(Vector3<S>(2.0, 2.0, 2.0))));
-        EXPECT_EQ(model->num_vertices, 16);
-        EXPECT_EQ(model->num_tris, 24);
-        EXPECT_EQ(model->build_state, BVH_BUILD_STATE_PROCESSED);
+        checkNumVerticesAndTris(*model, box, 8, 12);
       }
     }
   }
@@ -84,19 +109,9 @@ void testBVHModelFromSphere()
       S n_low_bound = sqrtf(n / 2.0) * r * r;
       unsigned int ring = ceil(n_low_bound);
 
-      std::shared_ptr<BVHModel<BV> > model(new BVHModel<BV>);
+      std::shared_ptr<BVHModel<BV>> model(new BVHModel<BV>);
   
-      // Testing the overload with num_faces defined ends up in a call to both
-      generateBVHModel(*model, sphere, Transform3<S>::Identity(), n, FinalizeModel::DONT_FINALIZE);
-      EXPECT_EQ(model->num_vertices, static_cast<int>(2 + ring * ring));
-      EXPECT_EQ(model->num_tris, static_cast<int>(2 * ring * ring));
-      EXPECT_EQ(model->build_state, BVH_BUILD_STATE_BEGUN);
-
-      // Test that we can add another sphere to the model
-      generateBVHModel(*model, sphere, Transform3<S>(Translation3<S>(Vector3<S>(2.0, 2.0, 2.0))), n);
-      EXPECT_EQ(model->num_vertices, static_cast<int>(2 * ( 2 + ring * ring)));
-      EXPECT_EQ(model->num_tris, static_cast<int>(2 * (2 * ring * ring)));
-      EXPECT_EQ(model->build_state, BVH_BUILD_STATE_PROCESSED);
+      checkNumVerticesAndTris(*model, sphere, n, static_cast<int>(2 + ring * ring), static_cast<int>(2 * ring * ring));
     }
   }
 }
@@ -121,19 +136,9 @@ void testBVHModelFromEllipsoid()
         for(uint8_t n = 4; n <= 32; n += 8){
           const S n_low_bound = std::sqrt(n / 2.0) * ratio;
           const unsigned int ring = std::ceil(n_low_bound);
-          std::shared_ptr<BVHModel<BV> > model(new BVHModel<BV>);
-
-          // Testing the overload with num_faces defined ends up in a call to both
-          generateBVHModel(*model, ellipsoid, Transform3<S>::Identity(), n, FinalizeModel::DONT_FINALIZE);
-          EXPECT_EQ(model->num_vertices, static_cast<int>(2 + ring * ring));
-          EXPECT_EQ(model->num_tris, static_cast<int>(2 * ring * ring));
-          EXPECT_EQ(model->build_state, BVH_BUILD_STATE_BEGUN);
-
-          // Make sure we can add another ellipsoid to the model
-          generateBVHModel(*model, ellipsoid, Transform3<S>(Translation3<S>(Vector3<S>(2.0, 2.0, 2.0))), n);
-          EXPECT_EQ(model->num_vertices, static_cast<int>(2 * ( 2 + ring * ring)));
-          EXPECT_EQ(model->num_tris, static_cast<int>(2 * (2 * ring * ring)));
-          EXPECT_EQ(model->build_state, BVH_BUILD_STATE_PROCESSED);
+          std::shared_ptr<BVHModel<BV>> model(new BVHModel<BV>);
+
+          checkNumVerticesAndTris(*model, ellipsoid, n, static_cast<int>(2 + ring * ring), static_cast<int>(2 * ring * ring));
         }
       }
     }
@@ -155,19 +160,9 @@ void testBVHModelFromCylinder()
         unsigned int n_tot = n * r;
         unsigned int h_num = ceil(h / ((pi * 2 / n_tot) * r));
 
-        std::shared_ptr<BVHModel<BV> > model(new BVHModel<BV>);
+        std::shared_ptr<BVHModel<BV>> model(new BVHModel<BV>);
 
-        // Testing the overload with num_faces defined ends up in a call to both
-        generateBVHModel(*model, cylinder, Transform3<S>::Identity(), n, FinalizeModel::DONT_FINALIZE);
-
-        EXPECT_EQ(model->num_vertices, static_cast<int>(2 + n_tot * (h_num + 1)));
-        EXPECT_EQ(model->num_tris, static_cast<int>((2 * h_num + 2) * n_tot));
-        EXPECT_EQ(model->build_state, BVH_BUILD_STATE_BEGUN);
-
-        generateBVHModel(*model, cylinder, Transform3<S>(Translation3<S>(Vector3<S>(2.0, 2.0, 2.0))), n);
-        EXPECT_EQ(model->num_vertices, static_cast<int>(2 * (2 + n_tot * (h_num + 1))));
-        EXPECT_EQ(model->num_tris, static_cast<int>(2 * ((2 * h_num + 2) * n_tot)));
-        EXPECT_EQ(model->build_state, BVH_BUILD_STATE_PROCESSED);
+        checkNumVerticesAndTris(*model, cylinder, n, static_cast<int>(2 + n_tot * (h_num + 1)), static_cast<int>((2 * h_num + 2) * n_tot));
       }
     }
   }
@@ -189,18 +184,9 @@ void testBVHModelFromCone()
         unsigned int n_tot = n * r;
         unsigned int h_num = ceil(h / ((pi * 2 / n_tot) * r));
 
-        std::shared_ptr<BVHModel<BV> > model(new BVHModel<BV>);
-
-        // Testing the overload with num_faces defined ends up in a call to both
-        generateBVHModel(*model, cone, Transform3<S>::Identity(), n, FinalizeModel::DONT_FINALIZE);
-        EXPECT_EQ(model->num_vertices, static_cast<int>(2 + n_tot * h_num));
-        EXPECT_EQ(model->num_tris, static_cast<int>(2 * n_tot * h_num)); 
-        EXPECT_EQ(model->build_state, BVH_BUILD_STATE_BEGUN);
- 
-        generateBVHModel(*model, cone, Transform3<S>(Translation3<S>(Vector3<S>(2.0, 2.0, 2.0))), n);
-        EXPECT_EQ(model->num_vertices, static_cast<int>(2 * (2 + n_tot * h_num)));
-        EXPECT_EQ(model->num_tris, static_cast<int>(4 * n_tot * h_num));
-        EXPECT_EQ(model->build_state, BVH_BUILD_STATE_PROCESSED);
+        std::shared_ptr<BVHModel<BV>> model(new BVHModel<BV>);
+        checkNumVerticesAndTris(*model, cone, n, static_cast<int>(2 + n_tot * h_num), static_cast<int>(2 * n_tot * h_num));
+
       }
     }
   }

From beb10098be0d46f155ad3464c4603a0b794e163a Mon Sep 17 00:00:00 2001
From: Nico van Duijn <nico@auterion.com>
Date: Mon, 9 Jul 2018 11:07:51 +0200
Subject: [PATCH 17/24] Add error handling to generateBVHModel

---
 .../geometric_shape_to_BVH_model-inl.h        | 52 +++++++++++--------
 .../geometry/geometric_shape_to_BVH_model.h   | 20 +++----
 2 files changed, 39 insertions(+), 33 deletions(-)

diff --git a/include/fcl/geometry/geometric_shape_to_BVH_model-inl.h b/include/fcl/geometry/geometric_shape_to_BVH_model-inl.h
index ee99b1211..528f0e3cd 100644
--- a/include/fcl/geometry/geometric_shape_to_BVH_model-inl.h
+++ b/include/fcl/geometry/geometric_shape_to_BVH_model-inl.h
@@ -45,7 +45,7 @@ namespace fcl
 
 //==============================================================================
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Box<typename BV::S>& shape, const Transform3<typename BV::S>& pose, FinalizeModel finalize_model)
+int generateBVHModel(BVHModel<BV>& model, const Box<typename BV::S>& shape, const Transform3<typename BV::S>& pose, FinalizeModel finalize_model)
 {
   using S = typename BV::S;
 
@@ -81,13 +81,13 @@ void generateBVHModel(BVHModel<BV>& model, const Box<typename BV::S>& shape, con
     points[i] = pose * points[i];
   }
 
-  addTriangles(model, points, tri_indices, finalize_model);
+  return addTriangles(model, points, tri_indices, finalize_model);
 }
 
 
 //==============================================================================
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Sphere<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int seg, unsigned int ring, FinalizeModel finalize_model)
+int generateBVHModel(BVHModel<BV>& model, const Sphere<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int seg, unsigned int ring, FinalizeModel finalize_model)
 {
   using S = typename BV::S;
 
@@ -146,12 +146,12 @@ void generateBVHModel(BVHModel<BV>& model, const Sphere<typename BV::S>& shape,
     points[i] = pose * points[i];
   }
 
-  addTriangles(model, points, tri_indices, finalize_model);
+  return addTriangles(model, points, tri_indices, finalize_model);
 }
 
 //==============================================================================
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Sphere<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int n_faces_for_unit_sphere, FinalizeModel finalize_model)
+int generateBVHModel(BVHModel<BV>& model, const Sphere<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int n_faces_for_unit_sphere, FinalizeModel finalize_model)
 {
   using S = typename BV::S;
 
@@ -160,12 +160,12 @@ void generateBVHModel(BVHModel<BV>& model, const Sphere<typename BV::S>& shape,
   unsigned int ring = ceil(n_low_bound);
   unsigned int seg = ceil(n_low_bound);
 
-  generateBVHModel(model, shape, pose, seg, ring, finalize_model);
+  return generateBVHModel(model, shape, pose, seg, ring, finalize_model);
 }
 
 //==============================================================================
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Ellipsoid<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int seg, unsigned int ring, FinalizeModel finalize_model)
+int generateBVHModel(BVHModel<BV>& model, const Ellipsoid<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int seg, unsigned int ring, FinalizeModel finalize_model)
 {
   using S = typename BV::S;
 
@@ -227,12 +227,12 @@ void generateBVHModel(BVHModel<BV>& model, const Ellipsoid<typename BV::S>& shap
     points[i] = pose * points[i];
   }
 
-  addTriangles(model, points, tri_indices, finalize_model);
+  return addTriangles(model, points, tri_indices, finalize_model);
 }
 
 //==============================================================================
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Ellipsoid<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int n_faces_for_unit_ellipsoid, FinalizeModel finalize_model)
+int generateBVHModel(BVHModel<BV>& model, const Ellipsoid<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int n_faces_for_unit_ellipsoid, FinalizeModel finalize_model)
 {
   using S = typename BV::S;
 
@@ -248,12 +248,12 @@ void generateBVHModel(BVHModel<BV>& model, const Ellipsoid<typename BV::S>& shap
   const unsigned int ring = std::ceil(n_low_bound);
   const unsigned int seg = std::ceil(n_low_bound);
 
-  generateBVHModel(model, shape, pose, seg, ring, finalize_model);
+  return generateBVHModel(model, shape, pose, seg, ring, finalize_model);
 }
 
 //==============================================================================
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Cylinder<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int circle_split_tot, unsigned int h_num, FinalizeModel finalize_model)
+int generateBVHModel(BVHModel<BV>& model, const Cylinder<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int circle_split_tot, unsigned int h_num, FinalizeModel finalize_model)
 {
   using S = typename BV::S;
 
@@ -313,12 +313,12 @@ void generateBVHModel(BVHModel<BV>& model, const Cylinder<typename BV::S>& shape
     points[i] = pose * points[i];
   }
 
-  addTriangles(model, points, tri_indices, finalize_model);
+  return addTriangles(model, points, tri_indices, finalize_model);
 }
 
 //==============================================================================
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Cylinder<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int circle_split_tot_for_unit_cylinder, FinalizeModel finalize_model)
+int generateBVHModel(BVHModel<BV>& model, const Cylinder<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int circle_split_tot_for_unit_cylinder, FinalizeModel finalize_model)
 {
   using S = typename BV::S;
 
@@ -332,12 +332,12 @@ void generateBVHModel(BVHModel<BV>& model, const Cylinder<typename BV::S>& shape
   S circle_edge = phid * r;
   unsigned int h_num = ceil(h / circle_edge);
 
-  generateBVHModel(model, shape, pose, circle_split_tot, h_num, finalize_model);
+  return generateBVHModel(model, shape, pose, circle_split_tot, h_num, finalize_model);
 }
 
 //==============================================================================
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Cone<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int circle_split_tot, unsigned int h_num, FinalizeModel finalize_model)
+int generateBVHModel(BVHModel<BV>& model, const Cone<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int circle_split_tot, unsigned int h_num, FinalizeModel finalize_model)
 {
   using S = typename BV::S;
 
@@ -397,16 +397,17 @@ void generateBVHModel(BVHModel<BV>& model, const Cone<typename BV::S>& shape, co
     points[i] = pose * points[i];
   }
 
-  addTriangles(model, points, tri_indices, finalize_model);
+  return addTriangles(model, points, tri_indices, finalize_model);
   
 }
 
 //==============================================================================
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Cone<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int circle_split_tot_for_unit_cone, FinalizeModel finalize_model)
+int generateBVHModel(BVHModel<BV>& model, const Cone<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int circle_split_tot_for_unit_cone, FinalizeModel finalize_model)
 {
   using S = typename BV::S;
 
+
   S r = shape.radius;
   S h = shape.lz;
 
@@ -417,22 +418,27 @@ void generateBVHModel(BVHModel<BV>& model, const Cone<typename BV::S>& shape, co
   S circle_edge = phid * r;
   unsigned int h_num = ceil(h / circle_edge);
 
-  generateBVHModel(model, shape, pose, circle_split_tot, h_num, finalize_model);
+  return generateBVHModel(model, shape, pose, circle_split_tot, h_num, finalize_model);
 }
 
 //==============================================================================
 template<typename BV>
-void addTriangles(BVHModel<BV>& model, const std::vector<Vector3<typename BV::S>>& points, const std::vector<Triangle>& tri_indices, FinalizeModel finalize_model)
+int addTriangles(BVHModel<BV>& model, const std::vector<Vector3<typename BV::S>>& points, const std::vector<Triangle>& tri_indices, FinalizeModel finalize_model)
 {
+  int retval = BVH_OK;
   if(model.build_state == BVH_BUILD_STATE_EMPTY){
-    model.beginModel();
+    retval = model.beginModel();
+  }
+
+  if(retval == BVH_OK){
+    retval = model.addSubModel(points, tri_indices);
   }
 
-  model.addSubModel(points, tri_indices);
-  if(finalize_model == FinalizeModel::DO){
-    model.endModel();
+  if(retval == BVH_OK && finalize_model == FinalizeModel::DO){
+    retval = model.endModel();
     model.computeLocalAABB();
   }
+  return retval;
 }
 
 
diff --git a/include/fcl/geometry/geometric_shape_to_BVH_model.h b/include/fcl/geometry/geometric_shape_to_BVH_model.h
index 9eff9de77..dcea0ce82 100644
--- a/include/fcl/geometry/geometric_shape_to_BVH_model.h
+++ b/include/fcl/geometry/geometric_shape_to_BVH_model.h
@@ -77,7 +77,7 @@ enum class FinalizeModel{
 
 /// @brief Generate BVH model from box
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Box<typename BV::S>& shape, const Transform3<typename BV::S>& pose, FinalizeModel finalize_model = FinalizeModel::DO);
+int generateBVHModel(BVHModel<BV>& model, const Box<typename BV::S>& shape, const Transform3<typename BV::S>& pose, FinalizeModel finalize_model = FinalizeModel::DO);
 
 /**
 @brief Generate BVH model from sphere
@@ -85,7 +85,7 @@ void generateBVHModel(BVHModel<BV>& model, const Box<typename BV::S>& shape, con
 @param[in]   ring The number of rings along latitude
 **/
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Sphere<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int seg, unsigned int ring, FinalizeModel finalize_model = FinalizeModel::DO);
+int generateBVHModel(BVHModel<BV>& model, const Sphere<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int seg, unsigned int ring, FinalizeModel finalize_model = FinalizeModel::DO);
 
 /**
 @brief     Generate BVH model from sphere
@@ -94,7 +94,7 @@ void generateBVHModel(BVHModel<BV>& model, const Sphere<typename BV::S>& shape,
 @param[in] n_faces_for_unit_sphere The number of triangles for a unit-sized sphere
 **/
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Sphere<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int n_faces_for_unit_sphere, FinalizeModel finalize_model = FinalizeModel::DO);
+int generateBVHModel(BVHModel<BV>& model, const Sphere<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int n_faces_for_unit_sphere, FinalizeModel finalize_model = FinalizeModel::DO);
 
 /**
 @brief      Generate BVH model from ellipsoid
@@ -102,7 +102,7 @@ void generateBVHModel(BVHModel<BV>& model, const Sphere<typename BV::S>& shape,
 @param[in]  ring The number of rings along latitude
 **/
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Ellipsoid<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int seg, unsigned int ring, FinalizeModel finalize_model = FinalizeModel::DO);
+int generateBVHModel(BVHModel<BV>& model, const Ellipsoid<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int seg, unsigned int ring, FinalizeModel finalize_model = FinalizeModel::DO);
 
 /**
 @brief     Generate BVH model from ellipsoid
@@ -112,7 +112,7 @@ void generateBVHModel(BVHModel<BV>& model, const Ellipsoid<typename BV::S>& shap
 @param[in] n_faces_for_unit_ellipsoid The number of faces a unit ellipsoid would have
 **/
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Ellipsoid<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int n_faces_for_unit_ellipsoid, FinalizeModel finalize_model = FinalizeModel::DO);
+int generateBVHModel(BVHModel<BV>& model, const Ellipsoid<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int n_faces_for_unit_ellipsoid, FinalizeModel finalize_model = FinalizeModel::DO);
 
 /**
 @brief     Generate BVH model from cylinder, given the number of segments along circle and the number of segments along axis.
@@ -120,7 +120,7 @@ void generateBVHModel(BVHModel<BV>& model, const Ellipsoid<typename BV::S>& shap
 @param[in] h_num The number of segments along the axis the cylinder is split into
 **/
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Cylinder<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int circle_split_tot, unsigned int h_num, FinalizeModel finalize_model = FinalizeModel::DO);
+int generateBVHModel(BVHModel<BV>& model, const Cylinder<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int circle_split_tot, unsigned int h_num, FinalizeModel finalize_model = FinalizeModel::DO);
 
 /**
 @brief     Generate BVH model from cylinder
@@ -129,7 +129,7 @@ void generateBVHModel(BVHModel<BV>& model, const Cylinder<typename BV::S>& shape
 @param[in] circle_split_tot_for_unit_cylinder The number of segments the bottom plate of an equivalent unit-sized cylinder would be split into
 **/
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Cylinder<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int circle_split_tot_for_unit_cylinder, FinalizeModel finalize_model = FinalizeModel::DO);
+int generateBVHModel(BVHModel<BV>& model, const Cylinder<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int circle_split_tot_for_unit_cylinder, FinalizeModel finalize_model = FinalizeModel::DO);
 
 
 /**
@@ -138,7 +138,7 @@ void generateBVHModel(BVHModel<BV>& model, const Cylinder<typename BV::S>& shape
 @param[in] h_num an The number of segments along the axis the cone is split into
 **/
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Cone<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int circle_split_tot, unsigned int h_num, FinalizeModel finalize_model = FinalizeModel::DO);
+int generateBVHModel(BVHModel<BV>& model, const Cone<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int circle_split_tot, unsigned int h_num, FinalizeModel finalize_model = FinalizeModel::DO);
 
 /**
 @brief     Generate BVH model from cone
@@ -147,7 +147,7 @@ void generateBVHModel(BVHModel<BV>& model, const Cone<typename BV::S>& shape, co
 @param[in] circle_split_tot_for_unit_cone The number of segments the bottom plate of an equivalent unit-sized cone would be split into
 **/
 template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Cone<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int circle_split_tot_for_unit_cone, FinalizeModel finalize_model = FinalizeModel::DO);
+int generateBVHModel(BVHModel<BV>& model, const Cone<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int circle_split_tot_for_unit_cone, FinalizeModel finalize_model = FinalizeModel::DO);
 
 /**
 @brief          AddTriangles to a BVHModel
@@ -157,7 +157,7 @@ void generateBVHModel(BVHModel<BV>& model, const Cone<typename BV::S>& shape, co
 @param[in]      finalize_model An enum indicating whether to close off the model afterwards
 **/
 template<typename BV>
-void addTriangles(BVHModel<BV>& model, const std::vector<Vector3<typename BV::S>>& points, const std::vector<Triangle>& tri_indices, FinalizeModel finalize_model);
+int addTriangles(BVHModel<BV>& model, const std::vector<Vector3<typename BV::S>>& points, const std::vector<Triangle>& tri_indices, FinalizeModel finalize_model);
 
 /**@} */ // end of doxygen group generateBVHModel
 

From 77a119bc56ce25bb668375cf1426ab122ac52f29 Mon Sep 17 00:00:00 2001
From: Nico van Duijn <nico@auterion.com>
Date: Mon, 9 Jul 2018 11:12:36 +0200
Subject: [PATCH 18/24] Add test for adding to closed models

---
 test/test_fcl_generate_bvh_model.cpp | 30 +++++++++++++++++++++++++---
 1 file changed, 27 insertions(+), 3 deletions(-)

diff --git a/test/test_fcl_generate_bvh_model.cpp b/test/test_fcl_generate_bvh_model.cpp
index de006124d..093ba96db 100644
--- a/test/test_fcl_generate_bvh_model.cpp
+++ b/test/test_fcl_generate_bvh_model.cpp
@@ -66,16 +66,36 @@ template<typename BV>
 void checkNumVerticesAndTris(BVHModel<BV>& model, const Box<typename BV::S>& shape, int vertices, int tris)
 {
   using S = typename BV::S;
-  generateBVHModel(model, shape, Transform3<S>::Identity(), FinalizeModel::DONT);
+  auto ret  = generateBVHModel(model, shape, Transform3<S>::Identity(), FinalizeModel::DONT);
+  EXPECT_EQ(ret, BVH_OK);
   EXPECT_EQ(model.num_vertices, vertices);
   EXPECT_EQ(model.num_tris, tris); 
   EXPECT_EQ(model.build_state, BVH_BUILD_STATE_BEGUN);
-  generateBVHModel(model, shape, Transform3<S>(Translation3<S>(Vector3<S>(2.0, 2.0, 2.0))));
+  ret = generateBVHModel(model, shape, Transform3<S>(Translation3<S>(Vector3<S>(2.0, 2.0, 2.0))));
+  EXPECT_EQ(ret, BVH_OK);
   EXPECT_EQ(model.num_vertices, 2*vertices);
   EXPECT_EQ(model.num_tris, 2*tris);
   EXPECT_EQ(model.build_state, BVH_BUILD_STATE_PROCESSED);
 }
 
+template<typename BV, typename ShapeType>
+void checkAddToFinishedModel(BVHModel<BV>& model, const ShapeType& shape, uint8_t n)
+{
+  using S = typename BV::S;
+  EXPECT_EQ(model.build_state, BVH_BUILD_STATE_PROCESSED);
+  auto ret = generateBVHModel(model, shape, Transform3<S>::Identity(), n, FinalizeModel::DONT);
+  EXPECT_EQ(ret, BVH_ERR_BUILD_OUT_OF_SEQUENCE);
+}
+
+template<typename BV>
+void checkAddToFinishedModel(BVHModel<BV>& model, const Box<typename BV::S>& shape)
+{
+  using S = typename BV::S;
+  EXPECT_EQ(model.build_state, BVH_BUILD_STATE_PROCESSED);
+  auto ret = generateBVHModel(model, shape, Transform3<S>::Identity(), FinalizeModel::DONT);
+  EXPECT_EQ(ret, BVH_ERR_BUILD_OUT_OF_SEQUENCE);
+}
+
 template<typename BV>
 void testBVHModelFromBox()
 {
@@ -90,6 +110,7 @@ void testBVHModelFromBox()
         Box<S> box(a, b, c);
 
         checkNumVerticesAndTris(*model, box, 8, 12);
+        checkAddToFinishedModel(*model, box);
       }
     }
   }
@@ -112,6 +133,7 @@ void testBVHModelFromSphere()
       std::shared_ptr<BVHModel<BV>> model(new BVHModel<BV>);
   
       checkNumVerticesAndTris(*model, sphere, n, static_cast<int>(2 + ring * ring), static_cast<int>(2 * ring * ring));
+      checkAddToFinishedModel(*model, sphere, n);
     }
   }
 }
@@ -139,6 +161,7 @@ void testBVHModelFromEllipsoid()
           std::shared_ptr<BVHModel<BV>> model(new BVHModel<BV>);
 
           checkNumVerticesAndTris(*model, ellipsoid, n, static_cast<int>(2 + ring * ring), static_cast<int>(2 * ring * ring));
+          checkAddToFinishedModel(*model, ellipsoid, n);
         }
       }
     }
@@ -163,6 +186,7 @@ void testBVHModelFromCylinder()
         std::shared_ptr<BVHModel<BV>> model(new BVHModel<BV>);
 
         checkNumVerticesAndTris(*model, cylinder, n, static_cast<int>(2 + n_tot * (h_num + 1)), static_cast<int>((2 * h_num + 2) * n_tot));
+        checkAddToFinishedModel(*model, cylinder, n);
       }
     }
   }
@@ -186,7 +210,7 @@ void testBVHModelFromCone()
 
         std::shared_ptr<BVHModel<BV>> model(new BVHModel<BV>);
         checkNumVerticesAndTris(*model, cone, n, static_cast<int>(2 + n_tot * h_num), static_cast<int>(2 * n_tot * h_num));
-
+        checkAddToFinishedModel(*model, cone, n);
       }
     }
   }

From 3dcafd817ac99f00bd03d865fb4af1a1e57e0fd1 Mon Sep 17 00:00:00 2001
From: Nico van Duijn <nico@auterion.com>
Date: Mon, 9 Jul 2018 11:19:18 +0200
Subject: [PATCH 19/24] Range based for loops in testBVHModel

---
 test/test_fcl_generate_bvh_model.cpp | 30 ++++++++++++++--------------
 1 file changed, 15 insertions(+), 15 deletions(-)

diff --git a/test/test_fcl_generate_bvh_model.cpp b/test/test_fcl_generate_bvh_model.cpp
index 093ba96db..9f76e6acb 100644
--- a/test/test_fcl_generate_bvh_model.cpp
+++ b/test/test_fcl_generate_bvh_model.cpp
@@ -102,9 +102,9 @@ void testBVHModelFromBox()
   using S = typename BV::S;
 
   // Test various box sizes
-  for(S a = 0.5; a <= 2.1; a += 0.8){
-    for(S b = 0.5; b <= 2.1; b += 0.8){
-      for(S c = 0.5; c <= 2.1; c += 0.8){
+  for(S a : {0.5, 1.0, 2.0, 3.1}){
+    for(S b : {0.5, 1.0, 2.0, 3.1}){
+      for(S c : {0.5, 1.0, 2.0, 3.1}){
 
         std::shared_ptr<BVHModel<BV>> model(new BVHModel<BV>);
         Box<S> box(a, b, c);
@@ -122,11 +122,11 @@ void testBVHModelFromSphere()
   using S = typename BV::S;
 
   // Test various radii
-  for(S r = 0.5; r <= 2.1; r += 0.8){
+  for(S r : {0.5, 1.0, 2.0, 3.1}){
     Sphere<S> sphere(r);
 
     // Test various resolutions
-    for(uint8_t n = 4; n <= 32; n += 8){
+    for(uint8_t n : {4, 8, 16, 32}){
       S n_low_bound = sqrtf(n / 2.0) * r * r;
       unsigned int ring = ceil(n_low_bound);
 
@@ -145,9 +145,9 @@ void testBVHModelFromEllipsoid()
   const S p = 1.6075;
 
   // Test various radii
-  for(S a = 0.5; a <= 2.1; a += 0.8){
-    for(S b = 0.5; b <= 2.1; b += 0.8){
-      for(S c = 0.5; c <= 2.1; c += 0.8){
+  for(S a : {0.5, 1.0, 2.0, 3.1}){
+    for(S b : {0.5, 1.0, 2.0, 3.1}){
+      for(S c : {0.5, 1.0, 2.0, 3.1}){
         Ellipsoid<S> ellipsoid(a, b, c);
         const S& ap = std::pow(a, p);
         const S& bp = std::pow(b, p);
@@ -155,7 +155,7 @@ void testBVHModelFromEllipsoid()
         const S ratio = std::pow((ap * bp + bp * cp + cp * ap) / 3.0, 1.0 / p);
 
         // Test various resolutions
-        for(uint8_t n = 4; n <= 32; n += 8){
+        for(uint8_t n : {4, 8, 16, 32}){
           const S n_low_bound = std::sqrt(n / 2.0) * ratio;
           const unsigned int ring = std::ceil(n_low_bound);
           std::shared_ptr<BVHModel<BV>> model(new BVHModel<BV>);
@@ -175,11 +175,11 @@ void testBVHModelFromCylinder()
   const S pi = constants<S>::pi();
 
   // Try various resolutions, radii and heights
-  for(S r = 0.5; r <= 2.1; r += 0.8){
-    for(S h = 0.5; h <= 2.1; h += 0.8){
+  for(S r : {0.5, 1.0, 2.0, 3.1}){
+    for(S h : {0.5, 1.0, 2.0, 3.1}){
       Cylinder<S> cylinder(r, h);
 
-      for(uint8_t n = 4; n <= 32; n += 8){
+      for(uint8_t n : {4, 8, 16, 32}){
         unsigned int n_tot = n * r;
         unsigned int h_num = ceil(h / ((pi * 2 / n_tot) * r));
 
@@ -200,11 +200,11 @@ void testBVHModelFromCone()
   const S pi = constants<S>::pi();
 
   // Try various resolutions, radii and heights
-  for(S r = 0.5; r <= 2.1; r += 0.8){
-    for(S h = 0.5; h <= 2.1; h += 0.8){
+  for(S r : {0.5, 1.0, 2.0, 3.1}){
+    for(S h : {0.5, 1.0, 2.0, 3.1}){
       Cone<S> cone(r,h);
   
-      for(uint8_t n = 4; n <= 32; n += 8){
+      for(uint8_t n : {4, 8, 16, 32}){
         unsigned int n_tot = n * r;
         unsigned int h_num = ceil(h / ((pi * 2 / n_tot) * r));
 

From 7c11d0d8cf179355d8de0926c41394ede46c7468 Mon Sep 17 00:00:00 2001
From: Nico van Duijn <nico@auterion.com>
Date: Mon, 9 Jul 2018 11:24:37 +0200
Subject: [PATCH 20/24] Better commenting

---
 test/test_fcl_generate_bvh_model.cpp | 9 +++++++++
 1 file changed, 9 insertions(+)

diff --git a/test/test_fcl_generate_bvh_model.cpp b/test/test_fcl_generate_bvh_model.cpp
index 9f76e6acb..397915a0d 100644
--- a/test/test_fcl_generate_bvh_model.cpp
+++ b/test/test_fcl_generate_bvh_model.cpp
@@ -43,6 +43,11 @@
 
 using namespace fcl;
 
+/**
+@details   This function tests adding geometric primitives to a model, by first creating one
+           and then appending to it. It checks proper functionality of those simply by 
+           verifying the return value, the number of vertices, triangles and the state of the model.
+**/
 template<typename BV, typename ShapeType>
 void checkNumVerticesAndTris(BVHModel<BV>& model, const ShapeType& shape, uint8_t n, int vertices, int tris)
 {  
@@ -78,6 +83,10 @@ void checkNumVerticesAndTris(BVHModel<BV>& model, const Box<typename BV::S>& sha
   EXPECT_EQ(model.build_state, BVH_BUILD_STATE_PROCESSED);
 }
 
+/**
+@details   This function tests that adding geometric primitives to a finalized model indeed
+           returns the BVH error we would expect.
+**/
 template<typename BV, typename ShapeType>
 void checkAddToFinishedModel(BVHModel<BV>& model, const ShapeType& shape, uint8_t n)
 {

From e54d8320b15eed6478a4672a0353775854894d65 Mon Sep 17 00:00:00 2001
From: Nico van Duijn <nico@auterion.com>
Date: Tue, 10 Jul 2018 10:57:10 +0200
Subject: [PATCH 21/24] Refactor unit tests for generateBVHModel

Includes response to PR review, with minor comment fixes
as well as a major refactor in the unit test function.
---
 .../geometric_shape_to_BVH_model-inl.h        |  42 +--
 .../geometry/geometric_shape_to_BVH_model.h   |  20 +-
 test/CMakeLists.txt                           |   3 +-
 ...l_generate_bvh_model_deferred_finalize.cpp | 333 ++++++++++++++++++
 ...est_fcl_generate_bvh_model_primitives.cpp} |  72 ++--
 5 files changed, 393 insertions(+), 77 deletions(-)
 create mode 100644 test/test_fcl_generate_bvh_model_deferred_finalize.cpp
 rename test/{test_fcl_generate_bvh_model.cpp => test_fcl_generate_bvh_model_primitives.cpp} (77%)

diff --git a/include/fcl/geometry/geometric_shape_to_BVH_model-inl.h b/include/fcl/geometry/geometric_shape_to_BVH_model-inl.h
index 528f0e3cd..7fe43860c 100644
--- a/include/fcl/geometry/geometric_shape_to_BVH_model-inl.h
+++ b/include/fcl/geometry/geometric_shape_to_BVH_model-inl.h
@@ -43,6 +43,27 @@
 namespace fcl
 {
 
+//==============================================================================
+// Local helper function to ease conditional adding of triangles to a BVHModel
+template<typename BV>
+int addTriangles(BVHModel<BV>& model, const std::vector<Vector3<typename BV::S>>& points, const std::vector<Triangle>& tri_indices, FinalizeModel finalize_model)
+{
+  int retval = BVH_OK;
+  if(model.build_state == BVH_BUILD_STATE_EMPTY){
+    retval = model.beginModel();
+  }
+
+  if(retval == BVH_OK){
+    retval = model.addSubModel(points, tri_indices);
+  }
+
+  if(retval == BVH_OK && finalize_model == FinalizeModel::DO){
+    retval = model.endModel();
+    model.computeLocalAABB();
+  }
+  return retval;
+}
+
 //==============================================================================
 template<typename BV>
 int generateBVHModel(BVHModel<BV>& model, const Box<typename BV::S>& shape, const Transform3<typename BV::S>& pose, FinalizeModel finalize_model)
@@ -421,27 +442,6 @@ int generateBVHModel(BVHModel<BV>& model, const Cone<typename BV::S>& shape, con
   return generateBVHModel(model, shape, pose, circle_split_tot, h_num, finalize_model);
 }
 
-//==============================================================================
-template<typename BV>
-int addTriangles(BVHModel<BV>& model, const std::vector<Vector3<typename BV::S>>& points, const std::vector<Triangle>& tri_indices, FinalizeModel finalize_model)
-{
-  int retval = BVH_OK;
-  if(model.build_state == BVH_BUILD_STATE_EMPTY){
-    retval = model.beginModel();
-  }
-
-  if(retval == BVH_OK){
-    retval = model.addSubModel(points, tri_indices);
-  }
-
-  if(retval == BVH_OK && finalize_model == FinalizeModel::DO){
-    retval = model.endModel();
-    model.computeLocalAABB();
-  }
-  return retval;
-}
-
-
 } // namespace fcl
 
 #endif
\ No newline at end of file
diff --git a/include/fcl/geometry/geometric_shape_to_BVH_model.h b/include/fcl/geometry/geometric_shape_to_BVH_model.h
index dcea0ce82..294fd7eb4 100644
--- a/include/fcl/geometry/geometric_shape_to_BVH_model.h
+++ b/include/fcl/geometry/geometric_shape_to_BVH_model.h
@@ -65,13 +65,19 @@ enum class FinalizeModel{
 /**
 @defgroup generateBVHModel
 @brief       Create a BVHModel using geometric primitives
-@details    The functions in this group can be used to add geometric primitives (Box, Sphere, Ellipsoid, Cylinder, Cone)
+@details     The functions in this group can be used to add geometric primitives (Box, Sphere, Ellipsoid, Cylinder, Cone)
              to a BVHModel. It can either close off the model or leave it unfinalized in order to add more primitives later.
-
+@note        All functions in this group have a common sub-set of parameters (listed below). In addition, each has unique
+             parameters related to the geometry type being added and how it should be tessellated. These additional parameters
+             are documented with their corresponding function
+@warning     If this function is used to create a BVHModel containing multiple geometric primitives, the BVHModel inherently 
+             represents the *union* of those primitives. The BVHModel structure does not retain any notion of the original 
+             geometric primitive.
 @param[out]  model The BVHModel to be generated or added to
 @param[in]   shape The geometric object to be added to the BVHModel
 @param[in]   pose The pose of the geometric object
 @param[in]   finalize_model an enum indicating whether the model is final or more submodels can be added later
+@return      BVHReturnCode indicating the success of the operation
 @{
 */
 
@@ -149,16 +155,6 @@ int generateBVHModel(BVHModel<BV>& model, const Cone<typename BV::S>& shape, con
 template<typename BV>
 int generateBVHModel(BVHModel<BV>& model, const Cone<typename BV::S>& shape, const Transform3<typename BV::S>& pose, unsigned int circle_split_tot_for_unit_cone, FinalizeModel finalize_model = FinalizeModel::DO);
 
-/**
-@brief          AddTriangles to a BVHModel
-@param[in, out] model The BVHModel
-@param[in]      points The points to add
-@param[in]      tri_indices The triangles to add
-@param[in]      finalize_model An enum indicating whether to close off the model afterwards
-**/
-template<typename BV>
-int addTriangles(BVHModel<BV>& model, const std::vector<Vector3<typename BV::S>>& points, const std::vector<Triangle>& tri_indices, FinalizeModel finalize_model);
-
 /**@} */ // end of doxygen group generateBVHModel
 
 } // namespace fcl
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 549ab2a4a..2b6c18171 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -52,7 +52,8 @@ set(tests
     test_fcl_distance.cpp
     test_fcl_frontlist.cpp
     test_fcl_general.cpp
-    test_fcl_generate_bvh_model.cpp
+    test_fcl_generate_bvh_model_deferred_finalize.cpp
+    test_fcl_generate_bvh_model_primitives.cpp
     test_fcl_geometric_shapes.cpp
     test_fcl_math.cpp
     test_fcl_profiler.cpp
diff --git a/test/test_fcl_generate_bvh_model_deferred_finalize.cpp b/test/test_fcl_generate_bvh_model_deferred_finalize.cpp
new file mode 100644
index 000000000..bd5569fbe
--- /dev/null
+++ b/test/test_fcl_generate_bvh_model_deferred_finalize.cpp
@@ -0,0 +1,333 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2016, Open Source Robotics Foundation
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Open Source Robotics Foundation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** @author Nico van Duijn */
+
+#include <gtest/gtest.h>
+
+#include "fcl/config.h"
+#include "fcl/geometry/geometric_shape_to_BVH_model.h"
+#include "test_fcl_utility.h"
+#include <iostream>
+
+using namespace fcl;
+
+/** 
+@description   This file tests functionality in generateBVHModel(). Specifically,
+               it tests that a BVHModel can be either finalized after adding a 
+               geometric primitive, or left "open" in order to add further
+               shapes at a later time. This functionality is tested without any
+               regard to proper functionality or special cases in the conversion from
+               geometric primitive to BVHModel. 
+**/
+
+
+/**
+@details   This function tests adding geometric primitives to an empty model.
+           It checks proper functionality of those simply by 
+           verifying the return value, the number of vertices, triangles and the state of the model.
+           In the process, the provided model will always be BVH_BUILD_STATE_BEGUN afterwards
+**/
+template<typename BV, typename ShapeType>
+void checkAddToEmptyModel(BVHModel<BV>& model, const ShapeType& shape)
+{  
+  using S = typename BV::S;
+  uint8_t n = 32; // Hard-coded mesh-resolution. Not testing corner cases where n=0 or such
+  int ret;
+
+  // Make sure we are given an empty model
+  GTEST_ASSERT_EQ(model.build_state, BVH_BUILD_STATE_EMPTY);
+  uint8_t num_vertices = model.num_vertices;
+  uint8_t num_tris = model.num_tris;
+  GTEST_ASSERT_EQ(num_vertices, 0);
+  GTEST_ASSERT_EQ(num_tris, 0);
+
+  // Add the shape to the model and count vertices and triangles to make sure it has been created
+  ret = generateBVHModel(model, shape, Transform3<S>::Identity(), n, FinalizeModel::DONT);
+  GTEST_ASSERT_EQ(ret, BVH_OK);
+  EXPECT_GT(model.num_vertices, num_vertices);
+  EXPECT_GT(model.num_tris, num_tris); 
+  EXPECT_EQ(model.build_state, BVH_BUILD_STATE_BEGUN);
+}
+
+// Specialization for boxes
+template<typename BV>
+void checkAddToEmptyModel(BVHModel<BV>& model, const Box<typename BV::S>& shape)
+{  
+  using S = typename BV::S;
+  int ret;
+
+  // Make sure we are given an empty model
+  GTEST_ASSERT_EQ(model.build_state, BVH_BUILD_STATE_EMPTY);
+  uint8_t num_vertices = model.num_vertices;
+  uint8_t num_tris = model.num_tris;
+  GTEST_ASSERT_EQ(num_vertices, 0);
+  GTEST_ASSERT_EQ(num_tris, 0);
+
+  // Add the shape to the model and count vertices and triangles to make sure it has been created
+  ret = generateBVHModel(model, shape, Transform3<S>::Identity(), FinalizeModel::DONT);
+  GTEST_ASSERT_EQ(ret, BVH_OK);
+  EXPECT_GT(model.num_vertices, num_vertices);
+  EXPECT_GT(model.num_tris, num_tris); 
+  EXPECT_EQ(model.build_state, BVH_BUILD_STATE_BEGUN);
+}
+
+
+/**
+@details   This function tests adding geometric primitives to an unfinalized model.
+           It checks proper functionality by verifying the return value, 
+           the number of vertices, triangles and the state of the model.
+           After execution, the provided model will always be BVH_BUILD_STATE_BEGUN.
+**/
+template<typename BV, typename ShapeType>
+void checkAddToUnfinalizedModel(BVHModel<BV>& model, const ShapeType& shape)
+{  
+  using S = typename BV::S;
+  const uint8_t n = 32;
+  int ret;
+
+  // Make sure we are given a model that is already begun
+  GTEST_ASSERT_EQ(model.build_state, BVH_BUILD_STATE_BEGUN);
+  uint8_t num_vertices = model.num_vertices;
+  uint8_t num_tris = model.num_tris;
+
+  // Add the shape to the model and count vertices and triangles to make sure it has been created
+  ret = generateBVHModel(model, shape, Transform3<S>(Translation3<S>(Vector3<S>(2.0, 2.0, 2.0))), n, FinalizeModel::DONT);
+  GTEST_ASSERT_EQ(ret, BVH_OK);
+  EXPECT_GT(model.num_vertices, num_vertices);
+  EXPECT_GT(model.num_tris, num_tris); 
+  EXPECT_EQ(model.build_state, BVH_BUILD_STATE_BEGUN);
+}
+
+// Specialization for boxes
+template<typename BV>
+void checkAddToUnfinalizedModel(BVHModel<BV>& model, const Box<typename BV::S>& shape)
+{  
+  using S = typename BV::S;
+  int ret;
+
+  // Make sure we are given a model that is already begun
+  GTEST_ASSERT_EQ(model.build_state, BVH_BUILD_STATE_BEGUN);
+  uint8_t num_vertices = model.num_vertices;
+  uint8_t num_tris = model.num_tris;
+
+  // Add the shape to the model and count vertices and triangles to make sure it has been created
+  ret = generateBVHModel(model, shape, Transform3<S>(Translation3<S>(Vector3<S>(2.0, 2.0, 2.0))), FinalizeModel::DONT);
+  GTEST_ASSERT_EQ(ret, BVH_OK);
+  EXPECT_GT(model.num_vertices, num_vertices);
+  EXPECT_GT(model.num_tris, num_tris); 
+  EXPECT_EQ(model.build_state, BVH_BUILD_STATE_BEGUN);
+}
+
+/**
+@details   This function tests adding primitives to a previously begun model
+           It checks proper functionality by checking the return value,
+           the number of vertices and triangles and the state of the model
+           after execution. After this call, the model is finalized.
+
+**/
+template<typename BV, typename ShapeType>
+void checkAddAndFinalizeModel(BVHModel<BV>& model, const ShapeType& shape){
+  using S = typename BV::S;
+  const uint8_t n = 32;
+  int ret;
+
+  // Make sure we are given a model that is already begun
+  GTEST_ASSERT_EQ(model.build_state, BVH_BUILD_STATE_BEGUN);
+  uint8_t num_vertices = model.num_vertices;
+  uint8_t num_tris = model.num_tris;
+
+  // Add another instance of the shape and make sure it was added to the model by counting vertices and tris
+  ret = generateBVHModel(model, shape, Transform3<S>(Translation3<S>(Vector3<S>(2.0, 2.0, 2.0))), n, FinalizeModel::DO);
+  GTEST_ASSERT_EQ(ret, BVH_OK);
+  EXPECT_GT(model.num_vertices, num_vertices);
+  EXPECT_GT(model.num_tris, num_tris);
+  EXPECT_EQ(model.build_state, BVH_BUILD_STATE_PROCESSED);
+}
+
+// Specialization for boxes
+template<typename BV>
+void checkAddAndFinalizeModel(BVHModel<BV>& model, const Box<typename BV::S>& shape){
+  using S = typename BV::S;
+  int ret;
+
+  // Make sure we are given a model that is already begun
+  GTEST_ASSERT_EQ(model.build_state, BVH_BUILD_STATE_BEGUN);
+  uint8_t num_vertices = model.num_vertices;
+  uint8_t num_tris = model.num_tris;
+
+  // Add another instance of the shape and make sure it was added to the model by counting vertices and tris
+  ret = generateBVHModel(model, shape, Transform3<S>(Translation3<S>(Vector3<S>(3.0, 3.0, 3.0))), FinalizeModel::DO);
+  GTEST_ASSERT_EQ(ret, BVH_OK);
+  EXPECT_GT(model.num_vertices, num_vertices);
+  EXPECT_GT(model.num_tris, num_tris);
+  EXPECT_EQ(model.build_state, BVH_BUILD_STATE_PROCESSED);
+}
+
+
+/**
+@details   This function tests that adding geometric primitives to a finalized model indeed
+           returns the BVH error we would expect.
+**/
+template<typename BV, typename ShapeType>
+void checkAddToFinalizedModel(BVHModel<BV>& model, const ShapeType& shape)
+{
+  using S = typename BV::S;
+  const uint8_t n = 32; 
+
+  GTEST_ASSERT_EQ(model.build_state, BVH_BUILD_STATE_PROCESSED);
+  auto ret = generateBVHModel(model, shape, Transform3<S>::Identity(), n, FinalizeModel::DONT);
+  EXPECT_EQ(ret, BVH_ERR_BUILD_OUT_OF_SEQUENCE);
+}
+
+// Specialization for boxes
+template<typename BV>
+void checkAddToFinalizedModel(BVHModel<BV>& model, const Box<typename BV::S>& shape)
+{
+  using S = typename BV::S;
+
+  GTEST_ASSERT_EQ(model.build_state, BVH_BUILD_STATE_PROCESSED);
+  auto ret = generateBVHModel(model, shape, Transform3<S>::Identity(), FinalizeModel::DONT);
+  EXPECT_EQ(ret, BVH_ERR_BUILD_OUT_OF_SEQUENCE);
+}
+
+template<typename BV>
+void testBVHModelFromBox()
+{
+  using S = typename BV::S;
+  const S a = 1.0;
+  const S b = 1.0;
+  const S c = 1.0;
+
+  std::shared_ptr<BVHModel<BV>> model(new BVHModel<BV>);
+  Box<S> box(a, b, c);
+
+  checkAddToEmptyModel(*model, box);
+  checkAddToUnfinalizedModel(*model, box);
+  checkAddAndFinalizeModel(*model, box);
+  checkAddToFinalizedModel(*model, box);
+
+}
+
+template<typename BV>
+void testBVHModelFromSphere()
+{
+  using S = typename BV::S;
+  const S r = 1.0;
+
+  Sphere<S> sphere(r);
+  std::shared_ptr<BVHModel<BV>> model(new BVHModel<BV>);
+  checkAddToEmptyModel(*model, sphere);
+  checkAddToUnfinalizedModel(*model, sphere);
+  checkAddAndFinalizeModel(*model, sphere);
+  checkAddToFinalizedModel(*model, sphere);
+}
+
+template<typename BV>
+void testBVHModelFromEllipsoid()
+{
+  using S = typename BV::S;
+  const S a = 1.0;
+  const S b = 1.0;
+  const S c = 1.0;
+
+  Ellipsoid<S> ellipsoid(a, b, c);
+  std::shared_ptr<BVHModel<BV>> model(new BVHModel<BV>);
+
+  checkAddToEmptyModel(*model, ellipsoid);
+  checkAddToUnfinalizedModel(*model, ellipsoid);
+  checkAddAndFinalizeModel(*model, ellipsoid);
+  checkAddToFinalizedModel(*model, ellipsoid);
+}
+
+template<typename BV>
+void testBVHModelFromCylinder()
+{
+  using S = typename BV::S;
+  const S r = 1.0;
+  const S h = 1.0;
+
+  Cylinder<S> cylinder(r, h);
+  std::shared_ptr<BVHModel<BV>> model(new BVHModel<BV>);
+
+  checkAddToEmptyModel(*model, cylinder);
+  checkAddToUnfinalizedModel(*model, cylinder);
+  checkAddAndFinalizeModel(*model, cylinder);
+  checkAddToFinalizedModel(*model, cylinder);
+}
+
+template<typename BV>
+void testBVHModelFromCone()
+{
+ using S = typename BV::S;
+  const S r = 1.0;
+  const S h = 1.0;
+
+  Cone<S> cone(r, h);
+  std::shared_ptr<BVHModel<BV>> model(new BVHModel<BV>);
+
+  checkAddToEmptyModel(*model, cone);
+  checkAddToUnfinalizedModel(*model, cone);
+  checkAddAndFinalizeModel(*model, cone);
+  checkAddToFinalizedModel(*model, cone);
+}
+
+template<typename BV>
+void testBVHModelFromPrimitives()
+{
+  testBVHModelFromBox<BV>();
+  testBVHModelFromSphere<BV>();
+  testBVHModelFromEllipsoid<BV>();
+  testBVHModelFromCylinder<BV>();
+  testBVHModelFromCone<BV>();
+}
+
+GTEST_TEST(FCL_GENERATE_BVH_MODELS, generating_bvh_models_from_primitives)
+{
+  testBVHModelFromPrimitives<AABB<double>>();
+  testBVHModelFromPrimitives<OBB<double>>();
+  testBVHModelFromPrimitives<RSS<double>>();
+  testBVHModelFromPrimitives<kIOS<double>>();
+  testBVHModelFromPrimitives<OBBRSS<double>>();
+  testBVHModelFromPrimitives<KDOP<double, 16> >();
+  testBVHModelFromPrimitives<KDOP<double, 18> >();
+  testBVHModelFromPrimitives<KDOP<double, 24> >();
+}
+
+//==============================================================================
+int main(int argc, char* argv[])
+{
+  ::testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
diff --git a/test/test_fcl_generate_bvh_model.cpp b/test/test_fcl_generate_bvh_model_primitives.cpp
similarity index 77%
rename from test/test_fcl_generate_bvh_model.cpp
rename to test/test_fcl_generate_bvh_model_primitives.cpp
index 397915a0d..a1abfdebb 100644
--- a/test/test_fcl_generate_bvh_model.cpp
+++ b/test/test_fcl_generate_bvh_model_primitives.cpp
@@ -44,25 +44,37 @@
 using namespace fcl;
 
 /**
-@details   This function tests adding geometric primitives to a model, by first creating one
-           and then appending to it. It checks proper functionality of those simply by 
-           verifying the return value, the number of vertices, triangles and the state of the model.
+@brief      This file tests creation of BVHModels from geometric primitives.
+
+@details    It checks proper functionality of those simply by verifying the return value,
+            the number of vertices, triangles and the state of the model.
+
+@note       In the process, the provided model will always be finalized.
+
+@warning    Currently, there are no checks that the geometric primitives created are
+            actually correct in terms of tesselation resolution, dimensions, radii etc.
+            The current state of this test is a very basic "bare bones" for a future more
+            adequate test.
+
+@todo       These logic checks should be implemented by someone knowledgeable enough about
+            the inner workings of the generateBVHModel() functions.
 **/
+
+
 template<typename BV, typename ShapeType>
 void checkNumVerticesAndTris(BVHModel<BV>& model, const ShapeType& shape, uint8_t n, int vertices, int tris)
 {  
   using S = typename BV::S;
 
+  GTEST_ASSERT_EQ(model.build_state, BVH_BUILD_STATE_EMPTY);
+  GTEST_ASSERT_EQ(model.num_vertices, 0);
+  GTEST_ASSERT_EQ(model.num_tris, 0);
+
   // Add the shape to the model and count vertices and triangles to make sure it has been created
-  generateBVHModel(model, shape, Transform3<S>::Identity(), n, FinalizeModel::DONT);
+  auto ret = generateBVHModel(model, shape, Transform3<S>::Identity(), n, FinalizeModel::DO);
+  EXPECT_EQ(ret, BVH_OK);
   EXPECT_EQ(model.num_vertices, vertices);
   EXPECT_EQ(model.num_tris, tris); 
-  EXPECT_EQ(model.build_state, BVH_BUILD_STATE_BEGUN);
-
-  // Add another instance of the shape and make sure it was added to the model by counting vertices and tris
-  generateBVHModel(model, shape, Transform3<S>(Translation3<S>(Vector3<S>(2.0, 2.0, 2.0))), n);
-  EXPECT_EQ(model.num_vertices, 2*vertices);
-  EXPECT_EQ(model.num_tris, 2*tris);
   EXPECT_EQ(model.build_state, BVH_BUILD_STATE_PROCESSED);
 }
 
@@ -71,38 +83,17 @@ template<typename BV>
 void checkNumVerticesAndTris(BVHModel<BV>& model, const Box<typename BV::S>& shape, int vertices, int tris)
 {
   using S = typename BV::S;
-  auto ret  = generateBVHModel(model, shape, Transform3<S>::Identity(), FinalizeModel::DONT);
+
+  GTEST_ASSERT_EQ(model.build_state, BVH_BUILD_STATE_EMPTY);
+  GTEST_ASSERT_EQ(model.num_vertices, 0);
+  GTEST_ASSERT_EQ(model.num_tris, 0);
+
+  // Add the shape to the model and count vertices and triangles to make sure it has been created
+  auto ret = generateBVHModel(model, shape, Transform3<S>::Identity(), FinalizeModel::DO);
   EXPECT_EQ(ret, BVH_OK);
   EXPECT_EQ(model.num_vertices, vertices);
   EXPECT_EQ(model.num_tris, tris); 
-  EXPECT_EQ(model.build_state, BVH_BUILD_STATE_BEGUN);
-  ret = generateBVHModel(model, shape, Transform3<S>(Translation3<S>(Vector3<S>(2.0, 2.0, 2.0))));
-  EXPECT_EQ(ret, BVH_OK);
-  EXPECT_EQ(model.num_vertices, 2*vertices);
-  EXPECT_EQ(model.num_tris, 2*tris);
-  EXPECT_EQ(model.build_state, BVH_BUILD_STATE_PROCESSED);
-}
-
-/**
-@details   This function tests that adding geometric primitives to a finalized model indeed
-           returns the BVH error we would expect.
-**/
-template<typename BV, typename ShapeType>
-void checkAddToFinishedModel(BVHModel<BV>& model, const ShapeType& shape, uint8_t n)
-{
-  using S = typename BV::S;
-  EXPECT_EQ(model.build_state, BVH_BUILD_STATE_PROCESSED);
-  auto ret = generateBVHModel(model, shape, Transform3<S>::Identity(), n, FinalizeModel::DONT);
-  EXPECT_EQ(ret, BVH_ERR_BUILD_OUT_OF_SEQUENCE);
-}
-
-template<typename BV>
-void checkAddToFinishedModel(BVHModel<BV>& model, const Box<typename BV::S>& shape)
-{
-  using S = typename BV::S;
   EXPECT_EQ(model.build_state, BVH_BUILD_STATE_PROCESSED);
-  auto ret = generateBVHModel(model, shape, Transform3<S>::Identity(), FinalizeModel::DONT);
-  EXPECT_EQ(ret, BVH_ERR_BUILD_OUT_OF_SEQUENCE);
 }
 
 template<typename BV>
@@ -119,7 +110,6 @@ void testBVHModelFromBox()
         Box<S> box(a, b, c);
 
         checkNumVerticesAndTris(*model, box, 8, 12);
-        checkAddToFinishedModel(*model, box);
       }
     }
   }
@@ -142,7 +132,6 @@ void testBVHModelFromSphere()
       std::shared_ptr<BVHModel<BV>> model(new BVHModel<BV>);
   
       checkNumVerticesAndTris(*model, sphere, n, static_cast<int>(2 + ring * ring), static_cast<int>(2 * ring * ring));
-      checkAddToFinishedModel(*model, sphere, n);
     }
   }
 }
@@ -170,7 +159,6 @@ void testBVHModelFromEllipsoid()
           std::shared_ptr<BVHModel<BV>> model(new BVHModel<BV>);
 
           checkNumVerticesAndTris(*model, ellipsoid, n, static_cast<int>(2 + ring * ring), static_cast<int>(2 * ring * ring));
-          checkAddToFinishedModel(*model, ellipsoid, n);
         }
       }
     }
@@ -195,7 +183,6 @@ void testBVHModelFromCylinder()
         std::shared_ptr<BVHModel<BV>> model(new BVHModel<BV>);
 
         checkNumVerticesAndTris(*model, cylinder, n, static_cast<int>(2 + n_tot * (h_num + 1)), static_cast<int>((2 * h_num + 2) * n_tot));
-        checkAddToFinishedModel(*model, cylinder, n);
       }
     }
   }
@@ -219,7 +206,6 @@ void testBVHModelFromCone()
 
         std::shared_ptr<BVHModel<BV>> model(new BVHModel<BV>);
         checkNumVerticesAndTris(*model, cone, n, static_cast<int>(2 + n_tot * h_num), static_cast<int>(2 * n_tot * h_num));
-        checkAddToFinishedModel(*model, cone, n);
       }
     }
   }

From 943b65ec7cee1fba433da398bd945306b8cffe64 Mon Sep 17 00:00:00 2001
From: Nico van Duijn <nico@auterion.com>
Date: Wed, 11 Jul 2018 08:32:15 +0200
Subject: [PATCH 22/24] Add hard-coding in unit test for box

---
 .../geometry/geometric_shape_to_BVH_model.h    |  2 +-
 ...cl_generate_bvh_model_deferred_finalize.cpp | 18 ++++++++----------
 2 files changed, 9 insertions(+), 11 deletions(-)

diff --git a/include/fcl/geometry/geometric_shape_to_BVH_model.h b/include/fcl/geometry/geometric_shape_to_BVH_model.h
index 294fd7eb4..d44350d38 100644
--- a/include/fcl/geometry/geometric_shape_to_BVH_model.h
+++ b/include/fcl/geometry/geometric_shape_to_BVH_model.h
@@ -77,7 +77,7 @@ enum class FinalizeModel{
 @param[in]   shape The geometric object to be added to the BVHModel
 @param[in]   pose The pose of the geometric object
 @param[in]   finalize_model an enum indicating whether the model is final or more submodels can be added later
-@return      BVHReturnCode indicating the success of the operation
+@return      Return code (as defined by BVHReturnCode) indicating the success of the operation
 @{
 */
 
diff --git a/test/test_fcl_generate_bvh_model_deferred_finalize.cpp b/test/test_fcl_generate_bvh_model_deferred_finalize.cpp
index bd5569fbe..11e9a0dc3 100644
--- a/test/test_fcl_generate_bvh_model_deferred_finalize.cpp
+++ b/test/test_fcl_generate_bvh_model_deferred_finalize.cpp
@@ -90,16 +90,14 @@ void checkAddToEmptyModel(BVHModel<BV>& model, const Box<typename BV::S>& shape)
 
   // Make sure we are given an empty model
   GTEST_ASSERT_EQ(model.build_state, BVH_BUILD_STATE_EMPTY);
-  uint8_t num_vertices = model.num_vertices;
-  uint8_t num_tris = model.num_tris;
-  GTEST_ASSERT_EQ(num_vertices, 0);
-  GTEST_ASSERT_EQ(num_tris, 0);
+  GTEST_ASSERT_EQ(model.num_vertices, 0);
+  GTEST_ASSERT_EQ(model.num_tris, 0);
 
   // Add the shape to the model and count vertices and triangles to make sure it has been created
   ret = generateBVHModel(model, shape, Transform3<S>::Identity(), FinalizeModel::DONT);
   GTEST_ASSERT_EQ(ret, BVH_OK);
-  EXPECT_GT(model.num_vertices, num_vertices);
-  EXPECT_GT(model.num_tris, num_tris); 
+  EXPECT_EQ(model.num_vertices, 8);
+  EXPECT_EQ(model.num_tris, 12); 
   EXPECT_EQ(model.build_state, BVH_BUILD_STATE_BEGUN);
 }
 
@@ -145,8 +143,8 @@ void checkAddToUnfinalizedModel(BVHModel<BV>& model, const Box<typename BV::S>&
   // Add the shape to the model and count vertices and triangles to make sure it has been created
   ret = generateBVHModel(model, shape, Transform3<S>(Translation3<S>(Vector3<S>(2.0, 2.0, 2.0))), FinalizeModel::DONT);
   GTEST_ASSERT_EQ(ret, BVH_OK);
-  EXPECT_GT(model.num_vertices, num_vertices);
-  EXPECT_GT(model.num_tris, num_tris); 
+  EXPECT_EQ(model.num_vertices, num_vertices + 8);
+  EXPECT_EQ(model.num_tris, num_tris + 12); 
   EXPECT_EQ(model.build_state, BVH_BUILD_STATE_BEGUN);
 }
 
@@ -190,8 +188,8 @@ void checkAddAndFinalizeModel(BVHModel<BV>& model, const Box<typename BV::S>& sh
   // Add another instance of the shape and make sure it was added to the model by counting vertices and tris
   ret = generateBVHModel(model, shape, Transform3<S>(Translation3<S>(Vector3<S>(3.0, 3.0, 3.0))), FinalizeModel::DO);
   GTEST_ASSERT_EQ(ret, BVH_OK);
-  EXPECT_GT(model.num_vertices, num_vertices);
-  EXPECT_GT(model.num_tris, num_tris);
+  EXPECT_EQ(model.num_vertices, num_vertices + 8);
+  EXPECT_EQ(model.num_tris, num_tris + 12);
   EXPECT_EQ(model.build_state, BVH_BUILD_STATE_PROCESSED);
 }
 

From 3a480ac6156dc170e01ffe5b411b9ff0e3627bf1 Mon Sep 17 00:00:00 2001
From: Nico van Duijn <nico@auterion.com>
Date: Wed, 11 Jul 2018 08:34:51 +0200
Subject: [PATCH 23/24] Remove unit test for primitive generation

---
 test/CMakeLists.txt                           |   1 -
 ...test_fcl_generate_bvh_model_primitives.cpp | 241 ------------------
 2 files changed, 242 deletions(-)
 delete mode 100644 test/test_fcl_generate_bvh_model_primitives.cpp

diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 2b6c18171..b21c05adb 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -53,7 +53,6 @@ set(tests
     test_fcl_frontlist.cpp
     test_fcl_general.cpp
     test_fcl_generate_bvh_model_deferred_finalize.cpp
-    test_fcl_generate_bvh_model_primitives.cpp
     test_fcl_geometric_shapes.cpp
     test_fcl_math.cpp
     test_fcl_profiler.cpp
diff --git a/test/test_fcl_generate_bvh_model_primitives.cpp b/test/test_fcl_generate_bvh_model_primitives.cpp
deleted file mode 100644
index a1abfdebb..000000000
--- a/test/test_fcl_generate_bvh_model_primitives.cpp
+++ /dev/null
@@ -1,241 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2016, Open Source Robotics Foundation
- *  All rights reserved.
- *
- *  Redistribution and use in source and binary forms, with or without
- *  modification, are permitted provided that the following conditions
- *  are met:
- *
- *   * Redistributions of source code must retain the above copyright
- *     notice, this list of conditions and the following disclaimer.
- *   * Redistributions in binary form must reproduce the above
- *     copyright notice, this list of conditions and the following
- *     disclaimer in the documentation and/or other materials provided
- *     with the distribution.
- *   * Neither the name of Open Source Robotics Foundation nor the names of its
- *     contributors may be used to endorse or promote products derived
- *     from this software without specific prior written permission.
- *
- *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- *  POSSIBILITY OF SUCH DAMAGE.
- */
-
-/** @author Nico van Duijn */
-
-#include <gtest/gtest.h>
-
-#include "fcl/config.h"
-#include "fcl/geometry/geometric_shape_to_BVH_model.h"
-#include "test_fcl_utility.h"
-#include <iostream>
-
-using namespace fcl;
-
-/**
-@brief      This file tests creation of BVHModels from geometric primitives.
-
-@details    It checks proper functionality of those simply by verifying the return value,
-            the number of vertices, triangles and the state of the model.
-
-@note       In the process, the provided model will always be finalized.
-
-@warning    Currently, there are no checks that the geometric primitives created are
-            actually correct in terms of tesselation resolution, dimensions, radii etc.
-            The current state of this test is a very basic "bare bones" for a future more
-            adequate test.
-
-@todo       These logic checks should be implemented by someone knowledgeable enough about
-            the inner workings of the generateBVHModel() functions.
-**/
-
-
-template<typename BV, typename ShapeType>
-void checkNumVerticesAndTris(BVHModel<BV>& model, const ShapeType& shape, uint8_t n, int vertices, int tris)
-{  
-  using S = typename BV::S;
-
-  GTEST_ASSERT_EQ(model.build_state, BVH_BUILD_STATE_EMPTY);
-  GTEST_ASSERT_EQ(model.num_vertices, 0);
-  GTEST_ASSERT_EQ(model.num_tris, 0);
-
-  // Add the shape to the model and count vertices and triangles to make sure it has been created
-  auto ret = generateBVHModel(model, shape, Transform3<S>::Identity(), n, FinalizeModel::DO);
-  EXPECT_EQ(ret, BVH_OK);
-  EXPECT_EQ(model.num_vertices, vertices);
-  EXPECT_EQ(model.num_tris, tris); 
-  EXPECT_EQ(model.build_state, BVH_BUILD_STATE_PROCESSED);
-}
-
-// Slightly different for boxes, cannot define numFaces
-template<typename BV>
-void checkNumVerticesAndTris(BVHModel<BV>& model, const Box<typename BV::S>& shape, int vertices, int tris)
-{
-  using S = typename BV::S;
-
-  GTEST_ASSERT_EQ(model.build_state, BVH_BUILD_STATE_EMPTY);
-  GTEST_ASSERT_EQ(model.num_vertices, 0);
-  GTEST_ASSERT_EQ(model.num_tris, 0);
-
-  // Add the shape to the model and count vertices and triangles to make sure it has been created
-  auto ret = generateBVHModel(model, shape, Transform3<S>::Identity(), FinalizeModel::DO);
-  EXPECT_EQ(ret, BVH_OK);
-  EXPECT_EQ(model.num_vertices, vertices);
-  EXPECT_EQ(model.num_tris, tris); 
-  EXPECT_EQ(model.build_state, BVH_BUILD_STATE_PROCESSED);
-}
-
-template<typename BV>
-void testBVHModelFromBox()
-{
-  using S = typename BV::S;
-
-  // Test various box sizes
-  for(S a : {0.5, 1.0, 2.0, 3.1}){
-    for(S b : {0.5, 1.0, 2.0, 3.1}){
-      for(S c : {0.5, 1.0, 2.0, 3.1}){
-
-        std::shared_ptr<BVHModel<BV>> model(new BVHModel<BV>);
-        Box<S> box(a, b, c);
-
-        checkNumVerticesAndTris(*model, box, 8, 12);
-      }
-    }
-  }
-}
-
-template<typename BV>
-void testBVHModelFromSphere()
-{
-  using S = typename BV::S;
-
-  // Test various radii
-  for(S r : {0.5, 1.0, 2.0, 3.1}){
-    Sphere<S> sphere(r);
-
-    // Test various resolutions
-    for(uint8_t n : {4, 8, 16, 32}){
-      S n_low_bound = sqrtf(n / 2.0) * r * r;
-      unsigned int ring = ceil(n_low_bound);
-
-      std::shared_ptr<BVHModel<BV>> model(new BVHModel<BV>);
-  
-      checkNumVerticesAndTris(*model, sphere, n, static_cast<int>(2 + ring * ring), static_cast<int>(2 * ring * ring));
-    }
-  }
-}
-
-template<typename BV>
-void testBVHModelFromEllipsoid()
-{
-  using S = typename BV::S;
-  const S p = 1.6075;
-
-  // Test various radii
-  for(S a : {0.5, 1.0, 2.0, 3.1}){
-    for(S b : {0.5, 1.0, 2.0, 3.1}){
-      for(S c : {0.5, 1.0, 2.0, 3.1}){
-        Ellipsoid<S> ellipsoid(a, b, c);
-        const S& ap = std::pow(a, p);
-        const S& bp = std::pow(b, p);
-        const S& cp = std::pow(c, p);
-        const S ratio = std::pow((ap * bp + bp * cp + cp * ap) / 3.0, 1.0 / p);
-
-        // Test various resolutions
-        for(uint8_t n : {4, 8, 16, 32}){
-          const S n_low_bound = std::sqrt(n / 2.0) * ratio;
-          const unsigned int ring = std::ceil(n_low_bound);
-          std::shared_ptr<BVHModel<BV>> model(new BVHModel<BV>);
-
-          checkNumVerticesAndTris(*model, ellipsoid, n, static_cast<int>(2 + ring * ring), static_cast<int>(2 * ring * ring));
-        }
-      }
-    }
-  }
-}
-
-template<typename BV>
-void testBVHModelFromCylinder()
-{
-  using S = typename BV::S;
-  const S pi = constants<S>::pi();
-
-  // Try various resolutions, radii and heights
-  for(S r : {0.5, 1.0, 2.0, 3.1}){
-    for(S h : {0.5, 1.0, 2.0, 3.1}){
-      Cylinder<S> cylinder(r, h);
-
-      for(uint8_t n : {4, 8, 16, 32}){
-        unsigned int n_tot = n * r;
-        unsigned int h_num = ceil(h / ((pi * 2 / n_tot) * r));
-
-        std::shared_ptr<BVHModel<BV>> model(new BVHModel<BV>);
-
-        checkNumVerticesAndTris(*model, cylinder, n, static_cast<int>(2 + n_tot * (h_num + 1)), static_cast<int>((2 * h_num + 2) * n_tot));
-      }
-    }
-  }
-}
-
-template<typename BV>
-void testBVHModelFromCone()
-{
-
-  using S = typename BV::S;
-  const S pi = constants<S>::pi();
-
-  // Try various resolutions, radii and heights
-  for(S r : {0.5, 1.0, 2.0, 3.1}){
-    for(S h : {0.5, 1.0, 2.0, 3.1}){
-      Cone<S> cone(r,h);
-  
-      for(uint8_t n : {4, 8, 16, 32}){
-        unsigned int n_tot = n * r;
-        unsigned int h_num = ceil(h / ((pi * 2 / n_tot) * r));
-
-        std::shared_ptr<BVHModel<BV>> model(new BVHModel<BV>);
-        checkNumVerticesAndTris(*model, cone, n, static_cast<int>(2 + n_tot * h_num), static_cast<int>(2 * n_tot * h_num));
-      }
-    }
-  }
-}
-
-template<typename BV>
-void testBVHModelFromPrimitives()
-{
-  testBVHModelFromBox<BV>();
-  testBVHModelFromSphere<BV>();
-  testBVHModelFromEllipsoid<BV>();
-  testBVHModelFromCylinder<BV>();
-  testBVHModelFromCone<BV>();
-}
-
-GTEST_TEST(FCL_GENERATE_BVH_MODELS, generating_bvh_models_from_primitives)
-{
-  testBVHModelFromPrimitives<AABB<double>>();
-  testBVHModelFromPrimitives<OBB<double>>();
-  testBVHModelFromPrimitives<RSS<double>>();
-  testBVHModelFromPrimitives<kIOS<double>>();
-  testBVHModelFromPrimitives<OBBRSS<double>>();
-  testBVHModelFromPrimitives<KDOP<double, 16> >();
-  testBVHModelFromPrimitives<KDOP<double, 18> >();
-  testBVHModelFromPrimitives<KDOP<double, 24> >();
-}
-
-//==============================================================================
-int main(int argc, char* argv[])
-{
-  ::testing::InitGoogleTest(&argc, argv);
-  return RUN_ALL_TESTS();
-}

From e2ce071d51c22cd03e2ab8e9aa8d2e118eaa2d86 Mon Sep 17 00:00:00 2001
From: Nico van Duijn <nico@auterion.com>
Date: Wed, 11 Jul 2018 08:46:33 +0200
Subject: [PATCH 24/24] WIP add unit test for primitive generation

WIP

This reverts commit 3a480ac6156dc170e01ffe5b411b9ff0e3627bf1.
Essentially adding the bare bones for a unit test that ensures
proper BVHModel generation from geometric primitives. So far
this test only checks the number of vertices and triangles, which
is insufficient to assert proper functionality of the
generateBVHModel() functions.
---
 test/CMakeLists.txt                           |   1 +
 ...test_fcl_generate_bvh_model_primitives.cpp | 241 ++++++++++++++++++
 2 files changed, 242 insertions(+)
 create mode 100644 test/test_fcl_generate_bvh_model_primitives.cpp

diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index b21c05adb..2b6c18171 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -53,6 +53,7 @@ set(tests
     test_fcl_frontlist.cpp
     test_fcl_general.cpp
     test_fcl_generate_bvh_model_deferred_finalize.cpp
+    test_fcl_generate_bvh_model_primitives.cpp
     test_fcl_geometric_shapes.cpp
     test_fcl_math.cpp
     test_fcl_profiler.cpp
diff --git a/test/test_fcl_generate_bvh_model_primitives.cpp b/test/test_fcl_generate_bvh_model_primitives.cpp
new file mode 100644
index 000000000..a1abfdebb
--- /dev/null
+++ b/test/test_fcl_generate_bvh_model_primitives.cpp
@@ -0,0 +1,241 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2016, Open Source Robotics Foundation
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Open Source Robotics Foundation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** @author Nico van Duijn */
+
+#include <gtest/gtest.h>
+
+#include "fcl/config.h"
+#include "fcl/geometry/geometric_shape_to_BVH_model.h"
+#include "test_fcl_utility.h"
+#include <iostream>
+
+using namespace fcl;
+
+/**
+@brief      This file tests creation of BVHModels from geometric primitives.
+
+@details    It checks proper functionality of those simply by verifying the return value,
+            the number of vertices, triangles and the state of the model.
+
+@note       In the process, the provided model will always be finalized.
+
+@warning    Currently, there are no checks that the geometric primitives created are
+            actually correct in terms of tesselation resolution, dimensions, radii etc.
+            The current state of this test is a very basic "bare bones" for a future more
+            adequate test.
+
+@todo       These logic checks should be implemented by someone knowledgeable enough about
+            the inner workings of the generateBVHModel() functions.
+**/
+
+
+template<typename BV, typename ShapeType>
+void checkNumVerticesAndTris(BVHModel<BV>& model, const ShapeType& shape, uint8_t n, int vertices, int tris)
+{  
+  using S = typename BV::S;
+
+  GTEST_ASSERT_EQ(model.build_state, BVH_BUILD_STATE_EMPTY);
+  GTEST_ASSERT_EQ(model.num_vertices, 0);
+  GTEST_ASSERT_EQ(model.num_tris, 0);
+
+  // Add the shape to the model and count vertices and triangles to make sure it has been created
+  auto ret = generateBVHModel(model, shape, Transform3<S>::Identity(), n, FinalizeModel::DO);
+  EXPECT_EQ(ret, BVH_OK);
+  EXPECT_EQ(model.num_vertices, vertices);
+  EXPECT_EQ(model.num_tris, tris); 
+  EXPECT_EQ(model.build_state, BVH_BUILD_STATE_PROCESSED);
+}
+
+// Slightly different for boxes, cannot define numFaces
+template<typename BV>
+void checkNumVerticesAndTris(BVHModel<BV>& model, const Box<typename BV::S>& shape, int vertices, int tris)
+{
+  using S = typename BV::S;
+
+  GTEST_ASSERT_EQ(model.build_state, BVH_BUILD_STATE_EMPTY);
+  GTEST_ASSERT_EQ(model.num_vertices, 0);
+  GTEST_ASSERT_EQ(model.num_tris, 0);
+
+  // Add the shape to the model and count vertices and triangles to make sure it has been created
+  auto ret = generateBVHModel(model, shape, Transform3<S>::Identity(), FinalizeModel::DO);
+  EXPECT_EQ(ret, BVH_OK);
+  EXPECT_EQ(model.num_vertices, vertices);
+  EXPECT_EQ(model.num_tris, tris); 
+  EXPECT_EQ(model.build_state, BVH_BUILD_STATE_PROCESSED);
+}
+
+template<typename BV>
+void testBVHModelFromBox()
+{
+  using S = typename BV::S;
+
+  // Test various box sizes
+  for(S a : {0.5, 1.0, 2.0, 3.1}){
+    for(S b : {0.5, 1.0, 2.0, 3.1}){
+      for(S c : {0.5, 1.0, 2.0, 3.1}){
+
+        std::shared_ptr<BVHModel<BV>> model(new BVHModel<BV>);
+        Box<S> box(a, b, c);
+
+        checkNumVerticesAndTris(*model, box, 8, 12);
+      }
+    }
+  }
+}
+
+template<typename BV>
+void testBVHModelFromSphere()
+{
+  using S = typename BV::S;
+
+  // Test various radii
+  for(S r : {0.5, 1.0, 2.0, 3.1}){
+    Sphere<S> sphere(r);
+
+    // Test various resolutions
+    for(uint8_t n : {4, 8, 16, 32}){
+      S n_low_bound = sqrtf(n / 2.0) * r * r;
+      unsigned int ring = ceil(n_low_bound);
+
+      std::shared_ptr<BVHModel<BV>> model(new BVHModel<BV>);
+  
+      checkNumVerticesAndTris(*model, sphere, n, static_cast<int>(2 + ring * ring), static_cast<int>(2 * ring * ring));
+    }
+  }
+}
+
+template<typename BV>
+void testBVHModelFromEllipsoid()
+{
+  using S = typename BV::S;
+  const S p = 1.6075;
+
+  // Test various radii
+  for(S a : {0.5, 1.0, 2.0, 3.1}){
+    for(S b : {0.5, 1.0, 2.0, 3.1}){
+      for(S c : {0.5, 1.0, 2.0, 3.1}){
+        Ellipsoid<S> ellipsoid(a, b, c);
+        const S& ap = std::pow(a, p);
+        const S& bp = std::pow(b, p);
+        const S& cp = std::pow(c, p);
+        const S ratio = std::pow((ap * bp + bp * cp + cp * ap) / 3.0, 1.0 / p);
+
+        // Test various resolutions
+        for(uint8_t n : {4, 8, 16, 32}){
+          const S n_low_bound = std::sqrt(n / 2.0) * ratio;
+          const unsigned int ring = std::ceil(n_low_bound);
+          std::shared_ptr<BVHModel<BV>> model(new BVHModel<BV>);
+
+          checkNumVerticesAndTris(*model, ellipsoid, n, static_cast<int>(2 + ring * ring), static_cast<int>(2 * ring * ring));
+        }
+      }
+    }
+  }
+}
+
+template<typename BV>
+void testBVHModelFromCylinder()
+{
+  using S = typename BV::S;
+  const S pi = constants<S>::pi();
+
+  // Try various resolutions, radii and heights
+  for(S r : {0.5, 1.0, 2.0, 3.1}){
+    for(S h : {0.5, 1.0, 2.0, 3.1}){
+      Cylinder<S> cylinder(r, h);
+
+      for(uint8_t n : {4, 8, 16, 32}){
+        unsigned int n_tot = n * r;
+        unsigned int h_num = ceil(h / ((pi * 2 / n_tot) * r));
+
+        std::shared_ptr<BVHModel<BV>> model(new BVHModel<BV>);
+
+        checkNumVerticesAndTris(*model, cylinder, n, static_cast<int>(2 + n_tot * (h_num + 1)), static_cast<int>((2 * h_num + 2) * n_tot));
+      }
+    }
+  }
+}
+
+template<typename BV>
+void testBVHModelFromCone()
+{
+
+  using S = typename BV::S;
+  const S pi = constants<S>::pi();
+
+  // Try various resolutions, radii and heights
+  for(S r : {0.5, 1.0, 2.0, 3.1}){
+    for(S h : {0.5, 1.0, 2.0, 3.1}){
+      Cone<S> cone(r,h);
+  
+      for(uint8_t n : {4, 8, 16, 32}){
+        unsigned int n_tot = n * r;
+        unsigned int h_num = ceil(h / ((pi * 2 / n_tot) * r));
+
+        std::shared_ptr<BVHModel<BV>> model(new BVHModel<BV>);
+        checkNumVerticesAndTris(*model, cone, n, static_cast<int>(2 + n_tot * h_num), static_cast<int>(2 * n_tot * h_num));
+      }
+    }
+  }
+}
+
+template<typename BV>
+void testBVHModelFromPrimitives()
+{
+  testBVHModelFromBox<BV>();
+  testBVHModelFromSphere<BV>();
+  testBVHModelFromEllipsoid<BV>();
+  testBVHModelFromCylinder<BV>();
+  testBVHModelFromCone<BV>();
+}
+
+GTEST_TEST(FCL_GENERATE_BVH_MODELS, generating_bvh_models_from_primitives)
+{
+  testBVHModelFromPrimitives<AABB<double>>();
+  testBVHModelFromPrimitives<OBB<double>>();
+  testBVHModelFromPrimitives<RSS<double>>();
+  testBVHModelFromPrimitives<kIOS<double>>();
+  testBVHModelFromPrimitives<OBBRSS<double>>();
+  testBVHModelFromPrimitives<KDOP<double, 16> >();
+  testBVHModelFromPrimitives<KDOP<double, 18> >();
+  testBVHModelFromPrimitives<KDOP<double, 24> >();
+}
+
+//==============================================================================
+int main(int argc, char* argv[])
+{
+  ::testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}