convex.h

Go to the documentation of this file.

/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2011-2014, Willow Garage, Inc.
 *  Copyright (c) 2014-2016, Open Source Robotics Foundation
 *  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.
 */

#ifndef FCL_SHAPE_CONVEX_H
#define FCL_SHAPE_CONVEX_H

#include <ostream>
#include <memory>
#include <string>
#include <vector>

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

namespace fcl
{

template <typename S_>
class FCL_EXPORT Convex : public ShapeBase<S_>
{
public:

  using S = S_;

  // TODO(SeanCurtis-TRI): A huge shopping list of issues with this class are
  // enumerated in https://github.com/flexible-collision-library/fcl/issues/326.

  Convex(const std::shared_ptr<const std::vector<Vector3<S>>>& vertices,
         int num_faces, const std::shared_ptr<const std::vector<int>>& faces,
         bool throw_if_invalid = false);

  Convex(const Convex& other);

  ~Convex() = default;

  void computeLocalAABB() override;

  NODE_TYPE getNodeType() const override;

  const std::vector<Vector3<S>>& getVertices() const { return *vertices_; }

  int getFaceCount() const { return num_faces_; }

  const std::vector<int>& getFaces() const { return *faces_; }

  const Vector3<S>& getInteriorPoint() const { return interior_point_; }

  // Documentation inherited.
  Matrix3<S> computeMomentofInertia() const override;

  // Documentation inherited.
  Vector3<S> computeCOM() const override;

  // Documentation inherited.
  S computeVolume() const override;

  std::vector<Vector3<S>> getBoundVertices(const Transform3<S>& tf) const;

  const Vector3<S>& findExtremeVertex(const Vector3<S>& v_C) const;

  std::string representation(int precision = 20) 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:
  // 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_;
  // This is stored to support representation().
  const bool throw_if_invalid_{};
  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
// should be moved back into the class definition once we no longer need to
// support GCC versions prior to 6.3.
template <typename S>
Convex<S>::Convex(const Convex<S>& other) = default;

using Convexf = Convex<float>;
using Convexd = Convex<double>;

} // namespace fcl

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

#endif