Clean up Convex geometry (including fixing AABB computation error)

The Convex geometry had an error in computing its local AABB. This fixes
that error.

It includes a unit test that exposes the error without the fix and
validates the fix.

This also, incidentally, cleans up some of the documentation.

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

@@ -50,24 +50,24 @@ class FCL_EXPORT Convex<double>;
 //==============================================================================
 template <typename S>
 Convex<S>::Convex(
-    Vector3<S>* plane_normals, S* plane_dis, int num_planes_,
-    Vector3<S>* points, int num_points_, int* polygons_)
+    Vector3<S>* plane_normals_in, S* plane_dis_in, int num_planes_in,
+    Vector3<S>* points_in, int num_points_in, int* polygons_in)
   : ShapeBase<S>()
 {
-  plane_normals = plane_normals;
-  plane_dis = plane_dis;
-  num_planes = num_planes_;
-  points = points;
-  num_points = num_points_;
-  polygons = polygons_;
+  plane_normals = plane_normals_in;
+  plane_dis = plane_dis_in;
+  num_planes = num_planes_in;
+  points = points_in;
+  num_points = num_points_in;
+  polygons = polygons_in;
   edges = nullptr;
 
+  // Compute an interior point: the mean vertex (we're calling it "center").
   Vector3<S> sum = Vector3<S>::Zero();
   for(int i = 0; i < num_points; ++i)
   {
     sum += points[i];
   }
-
   center = sum * (S)(1.0 / num_points);
 
   fillEdges();
@@ -98,8 +98,8 @@ Convex<S>::~Convex()
 template <typename S>
 void Convex<S>::computeLocalAABB()
 {
-  this->aabb_local.min_.setConstant(-std::numeric_limits<S>::max());
-  this->aabb_local.max_.setConstant(std::numeric_limits<S>::max());
+  this->aabb_local.min_.setConstant(std::numeric_limits<S>::max());
+  this->aabb_local.max_.setConstant(-std::numeric_limits<S>::max());
   for(int i = 0; i < num_points; ++i)
     this->aabb_local += points[i];
 

include/fcl/geometry/shape/convex.h

@@ -51,7 +51,37 @@ class FCL_EXPORT Convex : public ShapeBase<S_>
 
   using S = S_;
 
-  /// @brief Constructing a convex, providing normal and offset of each polytype surface, and the points and shape topology information 
+  // TODO(SeanCurtis-TRI): Figure out what the ownership of this data is. It
+  // *seems* that this struct only owns the edges.
+  /// @brief Definition of a convex polytope.
+  ///
+  /// This requires *two* coordinated representations of the convex hull:
+  ///   - The set of half spaces that bound the hull region and
+  ///   - the polytope mesh formed by the intersection of the half spaces.
+  ///
+  /// The half spaces are defined by the implicit equation of a plane:
+  /// `Π(x, y, z) = Ax + By + Cz + d = 0` where `n̂ = [A, B, C]` and |n̂| = 1.
+  /// The normal points *outside* of the convex region. There should be *no*
+  /// redundant planes in the set.
+  ///
+  /// For each plane Π there should corresponding face f that lies on that plane
+  /// and whose edges and vertices are defined by the intersection of all other
+  /// planes with plane Π.
+  ///
+  /// @note: The %Convex geometry does *not* take ownership of any of the data
+  /// provided. The data must remain valid for as long as the %Convex instance
+  /// and must be cleaned up explicitly.
+  ///
+  /// @param plane_normals  For m planes, the normals of plane i:
+  ///                       `[n̂₀, n̂₁, ..., n̂ₘ₋₁]`.
+  /// @param plane_dis      For m planes, the offset values of plane i:
+  ///                       `[d₀, d₁, ..., dₘ₋₁]`.
+  /// @param num_planes     The number of planes `m`.
+  /// @param points         All unique vertices formed by the intersection of
+  ///                       three or more planes.
+  /// @param num_points     The number of vertices in `points`.
+  /// @param polygons       Encoding of the faces for each plane. See member
+  ///                       documentation for details on encoding.
   Convex(Vector3<S>* plane_normals,
          S* plane_dis,
          int num_planes,
@@ -67,15 +97,35 @@ class FCL_EXPORT Convex : public ShapeBase<S_>
   /// @brief Compute AABB<S>
   void computeLocalAABB() override;
 
-  /// @brief Get node type: a conex polytope 
+  /// @brief Get node type: a convex polytope
   NODE_TYPE getNodeType() const override;
 
-
+  /// @brief
   Vector3<S>* plane_normals;
   S* plane_dis;
 
-  /// @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 The representation of the *faces* of the convex hull.
+  ///
+  /// The array is the concatenation of integer-based representations of each
+  /// face. A single face is encoded as a sub-array of ints where the first int
+  /// is the *number* n of vertices in the face, and the following n values
+  /// are ordered indices into `points` of the vertices in a *counter-clockwise*
+  /// order (viewed from the outside).
+  ///
+  /// For a well-formed face `f` consisting of indices [v₀, v₁, ..., vₘ₋₁], it
+  /// should be the case that:
+  ///
+  ///    `rᵢ × rᵢ₊₁ · n̂ₚ = |rᵢ × rᵢ₊₁|, ∀ 0 ≤ i < m, i ∈ ℤ`, where
+  ///    `n̂ₚ` is the normal for plane `p` on which the face `f` lies.
+  ///    `rᵢ = points[vᵢ] - points[vᵢ₋₁]` is the displacement of the edge of
+  ///    face `f` defined by adjacent vertex indices at iᵗʰ vertex (wrapping
+  ///    around such that i - 1 = m - 1 for i = 0).
+  ///
+  /// Satisfying this condition implies the following:
+  ///    1. Vertices are not coincident,
+  ///    2. The nᵗʰ encoded polygon corresponds with the nᵗʰ plane normal,
+  ///    3. The indices of the face correspond to a proper counter-clockwise
+  ///       ordering.
   int* polygons;
 
   Vector3<S>* points;
@@ -90,7 +140,10 @@ class FCL_EXPORT Convex : public ShapeBase<S_>
 
   Edge* edges;
 
-  /// @brief center of the convex polytope, this is used for collision: center is guaranteed in the internal of the polytope (as it is convex) 
+  /// @brief The mean point of the convex polytope, used for collision. This
+  /// point is guaranteed to be inside the convex hull.
+  /// note: The name "center" is misleading; it is neither the centroid nor the
+  /// center of mass.
   Vector3<S> center;
 
   /// based on http://number-none.com/blow/inertia/bb_inertia.doc

test/CMakeLists.txt

@@ -96,4 +96,5 @@ foreach(test ${tests})
   add_fcl_test(${test})
 endforeach(test)
 
+add_subdirectory(geometry)
 add_subdirectory(narrowphase)

test/geometry/CMakeLists.txt

@@ -0,0 +1 @@
+add_subdirectory(shape)

test/geometry/shape/CMakeLists.txt

@@ -0,0 +1,8 @@
+set(tests
+        test_convex.cpp
+        )
+
+# Build all the tests
+foreach(test ${tests})
+    add_fcl_test(${test})
+endforeach(test)

test/geometry/shape/test_convex.cpp

@@ -0,0 +1,226 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2018. Toyota Research Institute
+ *  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 CNRS-LAAS and AIST 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 Sean Curtis ([email protected]) (2018) */
+
+// Tests the implementation of a convex hull.
+
+#include "fcl/geometry/shape/convex.h"
+
+#include <gtest/gtest.h>
+
+#include "eigen_matrix_compare.h"
+#include "fcl/common/types.h"
+
+// TODO(SeanCurtis-TRI): This is only the *first* component of the tests for
+// the geometry representation. The following list of features must *also* be
+// tested
+//   - computeMomentofInertia
+//   - computeCOM()
+//   - computeVolume()
+//   - fillEdges() (via constructor)
+//   - getBoundVertices()
+//
+// The tests should also include some *other* convex shape where the planes are
+// not mutually orthogonal.
+
+namespace fcl {
+
+// A simple box with sides of length 1. It can provide the definition of its
+// own convex hull.
+template <typename S>
+class UnitBox {
+ public:
+  // Instantiate the box with the given pose of the box in the world frame
+  // (defaults to the identity pose).
+  explicit UnitBox(const Transform3<S>& X_WB = Transform3<S>::Identity()) :
+      mean_point_(X_WB.translation()){
+    // Note: gcc-4.8 does *not* allow normal_ and points_ to be initialized
+    // with these initializer lists at declaration. So, for broadest
+    // compatability, they are initialized here.
+    Vector3<S> normals_B[kFaceCount] = {{1, 0, 0},     // +x
+                                        {-1, 0, 0},    // -x
+                                        {0, 1, 0},     // +y
+                                        {0, -1, 0},    // -y
+                                        {0, 0, 1},     // +z
+                                        {0, 0, -1}};   // -z
+    for (int f = 0; f < kFaceCount; ++f) {
+      normals_[f] = X_WB.linear() * normals_B[f];
+    }
+
+    Vector3<S> points_B[kPointCount] = {{S(-0.5), S(-0.5), S(-0.5)},
+                                        {S(0.5), S(-0.5), S(-0.5)},
+                                        {S(-0.5), S(0.5), S(-0.5)},
+                                        {S(0.5), S(0.5), S(-0.5)},
+                                        {S(-0.5), S(-0.5), S(0.5)},
+                                         {S(0.5), S(-0.5), S(0.5)},
+                                        {S(-0.5), S(0.5), S(0.5)},
+                                        {S(0.5), S(0.5), S(0.5)}};
+    for (int p = 0; p < kPointCount; ++p) {
+      points_[p] = X_WB * points_B[p];
+    }
+  }
+
+  // Getters for convex hull instantiation.
+  Vector3<S>* plane_normals() { return &normals_[0]; }
+  S* plane_offsets() { return &offsets_[0]; }
+  int plane_count() const { return kFaceCount; }
+  Vector3<S>* points() { return &points_[0]; }
+  int point_count() const { return kPointCount; }
+  int* polygons() { return &polygons_[0]; }
+  Vector3<S> mean_point() const { return mean_point_; }
+  S aabb_radius() const { return sqrt(3) / 2; }
+  Convex<S> hull() {
+    return Convex<S>(plane_normals(), plane_offsets(), kFaceCount,
+                     points(), kPointCount, polygons());
+  }
+  Vector3<S> min_point() const {
+    Vector3<S> m;
+    m.setConstant(std::numeric_limits<S>::max());
+    for (int p = 0; p < kPointCount; ++p) {
+      for (int i = 0; i < 3; ++i) {
+        if (points_[p](i) < m(i)) m(i) = points_[p](i);
+      }
+    }
+    return m;
+  }
+  Vector3<S> max_point() const {
+    Vector3<S> m;
+    m.setConstant(-std::numeric_limits<S>::max());
+    for (int p = 0; p < kPointCount; ++p) {
+      for (int i = 0; i < 3; ++i) {
+        if (points_[p](i) > m(i)) m(i) = points_[p](i);
+      }
+    }
+    return m;
+  }
+
+ private:
+  static constexpr int kFaceCount = 6;
+  static constexpr int kPointCount = 8;
+  // The implicit equations of the six planes of the cube.
+  Vector3<S> normals_[kFaceCount];
+  S offsets_[kFaceCount] = {S(-0.5), S(-0.5), S(-0.5), S(-0.5), S(-0.5),
+                            S(-0.5)};
+  // The intersecting points of the planes (i.e., corners of the cube).
+  Vector3<S> points_[kPointCount];
+  // The convex-hull encoding of the cube faces. As a reality check, each vertex
+  // index should appear three times.
+  int polygons_[kFaceCount * 5] = {4, 1, 3, 7, 5,    // +x
+                                   4, 0, 4, 6, 2,    // -x
+                                   4, 4, 5, 7, 6,    // +y
+                                   4, 0, 2, 3, 1,    // -y
+                                   4, 6, 7, 3, 2,    // +z
+                                   4, 0, 1, 5, 4};   // -z
+  Vector3<S> mean_point_;
+};
+
+template <typename S>
+void testConvexConstruction(const Transform3<S>& X_WB) {
+  UnitBox<S> box(X_WB);
+  Convex<S> hull = box.hull();
+
+  // This doesn't depend on the correct logic in the constructor. But this is
+  // as convenient a time as any to test that it reports the right node type.
+  EXPECT_EQ(hull.getNodeType(), GEOM_CONVEX);
+
+  // The constructor does work. It computes the mean point (which it calls
+  // "center") and defines the edges. Let's confirm they are correct.
+  EXPECT_TRUE(CompareMatrices(hull.center, box.mean_point()));
+  GTEST_ASSERT_NE(hull.edges, nullptr);
+  // TODO(SeanCurtis-TRI): Test the edge definitions.
+}
+
+template <typename S>
+void testABBComputation(const Transform3<S>& X_WB, typename constants<S>::Real eps) {
+  UnitBox<S> box(X_WB);
+  Convex<S> hull = box.hull();
+  hull.computeLocalAABB();
+
+  EXPECT_NEAR(box.aabb_radius(), hull.aabb_radius, eps);
+  EXPECT_TRUE(CompareMatrices(box.mean_point(), hull.aabb_center, eps));
+  EXPECT_TRUE(CompareMatrices(box.min_point(), hull.aabb_local.min_, eps));
+  EXPECT_TRUE(CompareMatrices(box.max_point(), hull.aabb_local.max_, eps));
+}
+
+GTEST_TEST(ConvexGeometry, Constructor) {
+  testConvexConstruction(Transform3<double>::Identity());
+  testConvexConstruction(Transform3<float>::Identity());
+}
+
+
+GTEST_TEST(ConvexGeometry, LocalAABBComputation) {
+  Transform3<double> X_WB = Transform3<double>::Identity();
+
+  const double eps_d = constants<double>::eps();
+  const float eps_f = constants<float>::eps();
+
+  // Identity.
+  testABBComputation(X_WB, eps_d);
+  testABBComputation(X_WB.cast<float>(), eps_f);
+
+  // 90-degree rotation around each axis, in turn.
+  for (int i = 0; i < 3; ++i) {
+    X_WB.linear() = AngleAxis<double>(constants<double>::pi(),
+                                      Vector3<double>::Unit(i)).matrix();
+    testABBComputation(X_WB, eps_d);
+    testABBComputation(X_WB.cast<float>(), eps_f);
+  }
+
+  // Small angle away from identity.
+  X_WB.linear() = AngleAxis<double>(1e-5, Vector3<double>{1, 2, 3}.normalized())
+      .matrix();
+  // NOTE: The *really* small angle *kills* precision in computing the radius.
+  // Single- and double-precision only get the right answer to approximately the
+  // same tolerance.
+  // TODO(SeanCurtis-TRI): Figure out why this is the case.
+  testABBComputation(X_WB, 1e-5);
+  testABBComputation(X_WB.cast<float>(), 1e-5f);
+
+  // Simple translation.
+  X_WB.linear() = Matrix3<double>::Identity();
+  X_WB.translation() << 1, -2, 3;
+  testABBComputation(X_WB, eps_d);
+  testABBComputation(X_WB.cast<float>(), eps_f);
+}
+  // computeLocalAABB()
+  // getNodeType()
+}  // namespace fcl
+
+//==============================================================================
+int main(int argc, char *argv[]) {
+  ::testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
+