Commit 46b8f40e authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

[Convex] Add BVHModel::buildConvexRepresentation

parent 7823f4b9
......@@ -52,6 +52,8 @@ namespace hpp
namespace fcl
{
class ConvexBase;
/// @brief A class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewed as a degraded version of mesh)
template<typename BV>
class BVHModel : public CollisionGeometry
......@@ -81,6 +83,9 @@ public:
/// @brief Fitting rule to fit a BV node to a set of geometry primitives
boost::shared_ptr<BVFitterBase<BV> > bv_fitter;
/// @brief Convex<Triangle> representation of this object
boost::shared_ptr< ConvexBase > convex;
/// @brief Model type described by the instance
BVHModelType getModelType() const
......@@ -209,7 +214,12 @@ public:
/// @brief Check the number of memory used
int memUsage(int msg) const;
/// @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 Build this Convex<Triangle> representation of this model.
/// \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.
void buildConvexRepresentation();
/// @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
/// BV node. When traversing the BVH, this can save one matrix transformation.
void makeParentRelative()
{
......@@ -275,7 +285,6 @@ private:
int num_vertex_updated; /// for ccd vertex update
unsigned int* primitive_indices;
/// @brief Bounding volume hierarchy
BVNode<BV>* bvs;
......
......@@ -54,12 +54,15 @@ class Convex : public ConvexBase
{
public:
/// @brief Constructing a convex, providing normal and offset of each polytype surface, and the points and shape topology information
/// \param own_storage whether this class owns the pointers of points and
/// polygons. If owned, they are deleted upon destruction.
/// \param points_ list of 3D points
/// \param num_points_ number of 3D points
/// \param polygons_ \copydoc Convex::polygons
/// \param num_polygons_ the number of polygons.
/// \note num_polygons_ is not the allocated size of polygons_.
Convex(Vec3f* points_, int num_points_,
Convex(bool ownStorage,
Vec3f* points_, int num_points_,
PolygonT* polygons_, int num_polygons_);
/// @brief Copy constructor
......
......@@ -46,11 +46,9 @@ namespace fcl
{
template <typename PolygonT>
Convex<PolygonT>::Convex(Vec3f* points_,
int num_points_,
PolygonT* polygons_,
int num_polygons_) :
ConvexBase(points_, num_points_),
Convex<PolygonT>::Convex(bool own_storage, Vec3f* points_, int num_points_,
PolygonT* polygons_, int num_polygons_) :
ConvexBase(own_storage, points_, num_points_),
polygons (polygons_),
num_polygons (num_polygons_)
{
......@@ -62,11 +60,18 @@ Convex<PolygonT>::Convex(const Convex<PolygonT>& other) :
ConvexBase (other),
polygons (other.polygons),
num_polygons (other.num_polygons)
{}
{
if (own_storage_) {
polygons = new PolygonT[num_polygons];
memcpy(polygons, other.polygons, sizeof(PolygonT) * num_polygons);
}
}
template <typename PolygonT>
Convex<PolygonT>::~Convex()
{}
{
if (own_storage_) delete [] polygons;
}
template <typename PolygonT>
Matrix3f Convex<PolygonT>::computeMomentofInertia() const
......@@ -193,15 +198,15 @@ void Convex<PolygonT>::fillNeighbors()
{
size_type i = (j==0 ) ? n-1 : j-1;
size_type k = (j==n-1) ? 0 : j+1;
index_type pi = polygon[i+1],
pj = polygon[j+1],
pk = polygon[k+1];
index_type pi = polygon[i],
pj = polygon[j],
pk = polygon[k];
// Update neighbors of pj;
if (nneighbors[pj].count(pi) == (unsigned int)0) {
if (nneighbors[pj].count(pi) == 0) {
c_nneighbors++;
nneighbors[pj].insert(pi);
}
if (nneighbors[pj].count(pk) == (unsigned int)0) {
if (nneighbors[pj].count(pk) == 0) {
c_nneighbors++;
nneighbors[pj].insert(pk);
}
......
......@@ -300,7 +300,7 @@ protected:
/// @brief Constructing a convex, providing normal and offset of each polytype surface, and the points and shape topology information
/// \param points_ list of 3D points
/// \param num_points_ number of 3D points
ConvexBase(Vec3f* points_, int num_points_);
ConvexBase(bool ownStorage, Vec3f* points_, int num_points_);
/// @brief Copy constructor
/// Only the list of neighbors is copied.
......@@ -308,10 +308,14 @@ protected:
unsigned int* nneighbors_;
bool own_storage_;
private:
void computeCenter();
};
template <typename PolygonT> class Convex;
/// @brief Half Space: this is equivalent to the Plane in ODE. The separation plane is defined as n * x = d;
/// Points in the negative side of the separation plane (i.e. {x | n * x < d}) are inside the half space and points
/// in the positive side of the separation plane (i.e. {x | n * x > d}) are outside the half space
......
......@@ -37,6 +37,7 @@
#include <hpp/fcl/BVH/BVH_model.h>
#include <hpp/fcl/BV/BV.h>
#include <hpp/fcl/shape/convex.h>
#include <iostream>
#include <string.h>
......@@ -641,6 +642,13 @@ int BVHModel<BV>::memUsage(int msg) const
return BVH_OK;
}
template<typename BV>
void BVHModel<BV>::buildConvexRepresentation()
{
if (!convex) {
convex.reset(new Convex<Triangle>(false, vertices, num_vertices, tri_indices, num_vertices));
}
}
template<typename BV>
int BVHModel<BV>::buildTree()
......
......@@ -44,11 +44,11 @@ namespace hpp
namespace fcl
{
ConvexBase::ConvexBase(Vec3f* points_,
int num_points_) :
ConvexBase::ConvexBase(bool own_storage, Vec3f* points_, int num_points_) :
ShapeBase(),
points (points_),
num_points (num_points_)
num_points (num_points_),
own_storage_ (own_storage)
{
computeCenter();
}
......@@ -57,8 +57,14 @@ ConvexBase::ConvexBase(const ConvexBase& other) :
ShapeBase (other),
points (other.points),
num_points (other.num_points),
center (other.center)
center (other.center),
own_storage_ (other.own_storage_)
{
if (own_storage_) {
points = new Vec3f[num_points];
memcpy(points, other.points, sizeof(Vec3f) * num_points);
}
neighbors = new Neighbors[num_points];
memcpy(neighbors, other.neighbors, sizeof(Neighbors) * num_points);
......@@ -72,6 +78,7 @@ ConvexBase::~ConvexBase ()
{
delete [] neighbors;
delete [] nneighbors_;
if (own_storage_) delete [] points;
}
void ConvexBase::computeCenter()
......
......@@ -80,7 +80,7 @@ private:
Convex<Quadrilateral> buildBox (FCL_REAL l, FCL_REAL w, FCL_REAL d)
{
Vec3f pts[8];
Vec3f* pts = new Vec3f[8];
pts[0] = Vec3f( l, w, d);
pts[1] = Vec3f( l, w,-d);
pts[2] = Vec3f( l,-w, d);
......@@ -89,7 +89,8 @@ Convex<Quadrilateral> buildBox (FCL_REAL l, FCL_REAL w, FCL_REAL d)
pts[5] = Vec3f(-l, w,-d);
pts[6] = Vec3f(-l,-w, d);
pts[7] = Vec3f(-l,-w,-d);
std::vector<Quadrilateral> polygons (6);
Quadrilateral* polygons = new Quadrilateral[6];
polygons[0].set(0, 2, 3, 1); // x+ side
polygons[1].set(2, 6, 7, 3); // y- side
polygons[2].set(4, 5, 7, 6); // x- side
......@@ -97,10 +98,10 @@ Convex<Quadrilateral> buildBox (FCL_REAL l, FCL_REAL w, FCL_REAL d)
polygons[4].set(1, 3, 7, 5); // z- side
polygons[5].set(0, 2, 6, 4); // z+ side
return Convex<Quadrilateral> (
return Convex<Quadrilateral> (true,
pts, // points
8, // num points
polygons.data(),
polygons,
6 // number of polygons
);
}
......
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