Update class Convex to enable faster support retrieval.

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

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)                                      \

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()

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)
 

test/convex.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);
+}