Commit e8a5f65d authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

Update class Convex to enable faster support retrieval.

parent dbf93557
......@@ -275,40 +275,39 @@ public:
Convex(Vec3f* points_,
int num_points_,
int* polygons_,
int num_polygons_) : ShapeBase()
int num_polygons_) :
ShapeBase(),
points (points_),
num_points (num_points_),
polygons (polygons_),
num_polygons (num_polygons_)
{
num_polygons = num_polygons_;
points = points_;
num_points = num_points_;
polygons = polygons_;
edges = NULL;
Vec3f sum (0,0,0);
for(int i = 0; i < num_points; ++i)
{
sum += points[i];
}
center = sum * (FCL_REAL)(1.0 / num_points);
fillEdges();
initialize();
}
/// @brief Copy constructor
Convex(const Convex& other) : ShapeBase(other)
/// Only the list of neighbors is copied.
Convex(const Convex& other) :
ShapeBase (other),
points (other.points),
num_points (other.num_points),
polygons (other.polygons),
num_polygons (other.num_polygons),
center (other.center)
{
num_polygons = other.num_polygons;
points = other.points;
num_points = other.num_points;
polygons = other.polygons;
num_edges = other.num_edges;
edges = new Edge[num_edges];
memcpy(edges, other.edges, sizeof(Edge) * num_edges);
neighbors = new Neighbors[num_points];
memcpy(neighbors, other.neighbors, sizeof(Neighbors) * num_points);
int c_nneighbors = 0;
for (int i = 0; i < num_points; ++i) c_nneighbors += neighbors[i].count();
nneighbors_ = new unsigned int[c_nneighbors];
memcpy(nneighbors_, other.nneighbors_, sizeof(unsigned int) * c_nneighbors);
}
~Convex()
{
delete [] edges;
delete [] neighbors;
delete [] nneighbors_;
}
/// @brief Compute AABB
......@@ -317,21 +316,31 @@ public:
/// @brief Get node type: a conex polytope
NODE_TYPE getNodeType() const { return GEOM_CONVEX; }
/// @brief An array of indices to the points of each polygon, it should be the number of vertices
/// followed by that amount of indices to "points" in counter clockwise order
/// @brief An array of indices to the points of each polygon.
/// It should be the number of vertices
/// followed by that amount of indices to "points" in counter clockwise order.
int* polygons;
int num_polygons;
/// @brief An array of the points of the polygon.
Vec3f* points;
int num_points;
int num_edges;
int num_polygons;
struct Edge
class Neighbors
{
int first, second;
private:
unsigned char count_;
unsigned int* n_;
friend class Convex;
public:
unsigned char const& count () const { return count_; }
unsigned int & operator[] (int i) { assert(i<count_); return n_[i]; }
unsigned int const& operator[] (int i) const { assert(i<count_); return n_[i]; }
};
Edge* edges;
/// Neighbors of each vertex.
/// It is an array of size num_points. For each vertex, it contains the number
/// of neighbors and a list of indices to them.
Neighbors* neighbors;
/// @brief center of the convex polytope, this is used for collision: center is guaranteed in the internal of the polytope (as it is convex)
Vec3f center;
......@@ -445,13 +454,12 @@ public:
return vol / 6;
}
protected:
/// @brief Get edge information
void fillEdges();
};
void initialize();
private:
unsigned int* nneighbors_;
};
/// @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
......
......@@ -169,17 +169,28 @@ void getShapeSupport(const Cylinder* cylinder, const Vec3f& dir, Vec3f& support)
void getShapeSupport(const Convex* convex, const Vec3f& dir, Vec3f& support)
{
FCL_REAL maxdot = - std::numeric_limits<FCL_REAL>::max();
Vec3f* curp = convex->points;
for(int i = 0; i < convex->num_points; ++i, curp+=1)
const Vec3f* pts = convex->points;
const Convex::Neighbors* nn = convex->neighbors;
int i = 0;
FCL_REAL maxdot = pts[i].dot(dir);
FCL_REAL dot;
bool found = true;
while (found)
{
FCL_REAL dot = dir.dot(*curp);
if(dot > maxdot)
{
support = *curp;
maxdot = dot;
const Convex::Neighbors& n = nn[i];
found = false;
for (int in = 0; in < n.count(); ++in) {
dot = pts[n[in]].dot(dir);
if (dot > maxdot) {
maxdot = dot;
i = n[in];
found = true;
}
}
}
support = pts[i];
}
#define CALL_GET_SHAPE_SUPPORT(ShapeType) \
......
......@@ -39,65 +39,61 @@
#include <hpp/fcl/shape/geometric_shapes.h>
#include <hpp/fcl/shape/geometric_shapes_utility.h>
#include <set>
namespace hpp
{
namespace fcl
{
void Convex::fillEdges()
void Convex::initialize()
{
int* points_in_poly = polygons;
if(edges) delete [] edges;
center.setZero();
for(int i = 0; i < num_points; ++i)
center += points[i];
center /= num_points;
int num_edges_alloc = 0;
for(int i = 0; i < num_polygons; ++i)
{
num_edges_alloc += *points_in_poly;
points_in_poly += (*points_in_poly + 1);
}
int *points_in_poly = polygons;
neighbors = new Neighbors[num_points];
edges = new Edge[num_edges_alloc];
std::vector<std::set<unsigned int> > nneighbors (num_points);
unsigned int c_nneighbors = 0;
points_in_poly = polygons;
int* index = polygons + 1;
num_edges = 0;
Edge e;
bool isinset;
for(int i = 0; i < num_polygons; ++i)
for (int l = 0; l < num_polygons; ++l)
{
for(int j = 0; j < *points_in_poly; ++j)
{
e.first = std::min(index[j], index[(j+1)%*points_in_poly]);
e.second = std::max(index[j], index[(j+1)%*points_in_poly]);
isinset = false;
for(int k = 0; k < num_edges; ++k)
{
if((edges[k].first == e.first) && (edges[k].second == e.second))
{
isinset = true;
break;
}
int n = *points_in_poly;
for (int j = 0; j < n; ++j) {
int i = (j==0 ) ? n-1 : j-1;
int k = (j==n-1) ? 0 : j+1;
int pi = points_in_poly[i+1];
int pj = points_in_poly[j+1];
int pk = points_in_poly[k+1];
// Update neighbors of pj;
if (nneighbors[pj].count(pi) == 0) {
c_nneighbors++;
nneighbors[pj].insert(pi);
}
if(!isinset)
{
edges[num_edges].first = e.first;
edges[num_edges].second = e.second;
++num_edges;
if (nneighbors[pj].count(pk) == 0) {
c_nneighbors++;
nneighbors[pj].insert(pk);
}
}
points_in_poly += (*points_in_poly + 1);
index = points_in_poly + 1;
points_in_poly += n+1;
}
if(num_edges < num_edges_alloc)
{
Edge* tmp = new Edge[num_edges];
memcpy(tmp, edges, num_edges * sizeof(Edge));
delete [] edges;
edges = tmp;
nneighbors_ = new unsigned int[c_nneighbors];
unsigned int* p_nneighbors = nneighbors_;
for (int i = 0; i < num_points; ++i) {
Neighbors& n = 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 == nneighbors_ + c_nneighbors);
}
void Halfspace::unitNormalTest()
......
......@@ -42,6 +42,7 @@ add_fcl_test(simple simple.cpp)
add_fcl_test(capsule_box_1 capsule_box_1.cpp)
add_fcl_test(capsule_box_2 capsule_box_2.cpp)
add_fcl_test(obb obb.cpp)
add_fcl_test(convex convex.cpp)
add_fcl_test(bvh_models bvh_models.cpp)
......
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2019, LAAS-CNRS
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
/** \author Joseph Mirabel */
#define BOOST_TEST_MODULE FCL_GEOMETRIC_SHAPES
#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>
#include <hpp/fcl/shape/geometric_shapes.h>
using namespace hpp::fcl;
BOOST_AUTO_TEST_CASE(convex)
{
FCL_REAL l = 1, w = 1, d = 1;
Vec3f pts[8];
pts[0] = Vec3f( l, w, d);
pts[1] = Vec3f( l, w,-d);
pts[2] = Vec3f( l,-w, d);
pts[3] = Vec3f( l,-w,-d);
pts[4] = Vec3f(-l, w, d);
pts[5] = Vec3f(-l, w,-d);
pts[6] = Vec3f(-l,-w, d);
pts[7] = Vec3f(-l,-w,-d);
std::vector<int> polygons;
polygons.push_back(4);
polygons.push_back(0);
polygons.push_back(2);
polygons.push_back(3);
polygons.push_back(1);
polygons.push_back(4);
polygons.push_back(2);
polygons.push_back(6);
polygons.push_back(7);
polygons.push_back(3);
polygons.push_back(4);
polygons.push_back(4);
polygons.push_back(5);
polygons.push_back(7);
polygons.push_back(6);
polygons.push_back(4);
polygons.push_back(0);
polygons.push_back(1);
polygons.push_back(5);
polygons.push_back(4);
polygons.push_back(4);
polygons.push_back(1);
polygons.push_back(3);
polygons.push_back(7);
polygons.push_back(5);
polygons.push_back(4);
polygons.push_back(0);
polygons.push_back(2);
polygons.push_back(6);
polygons.push_back(4);
Convex box (
pts, // points
8, // num points
polygons.data(),
6 // number of polygons
);
// Check neighbors
for (int i = 0; i < 8; ++i) {
BOOST_CHECK_EQUAL(box.neighbors[i].count(), 3);
}
BOOST_CHECK_EQUAL(box.neighbors[0][0], 1);
BOOST_CHECK_EQUAL(box.neighbors[0][1], 2);
BOOST_CHECK_EQUAL(box.neighbors[0][2], 4);
BOOST_CHECK_EQUAL(box.neighbors[1][0], 0);
BOOST_CHECK_EQUAL(box.neighbors[1][1], 3);
BOOST_CHECK_EQUAL(box.neighbors[1][2], 5);
BOOST_CHECK_EQUAL(box.neighbors[2][0], 0);
BOOST_CHECK_EQUAL(box.neighbors[2][1], 3);
BOOST_CHECK_EQUAL(box.neighbors[2][2], 6);
BOOST_CHECK_EQUAL(box.neighbors[3][0], 1);
BOOST_CHECK_EQUAL(box.neighbors[3][1], 2);
BOOST_CHECK_EQUAL(box.neighbors[3][2], 7);
BOOST_CHECK_EQUAL(box.neighbors[4][0], 0);
BOOST_CHECK_EQUAL(box.neighbors[4][1], 5);
BOOST_CHECK_EQUAL(box.neighbors[4][2], 6);
BOOST_CHECK_EQUAL(box.neighbors[5][0], 1);
BOOST_CHECK_EQUAL(box.neighbors[5][1], 4);
BOOST_CHECK_EQUAL(box.neighbors[5][2], 7);
BOOST_CHECK_EQUAL(box.neighbors[6][0], 2);
BOOST_CHECK_EQUAL(box.neighbors[6][1], 4);
BOOST_CHECK_EQUAL(box.neighbors[6][2], 7);
BOOST_CHECK_EQUAL(box.neighbors[7][0], 3);
BOOST_CHECK_EQUAL(box.neighbors[7][1], 5);
BOOST_CHECK_EQUAL(box.neighbors[7][2], 6);
}
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