Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Humanoid Path Planner
hpp-fcl
Commits
6efdcedc
Commit
6efdcedc
authored
May 04, 2020
by
Joseph Mirabel
Browse files
Add function to build convex hull of set of points (using qhull)
parent
53d3daba
Changes
6
Hide whitespace changes
Inline
Side-by-side
CMakeLists.txt
View file @
6efdcedc
...
...
@@ -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
View file @
6efdcedc
...
...
@@ -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
View file @
6efdcedc
...
...
@@ -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
0 → 100644
View file @
6efdcedc
#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
View file @
6efdcedc
...
...
@@ -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
View file @
6efdcedc
...
...
@@ -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
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment