From e8a5f65d978999e279c04a3402a0e4459045f961 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Mon, 9 Sep 2019 11:18:54 +0200
Subject: [PATCH] Update class Convex to enable faster support retrieval.
---
include/hpp/fcl/shape/geometric_shapes.h | 84 +++++++-------
src/narrowphase/gjk.cpp | 27 +++--
src/shape/geometric_shapes.cpp | 80 +++++++------
test/CMakeLists.txt | 1 +
test/convex.cpp | 139 +++++++++++++++++++++++
5 files changed, 243 insertions(+), 88 deletions(-)
create mode 100644 test/convex.cpp
diff --git a/include/hpp/fcl/shape/geometric_shapes.h b/include/hpp/fcl/shape/geometric_shapes.h
index 7d00796f..1cb2bab4 100644
--- a/include/hpp/fcl/shape/geometric_shapes.h
+++ b/include/hpp/fcl/shape/geometric_shapes.h
@@ -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
diff --git a/src/narrowphase/gjk.cpp b/src/narrowphase/gjk.cpp
index 12fa4359..1b2892c1 100644
--- a/src/narrowphase/gjk.cpp
+++ b/src/narrowphase/gjk.cpp
@@ -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) \
diff --git a/src/shape/geometric_shapes.cpp b/src/shape/geometric_shapes.cpp
index 5f7e4f18..fa40e70e 100644
--- a/src/shape/geometric_shapes.cpp
+++ b/src/shape/geometric_shapes.cpp
@@ -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()
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 33b1a3df..80a17403 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -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)
diff --git a/test/convex.cpp b/test/convex.cpp
new file mode 100644
index 00000000..43372980
--- /dev/null
+++ b/test/convex.cpp
@@ -0,0 +1,139 @@
+/*
+ * 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);
+}
--
GitLab