Unverified Commit 3bad0587 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by GitHub
Browse files

Merge pull request #167 from jmirabel/devel

Add function to generate the convex hull.
parents 9ac69c5b 3f12bc6e
[submodule "cmake"] [submodule "cmake"]
path = cmake path = cmake
url = git://github.com/jrl-umi3218/jrl-cmakemodules.git url = git://github.com/jrl-umi3218/jrl-cmakemodules.git
[submodule "third-parties/qhull"]
path = third-parties/qhull
url = git://github.com/qhull/qhull.git
...@@ -98,6 +98,24 @@ else() ...@@ -98,6 +98,24 @@ else()
message(STATUS "FCL does not use Octomap") message(STATUS "FCL does not use Octomap")
endif() 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") ADD_REQUIRED_DEPENDENCY("assimp >= 2.0")
SET(${PROJECT_NAME}_HEADERS SET(${PROJECT_NAME}_HEADERS
......
...@@ -88,10 +88,10 @@ class XmlDocString (object): ...@@ -88,10 +88,10 @@ class XmlDocString (object):
def otherTags (self, node): def otherTags (self, node):
if node.text: if node.text:
self._write (node.text.strip()) self._write (node.text.strip().replace('"', r'\"'))
for c in node.iterchildren(): for c in node.iterchildren():
self.visit (c) self.visit (c)
if c.tail: self._write (c.tail.strip()) if c.tail: self._write (c.tail.strip().replace('"', r'\"'))
def emphasis (self, node): def emphasis (self, node):
self._write ("*") self._write ("*")
...@@ -103,10 +103,10 @@ class XmlDocString (object): ...@@ -103,10 +103,10 @@ class XmlDocString (object):
self.otherTags (node) self.otherTags (node)
def para (self, node): def para (self, node):
if node.text: self._write (node.text) if node.text: self._write (node.text.replace('"', r'\"'))
for c in node.iterchildren(): for c in node.iterchildren():
self.visit (c) self.visit (c)
if c.tail: self._write (c.tail) if c.tail: self._write (c.tail.replace('"', r'\"'))
self._newline() self._newline()
def ref (self, node): def ref (self, node):
......
...@@ -82,7 +82,7 @@ public: ...@@ -82,7 +82,7 @@ public:
/// @brief Convex<Triangle> representation of this object /// @brief Convex<Triangle> representation of this object
boost::shared_ptr< ConvexBase > convex; boost::shared_ptr< ConvexBase > convex;
/// @brief Model type described by the instance /// @brief Model type described by the instance
BVHModelType getModelType() const BVHModelType getModelType() const
{ {
...@@ -173,11 +173,25 @@ public: ...@@ -173,11 +173,25 @@ public:
/// @brief End BVH model update, will also refit or rebuild the bounding volume hierarchy /// @brief End BVH model update, will also refit or rebuild the bounding volume hierarchy
int endUpdateModel(bool refit = true, bool bottomup = true); int endUpdateModel(bool refit = true, bool bottomup = true);
/// @brief Build this Convex<Triangle> representation of this model. /// @brief Build this \ref Convex "Convex<Triangle>" representation of this model.
/// The result is stored in attribute \ref convex.
/// \note this only takes the points of this model. It does not check that the /// \note this only takes the points of this model. It does not check that the
/// object is convex. It does not compute a convex hull. /// object is convex. It does not compute a convex hull.
void buildConvexRepresentation(bool share_memory); void buildConvexRepresentation(bool share_memory);
/// @brief Build a convex hull
/// and store it in attribute \ref convex.
/// \param keepTriangle whether the convex should be triangulated.
/// \param qhullCommand see \ref ConvexBase::convexHull.
/// \return \c true if this object is convex, hence the convex hull represents
/// the same object.
/// \sa ConvexBase::convexHull
/// \warning At the moment, the return value only checks whether there are as
/// many points in the convex hull as in the original object. This is
/// neither necessary (duplicated vertices get merged) nor sufficient
/// (think of a U with 4 vertices and 3 edges).
bool buildConvexHull(bool keepTriangle, const char* qhullCommand = NULL);
virtual int memUsage(int msg) const = 0; virtual int memUsage(int msg) const = 0;
/// @brief This is a special acceleration: BVH_model default stores the BV's transform in world coordinate. However, we can also store each BV's transform related to its parent /// @brief This is a special acceleration: BVH_model default stores the BV's transform in world coordinate. However, we can also store each BV's transform related to its parent
......
...@@ -53,6 +53,9 @@ template <typename PolygonT> ...@@ -53,6 +53,9 @@ template <typename PolygonT>
class Convex : public ConvexBase class Convex : public ConvexBase
{ {
public: public:
/// @brief Construct an uninitialized convex object
Convex () : ConvexBase(), polygons(NULL), num_polygons(0) {}
/// @brief Constructing a convex, providing normal and offset of each polytype surface, and the points and shape topology information /// @brief Constructing a convex, providing normal and offset of each polytype surface, and the points and shape topology information
/// \param ownStorage whether this class owns the pointers of points and /// \param ownStorage whether this class owns the pointers of points and
/// polygons. If owned, they are deleted upon destruction. /// polygons. If owned, they are deleted upon destruction.
......
...@@ -48,11 +48,11 @@ namespace fcl ...@@ -48,11 +48,11 @@ namespace fcl
template <typename PolygonT> template <typename PolygonT>
Convex<PolygonT>::Convex(bool own_storage, Vec3f* points_, int num_points_, Convex<PolygonT>::Convex(bool own_storage, Vec3f* points_, int num_points_,
PolygonT* polygons_, int num_polygons_) : PolygonT* polygons_, int num_polygons_) : ConvexBase(),
ConvexBase(own_storage, points_, num_points_),
polygons (polygons_), polygons (polygons_),
num_polygons (num_polygons_) num_polygons (num_polygons_)
{ {
initialize(own_storage, points_, num_points_);
fillNeighbors(); fillNeighbors();
} }
......
...@@ -281,6 +281,20 @@ public: ...@@ -281,6 +281,20 @@ public:
class ConvexBase : public ShapeBase class ConvexBase : public ShapeBase
{ {
public: 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(); virtual ~ConvexBase();
...@@ -312,10 +326,16 @@ public: ...@@ -312,10 +326,16 @@ public:
Vec3f center; Vec3f center;
protected: protected:
/// @brief Constructing a convex, providing normal and offset of each polytype surface, and the points and shape topology information /// @brief Construct an uninitialized convex object
/// Initialization is done with ConvexBase::initialize.
ConvexBase () : ShapeBase(), points(NULL), num_points(0),
neighbors(NULL), nneighbors_(NULL), own_storage_(false) {}
/// @brief Initialize the points of the convex shape
/// This also initializes the ConvexBase::center.
/// \param points_ list of 3D points /// \param points_ list of 3D points
/// \param num_points_ number of 3D points /// \param num_points_ number of 3D points
ConvexBase(bool ownStorage, Vec3f* points_, int num_points_); void initialize(bool ownStorage, Vec3f* points_, int num_points_);
/// @brief Copy constructor /// @brief Copy constructor
/// Only the list of neighbors is copied. /// Only the list of neighbors is copied.
......
...@@ -50,6 +50,7 @@ ...@@ -50,6 +50,7 @@
#include <hpp/fcl/internal/BV_splitter.h> #include <hpp/fcl/internal/BV_splitter.h>
#include "doxygen_autodoc/hpp/fcl/BVH/BVH_model.h" #include "doxygen_autodoc/hpp/fcl/BVH/BVH_model.h"
#include "doxygen_autodoc/hpp/fcl/shape/geometric_shapes.h" #include "doxygen_autodoc/hpp/fcl/shape/geometric_shapes.h"
#include "doxygen_autodoc/functions.h"
#endif #endif
#include "../doc/python/doxygen.hh" #include "../doc/python/doxygen.hh"
...@@ -120,6 +121,13 @@ struct ConvexBaseWrapper ...@@ -120,6 +121,13 @@ struct ConvexBaseWrapper
n.append (convex.neighbors[i][j]); n.append (convex.neighbors[i][j]);
return n; return n;
} }
static ConvexBase* convexHull (const Vec3fs& points, bool keepTri,
const char* qhullCommand)
{
return ConvexBase::convexHull (points.data(), (int)points.size(), keepTri,
qhullCommand);
}
}; };
template <typename PolygonT> template <typename PolygonT>
...@@ -179,6 +187,10 @@ void exposeShapes () ...@@ -179,6 +187,10 @@ void exposeShapes ()
.DEF_RO_CLASS_ATTRIB (ConvexBase, num_points) .DEF_RO_CLASS_ATTRIB (ConvexBase, num_points)
.def ("points", &ConvexBaseWrapper::points) .def ("points", &ConvexBaseWrapper::points)
.def ("neighbors", &ConvexBaseWrapper::neighbors) .def ("neighbors", &ConvexBaseWrapper::neighbors)
.def ("convexHull", &ConvexBaseWrapper::convexHull,
doxygen::member_func_doc(&ConvexBase::convexHull),
return_value_policy<manage_new_object>())
.staticmethod("convexHull")
; ;
class_ <Convex<Triangle>, bases<ConvexBase>, shared_ptr<Convex<Triangle> >, noncopyable> class_ <Convex<Triangle>, bases<ConvexBase>, shared_ptr<Convex<Triangle> >, noncopyable>
...@@ -385,7 +397,8 @@ void exposeCollisionGeometries () ...@@ -385,7 +397,8 @@ void exposeCollisionGeometries ()
.def_readonly ("convex", &BVHModelBase::convex) .def_readonly ("convex", &BVHModelBase::convex)
.def ("buildConvexRepresentation", &BVHModelBase::buildConvexRepresentation) .DEF_CLASS_FUNC(BVHModelBase, buildConvexRepresentation)
.DEF_CLASS_FUNC(BVHModelBase, buildConvexHull)
// Expose function to build a BVH // Expose function to build a BVH
.def(dv::member_func("beginModel", &BVHModelBase::beginModel)) .def(dv::member_func("beginModel", &BVHModelBase::beginModel))
......
...@@ -100,6 +100,14 @@ void BVHModelBase::buildConvexRepresentation(bool share_memory) ...@@ -100,6 +100,14 @@ void BVHModelBase::buildConvexRepresentation(bool share_memory)
} }
} }
bool BVHModelBase::buildConvexHull(bool keepTriangle, const char* qhullCommand)
{
convex.reset(
ConvexBase::convexHull(vertices, num_vertices, keepTriangle, qhullCommand)
);
return num_vertices == convex->num_points;
}
template<typename BV> template<typename BV>
BVHModel<BV>::BVHModel(const BVHModel<BV>& other) : BVHModelBase(other), BVHModel<BV>::BVHModel(const BVHModel<BV>& other) : BVHModelBase(other),
bv_splitter(other.bv_splitter), bv_splitter(other.bv_splitter),
......
...@@ -48,6 +48,7 @@ set(${LIBRARY_NAME}_SOURCES ...@@ -48,6 +48,7 @@ set(${LIBRARY_NAME}_SOURCES
narrowphase/narrowphase.cpp narrowphase/narrowphase.cpp
narrowphase/gjk.cpp narrowphase/gjk.cpp
narrowphase/details.h narrowphase/details.h
shape/convex.cpp
shape/geometric_shapes.cpp shape/geometric_shapes.cpp
shape/geometric_shapes_utility.cpp shape/geometric_shapes_utility.cpp
distance_box_halfspace.cpp distance_box_halfspace.cpp
...@@ -79,6 +80,67 @@ set(${LIBRARY_NAME}_SOURCES ...@@ -79,6 +80,67 @@ set(${LIBRARY_NAME}_SOURCES
mesh_loader/loader.cpp 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) SET(PROJECT_HEADERS_FULL_PATH)
FOREACH(header ${${PROJECT_NAME}_HEADERS}) FOREACH(header ${${PROJECT_NAME}_HEADERS})
LIST(APPEND PROJECT_HEADERS_FULL_PATH ${PROJECT_SOURCE_DIR}/${header}) LIST(APPEND PROJECT_HEADERS_FULL_PATH ${PROJECT_SOURCE_DIR}/${header})
...@@ -101,6 +163,13 @@ TARGET_LINK_LIBRARIES(${LIBRARY_NAME} ...@@ -101,6 +163,13 @@ TARGET_LINK_LIBRARIES(${LIBRARY_NAME}
Boost::system 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} target_include_directories(${LIBRARY_NAME}
SYSTEM PUBLIC SYSTEM PUBLIC
${EIGEN3_INCLUDE_DIR} ${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;
// Compute the neighbors from the edges of the faces.
for (QhullFacet facet = qh.beginFacet(); facet != qh.endFacet(); facet = facet.next()) {
if (facet.isSimplicial()) {
// In 3D, simplicial faces have 3 vertices. We mark them as neighbors.
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 {
if (keepTriangles) { // TODO I think there is a memory leak here.
throw std::invalid_argument("You requested to keep triangles so you "
"must pass option \"Qt\" to qhull via the qhull command argument.");
}
// Non-simplicial faces have more than 3 vertices and contains a list of
// rigdes. Ridges are (3-1)D simplex (i.e. one edge). We mark the two
// vertices of each ridge as neighbors.
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());
// Fill the neighbor attribute of the returned object.
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
...@@ -44,12 +44,11 @@ namespace hpp ...@@ -44,12 +44,11 @@ namespace hpp
namespace fcl namespace fcl
{ {
ConvexBase::ConvexBase(bool own_storage, Vec3f* points_, int num_points_) : void ConvexBase::initialize(bool own_storage, Vec3f* points_, int num_points_)
ShapeBase(),
points (points_),
num_points (num_points_),
own_storage_ (own_storage)
{ {
points = points_;
num_points = num_points_;
own_storage_ = own_storage;
computeCenter(); computeCenter();
} }
...@@ -60,7 +59,11 @@ ConvexBase::ConvexBase(const ConvexBase& other) : ...@@ -60,7 +59,11 @@ ConvexBase::ConvexBase(const ConvexBase& other) :
center (other.center), center (other.center),
own_storage_ (other.own_storage_) own_storage_ (other.own_storage_)
{ {
if (neighbors) delete [] neighbors;
if (nneighbors_) delete [] nneighbors_;
if (own_storage_) { if (own_storage_) {
if (own_storage_ && points) delete [] points;
points = new Vec3f[num_points]; points = new Vec3f[num_points];
memcpy(points, other.points, sizeof(Vec3f) * num_points); memcpy(points, other.points, sizeof(Vec3f) * num_points);
} }
...@@ -76,9 +79,9 @@ ConvexBase::ConvexBase(const ConvexBase& other) : ...@@ -76,9 +79,9 @@ ConvexBase::ConvexBase(const ConvexBase& other) :
ConvexBase::~ConvexBase () ConvexBase::~ConvexBase ()
{ {
delete [] neighbors; if (neighbors) delete [] neighbors;
delete [] nneighbors_; if (nneighbors_) delete [] nneighbors_;
if (own_storage_) delete [] points; if (own_storage_ && points) delete [] points;
} }
void ConvexBase::computeCenter() void ConvexBase::computeCenter()
......
...@@ -11,6 +11,9 @@ macro(add_fcl_test test_name source) ...@@ -11,6 +11,9 @@ macro(add_fcl_test test_name source)
) )
PKG_CONFIG_USE_DEPENDENCY(${test_name} assimp) PKG_CONFIG_USE_DEPENDENCY(${test_name} assimp)
target_compile_options(${test_name} PRIVATE "-Wno-c99-extensions") 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()