Skip to content

Commit

Permalink
Performance optimization for supportConvex (flexible-collision-librar…
Browse files Browse the repository at this point in the history 
…y#488)

* Performance optimization for supportConvex

The original implementation of supportConvex did a linear search across
all vertices. This makes the Convex mesh smarter to make the search for
the support vertex cheaper.

 - It builds an adjacency graph on all vertices upon construction.
 - It introduces the method findExtremeVertex Method which does a walk
   along the edges of the mesh to find the extreme vertex.
   - This is faster for large meshes but slower for small meshes.
   - Empirical data suggests around 32 vertices to be a good cut off.
   - This uses vertex count to determine which algorithm to use.
 - It also introduces initial validation of the mesh -- that validation
   absolutely necessary for the edge walking to provide a correct answer.
 - Several aspects of the implementation have been tested and tweaked
   for performance. See:
   - use of vector<int8_t> in Convex::findExtremeVertex
   - cache-friendly implementation of adjacency graph Convex::neighbors_.
  • Loading branch information
@SeanCurtis-TRI
SeanCurtis-TRI authored Aug 28, 2020
1 parent 33cc3cf commit 0d98b83
Show file tree
Hide file tree
Showing 7 changed files with 835 additions and 54 deletions.
2 changes: 2 additions & 0 deletions CHANGELOG.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down
194 changes: 188 additions & 6 deletions include/fcl/geometry/shape/convex-inl.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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
Expand All @@ -66,6 +73,8 @@ Convex<S>::Convex(
sum += vertex;
}
interior_point_ = sum * (S)(1.0 / vertices_->size());
FindVertexNeighbors();
ValidateMesh(throw_if_invalid);
}

//==============================================================================
Expand Down Expand Up @@ -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).

Expand Down Expand Up @@ -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
106 changes: 97 additions & 9 deletions include/fcl/geometry/shape/convex.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,6 +40,8 @@
#define FCL_SHAPE_CONVEX_H

#include <iostream>
#include <memory>
#include <vector>

#include "fcl/geometry/shape/shape_base.h"

Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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
Expand Down
Loading

0 comments on commit 0d98b83

Please sign in to comment.