Commit 6efdcedc authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

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

parent 53d3daba
......@@ -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
......
......@@ -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();
......
......@@ -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}
......
#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
......@@ -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})
......
......@@ -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
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment