change contact, distance primitive id to intptr_t

The contact and distance result brief primitive information stored
the brief information in an `int`. However, some primitive ids can
only be stored by relative pointer (namely octree nodes), and `int`
is not wide enough on 64-bit platforms to safely store a pointer. So
upgrade the brief information to be `intptr_t` which is guaranteed to
be wide enough.

Also, provide a method, getNode, to make use of the query cell id
returned for `fcl::OcTree` cells. For getNode to efficiently find the
cell from the query cell id, it must be passed the contact point or
nearest point returned by the query to work correctly. The getNode
method returns a pointer to the corresponding OcTree node, and also
optionally returns the AABB representing the cell, the
octomap::OcTreeKey (which can be used with various octomap APIs) and
the depth of the cell in the tree (which may also be needed when
using octomap APIs).

Refer the contact info and distance result documentation to the new
getNode method for callers to know how to make use of these OcTree
query cell ids.

Add OcTree query cell id tests to test the new getNode method at the
same time as testing the BVH mesh ids in
test_fcl_octomap_collision.cpp.

include/fcl/common/types.h

@@ -60,6 +60,8 @@ using int64 = std::int64_t;
 using uint64 = std::uint64_t;
 using int32 = std::int32_t;
 using uint32 = std::uint32_t;
+using intptr_t = std::intptr_t;
+using uintptr_t = std::uintptr_t;
 
 template <typename S>
 using Vector2 = Eigen::Matrix<S, 2, 1>;

include/fcl/geometry/octree/octree-inl.h

@@ -42,6 +42,8 @@
 
 #include "fcl/config.h"
 
+#include "fcl/geometry/shape/utility.h"
+
 #if FCL_HAVE_OCTOMAP
 
 namespace fcl
@@ -296,6 +298,84 @@ void computeChildBV(const AABB<S>& root_bv, unsigned int i, AABB<S>& child_bv)
   }
 }
 
+//==============================================================================
+template <typename S>
+const typename OcTree<S>::OcTreeNode* OcTree<S>::getNodeByQueryCellId(
+    intptr_t id,
+    const Vector3<S>& point,
+    AABB<S>* aabb,
+    octomap::OcTreeKey* key,
+    unsigned int* depth) const
+{
+  octomap::OcTree::leaf_bbx_iterator it;
+  if (!getOctomapIterator(id, point, &it))
+  {
+    return nullptr;
+  }
+  if (aabb != nullptr)
+  {
+    Vector3<S> center(it.getX(), it.getY(), it.getZ());
+    double half_size = it.getSize() / 2.0;
+    Vector3<S> half_extent(half_size, half_size, half_size);
+    aabb->min_ = center - half_extent;
+    aabb->max_ = center + half_extent;
+  }
+  if (key != nullptr)
+    *key = it.getKey();
+  if (depth != nullptr)
+    *depth = it.getDepth();
+  return &(*it);
+}
+
+//==============================================================================
+template <typename S>
+bool OcTree<S>::getOctomapIterator(
+    intptr_t id,
+    const Vector3<S>& point,
+    octomap::OcTree::leaf_bbx_iterator* out) const
+{
+  assert(out != nullptr);
+  // The octomap tree structure provides no way to find a node from its pointer
+  // short of an exhaustive search. This could take a long time on a large
+  // tree. Instead, require the user to supply the contact point or nearest
+  // point returned by the query that also returned the id. Use the point to
+  // create the bounds to search for the node pointer.
+  const octomap::OcTreeKey point_key = tree->coordToKey(
+      point[0], point[1], point[2]);
+  // Set the min and max keys used for the bbx to the point key plus or minus
+  // one (if not at the limits of the data type) so we are guaranteed to hit
+  // the correct cell even when the point is on a boundary and rounds to the
+  // wrong cell.
+  octomap::OcTreeKey min_key, max_key;
+  for (unsigned int i = 0; i < 3; ++i)
+  {
+    min_key[i] = (point_key[i] > std::numeric_limits<octomap::key_type>::min() ?
+        point_key[i] - 1 : point_key[i]);
+    max_key[i] = (point_key[i] < std::numeric_limits<octomap::key_type>::max() ?
+        point_key[i] + 1 : point_key[i]);
+  }
+  octomap::OcTree::leaf_bbx_iterator it = tree->begin_leafs_bbx(
+      min_key, max_key);
+  const octomap::OcTree::leaf_bbx_iterator end = tree->end_leafs_bbx();
+  const OcTreeNode* const node = getRoot() + id;
+  // While it may appear like this loop could take forever, in reality it will
+  // only take a few iterations. Octomap iterators use a fixed end iterator
+  // (copied above), and we are guaranteed to get to the end iterator after
+  // incrementing past the bounds of the octomap::OcTree::leaf_bbx_iterator.
+  // Incrementing end will keep the iterator at end as well. This functionality
+  // of octomap iterators is tested extensively in the octomap package tests.
+  while (it != end)
+  {
+    if (node == &(*it))
+    {
+      *out = it;
+      return true;
+    }
+    ++it;
+  }
+  return false;
+}
+
 } // namespace fcl
 
 #endif

include/fcl/geometry/octree/octree.h

@@ -47,6 +47,7 @@
 
 #include <octomap/octomap.h>
 #include "fcl/math/bv/AABB.h"
+#include "fcl/geometry/shape/box.h"
 #include "fcl/narrowphase/collision_object.h"
 
 namespace fcl
@@ -136,6 +137,52 @@ class FCL_EXPORT OcTree : public CollisionGeometry<S>
 
   /// @brief return node type, it is an octree
   NODE_TYPE getNodeType() const;
+
+  /// @brief Access the node and corresponding info by query cell id
+  ///
+  /// The results of distance and collision queries include various pieces of
+  /// information about the objects involved in the query (in DistanceResult
+  /// and Contact, respectively). When an object in the query is an OcTree the
+  /// corresponding member (b1 or b2) contains an opaque identifier refered to
+  /// as the "query cell id" of the corresponding tree cell.
+  ///
+  /// This method returns the node pointer of the given query cell id.
+  /// In order to efficiently find the cell by its query cell id, the contact
+  /// point or nearest point in or on the cell must be given. This method also
+  /// provides optional access to properties of the OcTree cell associated
+  /// with the cell.
+  ///
+  /// If the given cell can not be found, nullptr is returned and no
+  /// information is stored in the output pointer locations. This might happen
+  /// if some other point not in or on the cell is provided or if given a stale
+  /// or invalid query cell id.
+  ///
+  /// @param[in]  id    The query cell id, as given in b1 or b2 in
+  ///                   DistanceResult or Contact.
+  /// @param[in]  point A point in or on the given cell id, from DistanceResult
+  ///                   or Contact.
+  /// @param[out] aabb  If valid, output pointer for the AABB of the cell.
+  /// @param[out] key   If valid, output pointer for the octomap::OcTreeKey of
+  ///                   the cell.
+  /// @param[out] depth If valid, output pointer for the tree depth of the
+  ///                   cell.
+  /// @return The node pointer for the given query cell id, or nullptr if not
+  ///         found.
+  const OcTreeNode* getNodeByQueryCellId(
+      intptr_t id,
+      const Vector3<S>& point,
+      AABB<S>* aabb = nullptr,
+      octomap::OcTreeKey* key = nullptr,
+      unsigned int* depth = nullptr) const;
+
+private:
+  // Get the leaf_bbx_iterator pointing to the given cell given a point on or
+  // in the cell and the query id.
+  // Returns true if the cell was found, and false otherwise.
+  bool getOctomapIterator(
+      intptr_t id,
+      const Vector3<S>& point,
+      octomap::OcTree::leaf_bbx_iterator* out) const;
 };
 
 using OcTreef = OcTree<float>;

include/fcl/narrowphase/contact.h

@@ -56,14 +56,16 @@ struct FCL_EXPORT Contact
   /// @brief contact primitive in object 1
   /// if object 1 is mesh or point cloud, it is the triangle or point id
   /// if object 1 is geometry shape, it is NONE (-1),
-  /// if object 1 is octree, it is the id of the cell
-  int b1;
+  /// if object 1 is octree, it is the query cell id (see
+  ///                OcTree::getNodeByQueryCellId)
+  intptr_t b1;
 
   /// @brief contact primitive in object 2
   /// if object 2 is mesh or point cloud, it is the triangle or point id
   /// if object 2 is geometry shape, it is NONE (-1),
-  /// if object 2 is octree, it is the id of the cell
-  int b2;
+  /// if object 2 is octree, it is the query cell id (see
+  ///                OcTree::getNodeByQueryCellId)
+  intptr_t b2;
 
   /// @brief contact normal, pointing from o1 to o2
   Vector3<S> normal;

include/fcl/narrowphase/distance_result.h

@@ -77,14 +77,16 @@ struct FCL_EXPORT DistanceResult
   /// @brief information about the nearest point in object 1
   /// if object 1 is mesh or point cloud, it is the triangle or point id
   /// if object 1 is geometry shape, it is NONE (-1),
-  /// if object 1 is octree, it is the id of the cell
-  int b1;
+  /// if object 1 is octree, it is the query cell id (see
+  ///                OcTree::getNodeByQueryCellId)
+  intptr_t b1;
 
   /// @brief information about the nearest point in object 2
   /// if object 2 is mesh or point cloud, it is the triangle or point id
   /// if object 2 is geometry shape, it is NONE (-1),
-  /// if object 2 is octree, it is the id of the cell
-  int b2;
+  /// if object 2 is octree, it is the query cell id (see
+  ///                OcTree::getNodeByQueryCellId)
+  intptr_t b2;
 
   /// @brief invalid contact primitive information
   static const int NONE = -1;

test/test_fcl_octomap_collision.cpp

@@ -61,7 +61,11 @@ void octomap_collision_test(S env_scale, std::size_t env_size, bool exhaustive,
 /// @brief Octomap collision with an environment mesh with 3 * env_size objects, asserting that correct triangle ids
 /// are returned when performing collision tests
 template <typename S>
-void octomap_collision_test_mesh_triangle_id(S env_scale, std::size_t env_size, std::size_t num_max_contacts, double resolution = 0.1);
+void octomap_collision_test_contact_primitive_id(
+    S env_scale,
+    std::size_t env_size,
+    std::size_t num_max_contacts,
+    double resolution = 0.1);
 
 template<typename BV>
 void octomap_collision_test_BVH(std::size_t n, bool exhaustive, double resolution = 0.1);
@@ -109,19 +113,19 @@ GTEST_TEST(FCL_OCTOMAP, test_octomap_collision_mesh)
 }
 
 template <typename S>
-void test_octomap_collision_mesh_triangle_id()
+void test_octomap_collision_contact_primitive_id()
 {
 #ifdef NDEBUG
-  octomap_collision_test_mesh_triangle_id<S>(1, 30, 100000);
+  octomap_collision_test_contact_primitive_id<S>(1, 30, 100000);
 #else
-  octomap_collision_test_mesh_triangle_id<S>(1, 10, 10000, 1.0);
+  octomap_collision_test_contact_primitive_id<S>(1, 10, 10000, 1.0);
 #endif
 }
 
-GTEST_TEST(FCL_OCTOMAP, test_octomap_collision_mesh_triangle_id)
+GTEST_TEST(FCL_OCTOMAP, test_octomap_collision_contact_primitive_id)
 {
-//  test_octomap_collision_mesh_triangle_id<float>();
-  test_octomap_collision_mesh_triangle_id<double>();
+//  test_octomap_collision_contact_primitive_id<float>();
+  test_octomap_collision_contact_primitive_id<double>();
 }
 
 template <typename S>
@@ -329,12 +333,18 @@ void octomap_collision_test(S env_scale, std::size_t env_size, bool exhaustive,
 }
 
 template <typename S>
-void octomap_collision_test_mesh_triangle_id(S env_scale, std::size_t env_size, std::size_t num_max_contacts, double resolution)
+void octomap_collision_test_contact_primitive_id(
+    S env_scale,
+    std::size_t env_size,
+    std::size_t num_max_contacts,
+    double resolution)
 {
   std::vector<CollisionObject<S>*> env;
   test::generateEnvironmentsMesh(env, env_scale, env_size);
 
-  OcTree<S>* tree = new OcTree<S>(std::shared_ptr<const octomap::OcTree>(test::generateOcTree(resolution)));
+  std::shared_ptr<const octomap::OcTree> octree(
+      test::generateOcTree(resolution));
+  OcTree<S>* tree = new OcTree<S>(octree);
   CollisionObject<S> tree_obj((std::shared_ptr<CollisionGeometry<S>>(tree)));
 
   std::vector<CollisionObject<S>*> boxes;
@@ -348,6 +358,31 @@ void octomap_collision_test_mesh_triangle_id(S env_scale, std::size_t env_size,
     for(std::size_t index=0; index<cResult.numContacts(); ++index)
     {
       const Contact<S>& contact = cResult.getContact(index);
+
+      const fcl::OcTree<S>* contact_tree = static_cast<const fcl::OcTree<S>*>(
+          contact.o1);
+      fcl::AABB<S> aabb;
+      octomap::OcTreeKey key;
+      unsigned int depth;
+      auto get_node_rv = contact_tree->getNodeByQueryCellId(
+          contact.b1,
+          contact.pos,
+          &aabb,
+          &key,
+          &depth);
+      EXPECT_TRUE(get_node_rv != nullptr);
+      auto center_octomap_point = octree->keyToCoord(key);
+      double cell_size = octree->getNodeSize(depth);
+      for (unsigned i = 0; i < 3; ++i)
+      {
+        EXPECT_FLOAT_EQ(
+            aabb.min_[i], center_octomap_point(i) - cell_size / 2.0);
+        EXPECT_FLOAT_EQ(
+            aabb.max_[i], center_octomap_point(i) + cell_size / 2.0);
+      }
+      auto octree_node = octree->search(key, depth);
+      EXPECT_TRUE(octree_node == get_node_rv);
+
       const fcl::BVHModel<fcl::OBBRSS<S>>* surface = static_cast<const fcl::BVHModel<fcl::OBBRSS<S>>*> (contact.o2);
       EXPECT_TRUE(surface->num_tris > contact.b2);
     }