Performance optimization for supportConvex

CHANGELOG.md

@@ -17,6 +17,8 @@
     [#467](https://github.com/flexible-collision-library/fcl/pull/467)
   * Bugs in RSS distance queries fixed:
     [#467](https://github.com/flexible-collision-library/fcl/pull/467)
+  * Convex gets *some* validation and improved support for the GJK `supportVertex()` API:
+    [#488](https://github.com/flexible-collision-library/fcl/pull/488)
 
 * Broadphase
 

include/fcl/geometry/shape/convex-inl.h

@@ -39,6 +39,10 @@
 #ifndef FCL_SHAPE_CONVEX_INL_H
 #define FCL_SHAPE_CONVEX_INL_H
 
+#include <map>
+#include <set>
+#include <utility>
+
 #include "fcl/geometry/shape/convex.h"
 
 namespace fcl
@@ -52,11 +56,14 @@ class FCL_EXPORT Convex<double>;
 template <typename S>
 Convex<S>::Convex(
     const std::shared_ptr<const std::vector<Vector3<S>>>& vertices,
-    int num_faces, const std::shared_ptr<const std::vector<int>>& faces)
-  : ShapeBase<S>(),
-    vertices_(vertices),
-    num_faces_(num_faces),
-    faces_(faces) {
+    int num_faces, const std::shared_ptr<const std::vector<int>>& faces,
+    bool throw_if_invalid)
+    : ShapeBase<S>(),
+      vertices_(vertices),
+      num_faces_(num_faces),
+      faces_(faces),
+      find_extreme_via_neighbors_{vertices->size() >
+                                  kMinVertCountForEdgeWalking} {
   assert(vertices != nullptr);
   assert(faces != nullptr);
   // Compute an interior point. We're computing the mean point and *not* some
@@ -66,6 +73,8 @@ Convex<S>::Convex(
     sum += vertex;
   }
   interior_point_ = sum * (S)(1.0 / vertices_->size());
+  FindVertexNeighbors();
+  ValidateMesh(throw_if_invalid);
 }
 
 //==============================================================================
@@ -212,7 +221,7 @@ template <typename S> S Convex<S>::computeVolume() const {
     face_center = face_center * (1.0 / vertex_count);
 
     // TODO(SeanCurtis-TRI): Because volume serves as the weights for
-    // center-of-mass an inertia computations, it should be refactored into its
+    // center-of-mass and inertia computations, it should be refactored into its
     // own function that can be invoked by providing three vertices (the fourth
     // being the origin).
 
@@ -248,6 +257,179 @@ std::vector<Vector3<S>> Convex<S>::getBoundVertices(
   return result;
 }
 
+//==============================================================================
+template <typename S>
+const Vector3<S>& Convex<S>::findExtremeVertex(const Vector3<S>& v_C) const {
+  // TODO(SeanCurtis-TRI): Create an override of this that seeds the search with
+  //  the last extremal vertex index (assuming some kind of coherency in the
+  //  evaluation sequence).
+  const std::vector<Vector3<S>>& vertices = *vertices_;
+  // Note: A small amount of empirical testing suggests that a vector of int8_t
+  // yields a slight performance improvement over int and bool. This is *not*
+  // definitive.
+  std::vector<int8_t> visited(vertices.size(), 0);
+  int extreme_index = 0;
+  S extreme_value = v_C.dot(vertices[extreme_index]);
+
+  if (find_extreme_via_neighbors_) {
+    bool keep_searching = true;
+    while (keep_searching) {
+      keep_searching = false;
+      const int neighbor_start = neighbors_[extreme_index];
+      const int neighbor_count = neighbors_[neighbor_start];
+      for (int n_index = neighbor_start + 1;
+           n_index <= neighbor_start + neighbor_count; ++n_index) {
+        const int neighbor_index = neighbors_[n_index];
+        if (visited[neighbor_index]) continue;
+        visited[neighbor_index] = 1;
+        const S neighbor_value = v_C.dot(vertices[neighbor_index]);
+        // N.B. Testing >= (instead of >) protects us from the (very rare) case
+        // where the *starting* vertex is co-planar with all of its neighbors
+        // *and* the query direction v_C is perpendicular to that plane. With >
+        // we wouldn't walk away from the start vertex. With >= we will walk
+        // away and continue around (although it won't necessarily be the
+        // fastest walk down).
+        if (neighbor_value >= extreme_value) {
+          keep_searching = true;
+          extreme_index = neighbor_index;
+          extreme_value = neighbor_value;
+        }
+      }
+    }
+  } else {
+    // Simple linear search.
+    for (int i = 1; i < static_cast<int>(vertices.size()); ++i) {
+      S value = v_C.dot(vertices[i]);
+      if (value > extreme_value) {
+        extreme_index = i;
+        extreme_value = value;
+      }
+    }
+  }
+  return vertices[extreme_index];
+}
+
+//==============================================================================
+template <typename S>
+void Convex<S>::ValidateMesh(bool throw_on_error) {
+  ValidateTopology(throw_on_error);
+  // TODO(SeanCurtis-TRI) Implement the missing "all-faces-are-planar" test.
+  // TODO(SeanCurtis-TRI) Implement the missing "really-is-convex" test.
+}
+
+//==============================================================================
+template <typename S>
+void Convex<S>::ValidateTopology(bool throw_on_error) {
+  // Computing the vertex neighbors is a pre-requisite to determining validity.
+  assert(neighbors_.size() > vertices_->size());
+
+  std::stringstream ss;
+  ss << "Found errors in the Convex mesh:";
+
+  // To simplify the code, we define an edge as a pair of ints (A, B) such that
+  // A < B must be true.
+  auto make_edge = [](int v0, int v1) {
+      if (v0 > v1) std::swap(v0, v1);
+      return std::make_pair(v0, v1);
+  };
+
+  bool all_connected = true;
+  // Build a map from each unique edge to the _number_ of adjacent faces (see
+  // the definition of make_edge for the encoding of an edge).
+  std::map<std::pair<int, int>, int> per_edge_face_count;
+  // First, pre-populate all the edges found in the vertex neighbor calculation.
+  for (int v = 0; v < static_cast<int>(vertices_->size()); ++v) {
+    const int neighbor_start = neighbors_[v];
+    const int neighbor_count = neighbors_[neighbor_start];
+    if (neighbor_count == 0) {
+      if (all_connected) {
+        ss << "\n Not all vertices are connected.";
+        all_connected = false;
+      }
+      ss << "\n  Vertex " << v << " is not included in any faces.";
+    }
+    for (int n_index = neighbor_start + 1;
+         n_index <= neighbor_start + neighbor_count; ++n_index) {
+      const int n = neighbors_[n_index];
+        per_edge_face_count[make_edge(v, n)] = 0;
+    }
+  }
+
+  // To count adjacent faces, we must iterate through the faces; we can't infer
+  // it from how many times an edge appears in the neighbor list. In the
+  // degenerate case where three faces share an edge, that edge would only
+  // appear twice. So, we must explicitly examine each face.
+  const std::vector<int>& faces = *faces_;
+  int face_index = 0;
+  for (int f = 0; f < num_faces_; ++f) {
+      const int vertex_count = faces[face_index];
+      int prev_v = faces[face_index + vertex_count];
+      for (int i = face_index + 1; i <= face_index + vertex_count; ++i) {
+          const int v = faces[i];
+          ++per_edge_face_count[make_edge(v, prev_v)];
+          prev_v = v;
+      }
+      face_index += vertex_count + 1;
+  }
+
+  // Now examine the results.
+  bool is_watertight = true;
+  for (const auto& key_value_pair : per_edge_face_count) {
+    const auto& edge = key_value_pair.first;
+    const int count = key_value_pair.second;
+    if (count != 2) {
+      if (is_watertight) {
+        is_watertight = false;
+        ss << "\n The mesh is not watertight.";
+      }
+      ss << "\n  Edge between vertices " << edge.first << " and " << edge.second
+         << " is shared by " << count << " faces (should be 2).";
+    }
+  }
+  // We can't trust walking the edges on a mesh that isn't watertight.
+  const bool has_error = !(is_watertight && all_connected);
+  // Note: find_extreme_via_neighbors_ may already be false because the mesh
+  // is too small. Don't indirectly re-enable it just because the mesh doesn't
+  // have any errors.
+  find_extreme_via_neighbors_ = find_extreme_via_neighbors_ && !has_error;
+  if (has_error && throw_on_error) {
+    throw std::runtime_error(ss.str());
+  }
+}
+
+//==============================================================================
+template <typename S>
+void Convex<S>::FindVertexNeighbors() {
+  // We initially build it using sets. Two faces with a common edge will
+  // independently want to report that the edge's vertices are neighbors. So,
+  // we rely on the set to eliminate the redundant declaration and then dump
+  // the results to a more compact representation: the vector.
+  const int v_count = static_cast<int>(vertices_->size());
+  std::vector<std::set<int>> neighbors(v_count);
+  const std::vector<int>& faces = *faces_;
+  int face_index = 0;
+  for (int f = 0; f < num_faces_; ++f) {
+    const int vertex_count = faces[face_index];
+    int prev_v = faces[face_index + vertex_count];
+    for (int i = face_index + 1; i <= face_index + vertex_count; ++i) {
+      const int v = faces[i];
+      neighbors[v].insert(prev_v);
+      neighbors[prev_v].insert(v);
+      prev_v = v;
+    }
+    face_index += vertex_count + 1;
+  }
+
+  // Now build the encoded adjacency graph as documented.
+  neighbors_.resize(v_count);
+  for (int v = 0; v < v_count; ++v) {
+    const std::set<int>& v_neighbors = neighbors[v];
+    neighbors_[v] = static_cast<int>(neighbors_.size());
+    neighbors_.push_back(static_cast<int>(v_neighbors.size()));
+    neighbors_.insert(neighbors_.end(), v_neighbors.begin(), v_neighbors.end());
+  }
+}
+
 } // namespace fcl
 
 #endif

include/fcl/geometry/shape/convex.h

@@ -40,6 +40,8 @@
 #define FCL_SHAPE_CONVEX_H
 
 #include <iostream>
+#include <memory>
+#include <vector>
 
 #include "fcl/geometry/shape/shape_base.h"
 
@@ -92,16 +94,21 @@ class FCL_EXPORT Convex : public ShapeBase<S_>
   /// @note: The %Convex geometry assumes that the input data %vertices and
   /// %faces do not change through the life of the object.
   ///
-  /// @warning: The %Convex class does *not* validate the input; it trusts that
-  /// the inputs truly represent a coherent convex polytope.
+  /// @warning: The %Convex class only partially validates the input; it
+  /// generally trusts that the inputs truly represent a coherent convex
+  /// polytope. For those aspects that *are* validated, the constructor will
+  /// throw for failed tests if `throw_if_invalid` is set to `true`.
   ///
-  /// @param vertices       The positions of the polytope vertices.
-  /// @param num_faces      The number of faces defined in `faces`.
-  /// @param faces          Encoding of the polytope faces. Must encode
-  ///                       `num_faces` number of faces. See member
-  ///                       documentation for details on encoding.
+  /// @param vertices           The positions of the polytope vertices.
+  /// @param num_faces          The number of faces defined in `faces`.
+  /// @param faces              Encoding of the polytope faces. Must encode
+  ///                           `num_faces` number of faces. See member
+  ///                           documentation for details on encoding.
+  /// @param throw_if_invalid   If `true`, failed attempts to validate the mesh
+  ///                           will throw an exception.
   Convex(const std::shared_ptr<const std::vector<Vector3<S>>>& vertices,
-         int num_faces, const std::shared_ptr<const std::vector<int>>& faces);
+         int num_faces, const std::shared_ptr<const std::vector<int>>& faces,
+         bool throw_if_invalid = false);
 
   /// @brief Copy constructor 
   Convex(const Convex& other);
@@ -160,18 +167,99 @@ class FCL_EXPORT Convex : public ShapeBase<S_>
   /// a specific configuration
   std::vector<Vector3<S>> getBoundVertices(const Transform3<S>& tf) const;
 
+  /// @brief Reports a vertex in this convex polytope that lies farthest in the
+  /// given direction v_C. The direction vector must be expressed in the same
+  /// frame C as the vertices of `this` %Convex polytope.
+  /// @retval p_CE the position vector from Frame C's origin to the extreme
+  ///         vertex E. It is guaranteed that v_C⋅p_CE ≥ v_C⋅p_CF for all F ≠ E
+  ///         in the  %Convex polytope's set of vertices.
+  const Vector3<S>& findExtremeVertex(const Vector3<S>& v_C) const;
+
   friend
   std::ostream& operator<<(std::ostream& out, const Convex& convex) {
     out << "Convex(v count: " << convex.vertices_->size() << ", f count: "
         << convex.getFaceCount() << ")";
     return out;
   }
 
-private:
+ private:
+  // Test utility to examine Convex internal state.
+  friend class ConvexTester;
+
+  // TODO(SeanCurtis-TRI): Complete the validation.
+  // *Partially* validate the mesh. The following properties are validated:
+  //   - Confirms mesh is water tight (see IsWaterTight).
+  //   - Confirms that all vertices are included in a face.
+  // The following properties still need to be validated:
+  //   - There are at least four vertices (the minimum to enclose a *volume*.)
+  //   - the vertices associated with each face are planar.
+  //   - For each face, all vertices *not* forming the face lie "on" or "behind"
+  //     the face.
+  //
+  // Invoking this *can* have side effects. Internal configuration can change
+  // based on failed validation tests. For example, the performance of
+  // findExtremeVertex() depends on the mesh being "valid" -- an invalid mesh
+  // can be used, but will use a slower algorithm as a result of being found
+  // invalid.
+  //
+  // @param throw_on_error  If `true` and the convex is shown to be invalid, an
+  //                        exception is thrown.
+  void ValidateMesh(bool throw_on_error);
+
+  // Reports if the mesh is watertight and that every vertex is part of a face.
+  // The mesh is watertight if every edge is shared by two different faces.
+  //
+  // As a side effect, if this fails, find_extreme_via_neighbors_ is set to
+  // false because a water tight mesh is a prerequisite to being able to find
+  // extremal points by following edges.
+  //
+  // @param throw_on_error  If `true` and the convex is shown to be invalid, an
+  //                        exception is thrown.
+  // @pre FindVertexNeighbors() must have been called already.
+  void ValidateTopology(bool throw_on_error);
+
+  // Analyzes the convex specification and builds the map of vertex ->
+  // neighboring vertices.
+  void FindVertexNeighbors();
+
   const std::shared_ptr<const std::vector<Vector3<S>>> vertices_;
   const int num_faces_;
   const std::shared_ptr<const std::vector<int>> faces_;
   Vector3<S> interior_point_;
+
+  /* The encoding of vertex adjacency in the mesh. The encoding is as follows:
+   Entries [0, V-1] encode the location in `neighbors_` where the adjacency
+   data for the ith vertex lies in the array.
+   Following those offsets is the compact representation of each vertex's
+   neighbors as: number of neighbors, n0, n1, ....
+
+   The figure below shows that for vertex j, entry j tells us to jump into
+   the vector at neighbors_[j]. The value mⱼ -- the number of vertices adjacent
+   to j -- is stored at that location. The next mⱼ entries starting at
+   neighbors_[j] + 1 are the *indices* of those vertices adjacent to vertex j.
+
+               Index where
+               vertex j's     Vertex j's
+               data lies      neighbor count
+                    ↓          ↓
+           |_|...|_|_|_|......┃mⱼ┃n₁┃n₂┃...┃nₘ┃mⱼ₊₁|...|
+            0 ...   j             ↑   ↑ ... ↑
+                                 Indices of vertex j's
+                                 neighboring vertices.
+
+   A modicum testing indicated that this compact representation led to
+   measurably improved performance for findExtremeVertex() -- initial
+   hypothesis attributes it to improved cache hits. */
+  std::vector<int> neighbors_;
+
+  // If true, findExtremeVertex() can reliably use neighbor_vertices_ to walk
+  // along the surface of the mesh. If false, it must linearly search.
+  bool find_extreme_via_neighbors_{false};
+
+  // Empirical evidence suggests that finding the extreme vertex by walking the
+  // edges of the mesh is only more efficient if there are more than 32
+  // vertices.
+  static constexpr int kMinVertCountForEdgeWalking = 32;
 };
 
 // Workaround for https://gcc.gnu.org/bugzilla/show_bug.cgi?id=57728 which