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: ...@@ -275,40 +275,39 @@ public:
Convex(Vec3f* points_, Convex(Vec3f* points_,
int num_points_, int num_points_,
int* polygons_, 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_; initialize();
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();
} }
/// @brief Copy constructor /// @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; neighbors = new Neighbors[num_points];
points = other.points; memcpy(neighbors, other.neighbors, sizeof(Neighbors) * num_points);
num_points = other.num_points;
polygons = other.polygons; int c_nneighbors = 0;
num_edges = other.num_edges; for (int i = 0; i < num_points; ++i) c_nneighbors += neighbors[i].count();
edges = new Edge[num_edges]; nneighbors_ = new unsigned int[c_nneighbors];
memcpy(edges, other.edges, sizeof(Edge) * num_edges); memcpy(nneighbors_, other.nneighbors_, sizeof(unsigned int) * c_nneighbors);
} }
~Convex() ~Convex()
{ {
delete [] edges; delete [] neighbors;
delete [] nneighbors_;
} }
/// @brief Compute AABB /// @brief Compute AABB
...@@ -317,21 +316,31 @@ public: ...@@ -317,21 +316,31 @@ public:
/// @brief Get node type: a conex polytope /// @brief Get node type: a conex polytope
NODE_TYPE getNodeType() const { return GEOM_CONVEX; } 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 /// @brief An array of indices to the points of each polygon.
/// followed by that amount of indices to "points" in counter clockwise order /// It should be the number of vertices
/// followed by that amount of indices to "points" in counter clockwise order.
int* polygons; int* polygons;
int num_polygons;
/// @brief An array of the points of the polygon.
Vec3f* points; Vec3f* points;
int num_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]; }
}; };
/// Neighbors of each vertex.
Edge* edges; /// 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) /// @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; Vec3f center;
...@@ -445,13 +454,12 @@ public: ...@@ -445,13 +454,12 @@ public:
return vol / 6; return vol / 6;
} }
protected: protected:
/// @brief Get edge information void initialize();
void fillEdges();
};
private:
unsigned int* nneighbors_;
};
/// @brief Half Space: this is equivalent to the Plane in ODE. The separation plane is defined as n * x = d; /// @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 /// 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) ...@@ -169,17 +169,28 @@ void getShapeSupport(const Cylinder* cylinder, const Vec3f& dir, Vec3f& support)
void getShapeSupport(const Convex* convex, const Vec3f& dir, Vec3f& support) void getShapeSupport(const Convex* convex, const Vec3f& dir, Vec3f& support)
{ {
FCL_REAL maxdot = - std::numeric_limits<FCL_REAL>::max(); const Vec3f* pts = convex->points;
Vec3f* curp = convex->points; const Convex::Neighbors* nn = convex->neighbors;
for(int i = 0; i < convex->num_points; ++i, curp+=1)
int i = 0;
FCL_REAL maxdot = pts[i].dot(dir);
FCL_REAL dot;
bool found = true;
while (found)
{ {
FCL_REAL dot = dir.dot(*curp); const Convex::Neighbors& n = nn[i];
if(dot > maxdot) found = false;
{ for (int in = 0; in < n.count(); ++in) {
support = *curp; dot = pts[n[in]].dot(dir);
maxdot = dot; if (dot > maxdot) {
maxdot = dot;
i = n[in];
found = true;
}
} }
} }
support = pts[i];
} }
#define CALL_GET_SHAPE_SUPPORT(ShapeType) \ #define CALL_GET_SHAPE_SUPPORT(ShapeType) \
......
...@@ -39,65 +39,61 @@ ...@@ -39,65 +39,61 @@
#include <hpp/fcl/shape/geometric_shapes.h> #include <hpp/fcl/shape/geometric_shapes.h>
#include <hpp/fcl/shape/geometric_shapes_utility.h> #include <hpp/fcl/shape/geometric_shapes_utility.h>
#include <set>
namespace hpp namespace hpp
{ {
namespace fcl namespace fcl
{ {
void Convex::fillEdges() void Convex::initialize()
{ {
int* points_in_poly = polygons; center.setZero();
if(edges) delete [] edges; for(int i = 0; i < num_points; ++i)
center += points[i];
center /= num_points;
int num_edges_alloc = 0; int *points_in_poly = polygons;
for(int i = 0; i < num_polygons; ++i) neighbors = new Neighbors[num_points];
{
num_edges_alloc += *points_in_poly;
points_in_poly += (*points_in_poly + 1);
}
edges = new Edge[num_edges_alloc]; std::vector<std::set<unsigned int> > nneighbors (num_points);
unsigned int c_nneighbors = 0;
points_in_poly = polygons; for (int l = 0; l < num_polygons; ++l)
int* index = polygons + 1;
num_edges = 0;
Edge e;
bool isinset;
for(int i = 0; i < num_polygons; ++i)
{ {
for(int j = 0; j < *points_in_poly; ++j) int n = *points_in_poly;
{
e.first = std::min(index[j], index[(j+1)%*points_in_poly]); for (int j = 0; j < n; ++j) {
e.second = std::max(index[j], index[(j+1)%*points_in_poly]); int i = (j==0 ) ? n-1 : j-1;
isinset = false; int k = (j==n-1) ? 0 : j+1;
for(int k = 0; k < num_edges; ++k) int pi = points_in_poly[i+1];
{ int pj = points_in_poly[j+1];
if((edges[k].first == e.first) && (edges[k].second == e.second)) int pk = points_in_poly[k+1];
{ // Update neighbors of pj;
isinset = true; if (nneighbors[pj].count(pi) == 0) {
break; c_nneighbors++;
} nneighbors[pj].insert(pi);
} }
if (nneighbors[pj].count(pk) == 0) {
if(!isinset) c_nneighbors++;
{ nneighbors[pj].insert(pk);
edges[num_edges].first = e.first;
edges[num_edges].second = e.second;
++num_edges;
} }
} }
points_in_poly += (*points_in_poly + 1); points_in_poly += n+1;
index = points_in_poly + 1;
} }
if(num_edges < num_edges_alloc) nneighbors_ = new unsigned int[c_nneighbors];
{ unsigned int* p_nneighbors = nneighbors_;
Edge* tmp = new Edge[num_edges]; for (int i = 0; i < num_points; ++i) {
memcpy(tmp, edges, num_edges * sizeof(Edge)); Neighbors& n = neighbors[i];
delete [] edges; if (nneighbors[i].size() >= std::numeric_limits<unsigned char>::max())
edges = tmp; 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() void Halfspace::unitNormalTest()
......
...@@ -42,6 +42,7 @@ add_fcl_test(simple simple.cpp) ...@@ -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_1 capsule_box_1.cpp)
add_fcl_test(capsule_box_2 capsule_box_2.cpp) add_fcl_test(capsule_box_2 capsule_box_2.cpp)
add_fcl_test(obb obb.cpp) add_fcl_test(obb obb.cpp)
add_fcl_test(convex convex.cpp)
add_fcl_test(bvh_models bvh_models.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