Add function to build convex hull of set of points (using qhull)

CMakeLists.txt

@@ -98,6 +98,24 @@ else()
   message(STATUS "FCL does not use Octomap")
 endif()
 
+option(HPP_FCL_HAS_QHULL "use qhull library to compute convex hulls." FALSE)
+if(HPP_FCL_HAS_QHULL)
+  file(MAKE_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/third-parties)
+  execute_process(COMMAND ${CMAKE_COMMAND} -E create_symlink
+    ${CMAKE_SOURCE_DIR}/third-parties/qhull/src/libqhullcpp
+    ${CMAKE_CURRENT_BINARY_DIR}/third-parties/libqhullcpp
+    )
+  set(Qhullcpp_PREFIX ${CMAKE_BINARY_DIR}/third-parties)
+  find_path(Qhull_r_INCLUDE_DIR
+    NAMES libqhull_r/libqhull_r.h
+    PATHS ${Qhull_PREFIX}
+    )
+  find_library(Qhull_r_LIBRARY
+    NAMES libqhull_r.so
+    PATHS ${Qhull_PREFIX}
+    )
+endif()
+
 ADD_REQUIRED_DEPENDENCY("assimp >= 2.0")
 
 SET(${PROJECT_NAME}_HEADERS

include/hpp/fcl/shape/geometric_shapes.h

@@ -281,6 +281,20 @@ public:
 class ConvexBase : public ShapeBase
 {
 public:
+  /// @brief Build a convex hull based on Qhull library
+  /// and store the vertices and optionally the triangles
+  /// \param points, num_points the points whose convex hull should be computed.
+  /// \param keepTriangles if \c true, returns a Convex<Triangle> object which
+  ///        contains the triangle of the shape.
+  /// \param qhullCommand the command sent to qhull.
+  ///        - if \c keepTriangles is \c true, this parameter should include
+  ///          "Qt". If \c NULL, "Qt" is passed to Qhull.
+  ///        - if \c keepTriangles is \c false, an empty string is passed to
+  ///          Qhull.
+  /// \note hpp-fcl must have been compiled with option \c HPP_FCL_HAS_QHULL set
+  ///       to \c ON.
+  static ConvexBase* convexHull (const Vec3f* points, int num_points,
+      bool keepTriangles, const char* qhullCommand = NULL);
 
   virtual ~ConvexBase();
 

src/CMakeLists.txt

@@ -48,6 +48,7 @@ set(${LIBRARY_NAME}_SOURCES
   narrowphase/narrowphase.cpp
   narrowphase/gjk.cpp
   narrowphase/details.h
+  shape/convex.cpp
   shape/geometric_shapes.cpp
   shape/geometric_shapes_utility.cpp
   distance_box_halfspace.cpp
@@ -79,6 +80,67 @@ set(${LIBRARY_NAME}_SOURCES
   mesh_loader/loader.cpp
   )
 
+if(HPP_FCL_HAS_QHULL)
+  set(
+    libqhullcpp_HEADERS
+    ${Qhullcpp_PREFIX}/libqhullcpp/Coordinates.h
+    ${Qhullcpp_PREFIX}/libqhullcpp/functionObjects.h
+    ${Qhullcpp_PREFIX}/libqhullcpp/PointCoordinates.h
+    ${Qhullcpp_PREFIX}/libqhullcpp/Qhull.h
+    ${Qhullcpp_PREFIX}/libqhullcpp/QhullError.h
+    ${Qhullcpp_PREFIX}/libqhullcpp/QhullFacet.h
+    ${Qhullcpp_PREFIX}/libqhullcpp/QhullFacetList.h
+    ${Qhullcpp_PREFIX}/libqhullcpp/QhullFacetSet.h
+    ${Qhullcpp_PREFIX}/libqhullcpp/QhullHyperplane.h
+    ${Qhullcpp_PREFIX}/libqhullcpp/QhullIterator.h
+    ${Qhullcpp_PREFIX}/libqhullcpp/QhullLinkedList.h
+    ${Qhullcpp_PREFIX}/libqhullcpp/QhullPoint.h
+    ${Qhullcpp_PREFIX}/libqhullcpp/QhullPoints.h
+    ${Qhullcpp_PREFIX}/libqhullcpp/QhullPointSet.h
+    ${Qhullcpp_PREFIX}/libqhullcpp/QhullQh.h
+    ${Qhullcpp_PREFIX}/libqhullcpp/QhullRidge.h
+    ${Qhullcpp_PREFIX}/libqhullcpp/QhullSet.h
+    ${Qhullcpp_PREFIX}/libqhullcpp/QhullSets.h
+    ${Qhullcpp_PREFIX}/libqhullcpp/QhullStat.h
+    ${Qhullcpp_PREFIX}/libqhullcpp/QhullVertex.h
+    ${Qhullcpp_PREFIX}/libqhullcpp/QhullVertexSet.h
+    ${Qhullcpp_PREFIX}/libqhullcpp/RboxPoints.h
+    ${Qhullcpp_PREFIX}/libqhullcpp/RoadError.h
+    ${Qhullcpp_PREFIX}/libqhullcpp/RoadLogEvent.h
+    )
+
+  set(
+    libqhullcpp_SOURCES
+    ${Qhullcpp_PREFIX}/libqhullcpp/Coordinates.cpp
+    ${Qhullcpp_PREFIX}/libqhullcpp/PointCoordinates.cpp
+    ${Qhullcpp_PREFIX}/libqhullcpp/Qhull.cpp
+    ${Qhullcpp_PREFIX}/libqhullcpp/QhullFacet.cpp
+    ${Qhullcpp_PREFIX}/libqhullcpp/QhullFacetList.cpp
+    ${Qhullcpp_PREFIX}/libqhullcpp/QhullFacetSet.cpp
+    ${Qhullcpp_PREFIX}/libqhullcpp/QhullHyperplane.cpp
+    ${Qhullcpp_PREFIX}/libqhullcpp/QhullPoint.cpp
+    ${Qhullcpp_PREFIX}/libqhullcpp/QhullPointSet.cpp
+    ${Qhullcpp_PREFIX}/libqhullcpp/QhullPoints.cpp
+    ${Qhullcpp_PREFIX}/libqhullcpp/QhullQh.cpp
+    ${Qhullcpp_PREFIX}/libqhullcpp/QhullRidge.cpp
+    ${Qhullcpp_PREFIX}/libqhullcpp/QhullSet.cpp
+    ${Qhullcpp_PREFIX}/libqhullcpp/QhullStat.cpp
+    ${Qhullcpp_PREFIX}/libqhullcpp/QhullVertex.cpp
+    ${Qhullcpp_PREFIX}/libqhullcpp/QhullVertexSet.cpp
+    ${Qhullcpp_PREFIX}/libqhullcpp/RboxPoints.cpp
+    ${Qhullcpp_PREFIX}/libqhullcpp/RoadError.cpp
+    ${Qhullcpp_PREFIX}/libqhullcpp/RoadLogEvent.cpp
+    ${libqhullcpp_HEADERS}
+    )
+
+  # TODO We compile qhullcpp because it is not provided in the binary package while
+  # the other parts of Qhull are released.
+  # Compile Qhull package as found on github leads to a link error (because it generates
+  # only a static library). One should be careful that the version of the qhull submodule
+  # of this git repo matches the version of qhull provided by the system.
+  list(APPEND ${LIBRARY_NAME}_SOURCES ${libqhullcpp_SOURCES})
+endif()
+
 SET(PROJECT_HEADERS_FULL_PATH)
 FOREACH(header ${${PROJECT_NAME}_HEADERS})
   LIST(APPEND PROJECT_HEADERS_FULL_PATH ${PROJECT_SOURCE_DIR}/${header})
@@ -101,6 +163,13 @@ TARGET_LINK_LIBRARIES(${LIBRARY_NAME}
   Boost::system
   )
 
+if(HPP_FCL_HAS_QHULL)
+  target_compile_definitions(${LIBRARY_NAME} PRIVATE -DHPP_FCL_HAS_QHULL)
+  target_include_directories(${LIBRARY_NAME} SYSTEM PRIVATE
+    ${Qhull_r_INCLUDE_DIR} ${Qhullcpp_PREFIX})
+  target_link_libraries(${LIBRARY_NAME} PRIVATE "${Qhull_r_LIBRARY}")
+endif()
+
 target_include_directories(${LIBRARY_NAME}
   SYSTEM PUBLIC
   ${EIGEN3_INCLUDE_DIR}

src/shape/convex.cpp

+#include <hpp/fcl/shape/convex.h>
+
+#ifdef HPP_FCL_HAS_QHULL
+#include <libqhullcpp/QhullError.h>
+#include <libqhullcpp/QhullFacet.h>
+#include <libqhullcpp/QhullLinkedList.h>
+#include <libqhullcpp/QhullVertex.h>
+#include <libqhullcpp/QhullVertexSet.h>
+#include <libqhullcpp/QhullRidge.h>
+#include <libqhullcpp/Qhull.h>
+
+using orgQhull::Qhull;
+using orgQhull::QhullFacet;
+using orgQhull::QhullPoint;
+using orgQhull::QhullVertex;
+using orgQhull::QhullVertexList;
+using orgQhull::QhullVertexSet;
+using orgQhull::QhullRidgeSet;
+#endif
+
+namespace hpp {
+namespace fcl {
+
+ConvexBase* ConvexBase::convexHull(const Vec3f* pts, int num_points,
+    bool keepTriangles, const char* qhullCommand)
+{
+#ifdef HPP_FCL_HAS_QHULL
+  if (num_points <= 3) {
+    throw std::invalid_argument("You shouldn't use this function with less than"
+        " 4 points.");
+  }
+  assert(pts[0].data() + 3 == pts[1].data());
+
+  Qhull qh;
+  const char* command = qhullCommand ? qhullCommand : (keepTriangles ? "Qt" : "");
+  qh.runQhull("", 3, num_points, pts[0].data(), command);
+
+  if (qh.qhullStatus() != qh_ERRnone)
+  {
+    if (qh.hasQhullMessage())
+      std::cerr << qh.qhullMessage() << std::endl;
+    throw std::logic_error ("Qhull failed");
+  }
+
+  typedef int size_type;
+  typedef int index_type;
+
+  // Map index in pts to index in vertices. -1 means not used
+  std::vector<int> pts_to_vertices (num_points, -1);
+
+  // Initialize the vertices
+  int nvertex = qh.vertexCount();
+  Vec3f* vertices = new Vec3f[nvertex];
+  QhullVertexList vertexList (qh.vertexList());
+  int i_vertex = 0;
+  for (QhullVertexList::const_iterator v = vertexList.begin();
+      v != vertexList.end(); ++v) {
+    QhullPoint pt ((*v).point());
+    pts_to_vertices[pt.id()] = i_vertex;
+    vertices[i_vertex] = Vec3f(pt[0], pt[1], pt[2]);
+    ++i_vertex;
+  }
+  assert(i_vertex == nvertex);
+
+  Convex<Triangle>* convex_tri (NULL);
+  ConvexBase* convex (NULL);
+  if (keepTriangles)
+    convex = convex_tri = new Convex<Triangle>();
+  else
+    convex = new ConvexBase;
+  convex->initialize(true, vertices, nvertex);
+
+  // Build the neighbors
+  convex->neighbors = new Neighbors[nvertex];
+  std::vector<std::set<index_type> > nneighbors (nvertex);
+  if (keepTriangles) {
+    convex_tri->num_polygons = qh.facetCount();
+    convex_tri->polygons = new Triangle[convex_tri->num_polygons];
+  }
+
+  unsigned int c_nneighbors = 0;
+  unsigned int i_polygon = 0;
+
+  for (QhullFacet facet = qh.beginFacet(); facet != qh.endFacet(); facet = facet.next()) {
+    if (facet.isSimplicial()) {
+      QhullVertexSet f_vertices (facet.vertices());
+      int n = f_vertices.count();
+      assert(n == 3);
+      Triangle tri (
+          pts_to_vertices[f_vertices[0].point().id()],
+          pts_to_vertices[f_vertices[1].point().id()],
+          pts_to_vertices[f_vertices[2].point().id()]);
+      if (keepTriangles) convex_tri->polygons[i_polygon++] = tri;
+      for(size_type j = 0; j < n; ++j)
+      {
+        size_type i = (j==0  ) ? n-1 : j-1;
+        size_type k = (j==n-1) ? 0   : j+1;
+        // Update neighbors of pj;
+        if (nneighbors[tri[j]].insert(tri[i]).second) c_nneighbors++;
+        if (nneighbors[tri[j]].insert(tri[k]).second) c_nneighbors++;
+      }
+    } else {
+      QhullRidgeSet f_ridges (facet.ridges());
+      for(size_type j = 0; j < f_ridges.count(); ++j)
+      {
+        assert(f_ridges[j].vertices().count() == 2);
+        index_type pi = pts_to_vertices[f_ridges[j].vertices()[0].point().id()],
+                   pj = pts_to_vertices[f_ridges[j].vertices()[1].point().id()];
+        // Update neighbors of pi and pj;
+        if (nneighbors[pj].insert(pi).second) c_nneighbors++;
+        if (nneighbors[pi].insert(pj).second) c_nneighbors++;
+      }
+    }
+  }
+  assert(!keepTriangles || i_polygon == qh.facetCount());
+
+  convex->nneighbors_ = new unsigned int[c_nneighbors];
+  unsigned int* p_nneighbors = convex->nneighbors_;
+  for (int i = 0; i < nvertex; ++i) {
+    Neighbors& n = convex->neighbors[i];
+    if (nneighbors[i].size() >= std::numeric_limits<unsigned char>::max())
+      throw std::logic_error ("Too many neighbors.");
+    n.count_ = (unsigned char)nneighbors[i].size();
+    n.n_     = p_nneighbors;
+    p_nneighbors = std::copy (nneighbors[i].begin(), nneighbors[i].end(), p_nneighbors);
+  }
+  assert (p_nneighbors == convex->nneighbors_ + c_nneighbors);
+  return convex;
+#else
+  throw std::logic_error("Library built without qhull. Cannot build object of this type.");
+#endif
+}
+} // namespace fcl
+} // namespace hpp

test/CMakeLists.txt

@@ -11,6 +11,9 @@ macro(add_fcl_test test_name source)
     )
   PKG_CONFIG_USE_DEPENDENCY(${test_name} assimp)
   target_compile_options(${test_name} PRIVATE "-Wno-c99-extensions")
+  if(HPP_FCL_HAS_QHULL)
+    target_compile_options(${test_name} PRIVATE -DHPP_FCL_HAS_QHULL)
+  endif()
 endmacro(add_fcl_test)
 
 include_directories(${CMAKE_CURRENT_BINARY_DIR})

test/convex.cpp

@@ -238,3 +238,83 @@ BOOST_AUTO_TEST_CASE(compare_convex_box)
     compareShapeDistance    (box, convex_box, tf1, tf2, eps);
   }
 }
+
+#ifdef HPP_FCL_HAS_QHULL
+BOOST_AUTO_TEST_CASE(convex_hull_throw)
+{
+  std::vector<Vec3f> points ({
+    Vec3f ( 1, 1, 1),
+    Vec3f ( 0, 0, 0),
+    Vec3f ( 1, 0, 0),
+    });
+
+  BOOST_CHECK_THROW(ConvexBase::convexHull(points.data(), 0, false, NULL),
+      std::invalid_argument);
+  BOOST_CHECK_THROW(ConvexBase::convexHull(points.data(), 1, false, NULL),
+      std::invalid_argument);
+  BOOST_CHECK_THROW(ConvexBase::convexHull(points.data(), 2, false, NULL),
+      std::invalid_argument);
+  BOOST_CHECK_THROW(ConvexBase::convexHull(points.data(), 3, false, NULL),
+      std::invalid_argument);
+}
+
+BOOST_AUTO_TEST_CASE(convex_hull_quad)
+{
+  std::vector<Vec3f> points ({
+    Vec3f ( 1, 1, 1),
+    Vec3f ( 0, 0, 0),
+    Vec3f ( 1, 0, 0),
+    Vec3f ( 0, 0, 1),
+    });
+
+  ConvexBase* convexHull = ConvexBase::convexHull(points.data(), (int)points.size(),
+      false, NULL);
+
+  BOOST_REQUIRE_EQUAL(convexHull->num_points, 4);
+  BOOST_CHECK_EQUAL(convexHull->neighbors[0].count(), 3);
+  BOOST_CHECK_EQUAL(convexHull->neighbors[1].count(), 3);
+  BOOST_CHECK_EQUAL(convexHull->neighbors[2].count(), 3);
+  delete convexHull;
+}
+
+BOOST_AUTO_TEST_CASE(convex_hull_box_like)
+{
+  std::vector<Vec3f> points ({
+    Vec3f ( 1, 1, 1),
+    Vec3f ( 1, 1,-1),
+    Vec3f ( 1,-1, 1),
+    Vec3f ( 1,-1,-1),
+    Vec3f (-1, 1, 1),
+    Vec3f (-1, 1,-1),
+    Vec3f (-1,-1, 1),
+    Vec3f (-1,-1,-1),
+    Vec3f ( 0, 0, 0   ),
+    Vec3f ( 0, 0, 0.99),
+    });
+
+  ConvexBase* convexHull = ConvexBase::convexHull(points.data(), (int)points.size(),
+      false, NULL);
+
+  BOOST_REQUIRE_EQUAL(8, convexHull->num_points);
+  for (int i = 0; i < 8; ++i)
+  {
+    BOOST_CHECK(convexHull->points[i].cwiseAbs() == Vec3f(1,1,1));
+    BOOST_CHECK_EQUAL(convexHull->neighbors[i].count(), 3);
+  }
+  delete convexHull;
+
+  convexHull = ConvexBase::convexHull(points.data(), (int)points.size(),
+      true, NULL);
+  Convex<Triangle>* convex_tri = dynamic_cast<Convex<Triangle>*> (convexHull);
+  BOOST_CHECK(convex_tri != NULL);
+
+  BOOST_REQUIRE_EQUAL(8, convexHull->num_points);
+  for (int i = 0; i < 8; ++i)
+  {
+    BOOST_CHECK(convexHull->points[i].cwiseAbs() == Vec3f(1,1,1));
+    BOOST_CHECK(convexHull->neighbors[i].count() >= 3);
+    BOOST_CHECK(convexHull->neighbors[i].count() <= 6);
+  }
+  delete convexHull;
+}
+#endif