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 an API to make use of the opaque id returned for
`fcl::OcTree` cells. There are four new APIs. The last three also have
to be passed the contact point or nearest point on the cell to work
correctly:

getNode(id): returns the octomap node pointer for an id
getNodeBV(id, point): gets cell AABB
getNodeBox(id, point): gets a box, tf pair
getNodeKeyAndDepth(id, point): gets an octree key, depth pair

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

Add OcTree id tests to test the new API 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,90 @@ void computeChildBV(const AABB<S>& root_bv, unsigned int i, AABB<S>& child_bv)
   }
 }
 
+//==============================================================================
+template <typename S>
+const typename OcTree<S>::OcTreeNode* OcTree<S>::getNode(intptr_t id) const
+{
+  return getRoot() + id;
+}
+
+//==============================================================================
+template <typename S>
+bool OcTree<S>::getNodeBV(intptr_t id, const Vector3<S>& point, AABB<S>& aabb) const
+{
+  octomap::OcTree::leaf_bbx_iterator it;
+  if (getOctomapIterator(id, point, &it))
+  {
+    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;
+    return true;
+  }
+  return false;
+}
+
+//==============================================================================
+template <typename S>
+bool OcTree<S>::getNodeBox(intptr_t id, const Vector3<S>& point, Box<S>& box, Transform3<S>& box_tf) const
+{
+  AABB<S> aabb;
+  if (getNodeBV(id, point, aabb))
+  {
+    constructBox(aabb, box, box_tf);
+    return true;
+  }
+  return false;
+}
+
+template <typename S>
+bool OcTree<S>::getNodeKeyAndDepth(intptr_t id, const Vector3<S>& point, octomap::OcTreeKey& key, unsigned int& depth) const
+{
+  octomap::OcTree::leaf_bbx_iterator it;
+  if (getOctomapIterator(id, point, &it))
+  {
+    key = it.getKey();
+    depth = it.getDepth();
+    return true;
+  }
+  return false;
+}
+
+template <typename S>
+bool OcTree<S>::getOctomapIterator(intptr_t id, const Vector3<S>& point, octomap::OcTree::leaf_bbx_iterator* out) const
+{
+  // 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.
+  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 : 0;
+    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);
+  octomap::OcTree::leaf_bbx_iterator end = tree->end_leafs_bbx();
+  const OcTreeNode* node = getNode(id);
+  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
@@ -131,6 +132,36 @@ class FCL_EXPORT OcTree : public CollisionGeometry<S>
 
   /// @brief return node type, it is an octree
   NODE_TYPE getNodeType() const;
+
+  /// @brief Access a node from an opaque cell id
+  ///
+  /// Return the node pointer from the opaque id of a cell.
+  /// The id of a cell is the opaque id returned by collision and
+  /// distance queries. The node pointer can be used to retrieve any
+  /// information stored in the cell, such as occupancy information.
+  const OcTreeNode* getNode(intptr_t id) const;
+
+  /// @brief Return the BV of the given opaque cell id and point on cell
+  /// @return true if the cell was found and aabb updated
+  bool getNodeBV(intptr_t id, const Vector3<S>& point, AABB<S>& aabb) const;
+
+  /// @brief Return the box of the given opaque cell id and point on cell
+  /// @return true if the cell was found and box and tf updated
+  bool getNodeBox(intptr_t id, const Vector3<S>& point, Box<S>& box, Transform3<S>& box_tf) const;
+
+  /// @brief Return the octomap::OcTreeKey and depth from the opaque id and point on cell
+  /// @return true if the cell was found and key and depth updated
+  bool getNodeKeyAndDepth(intptr_t id, const Vector3<S>& point, octomap::OcTreeKey& key, unsigned int& depth) const;
+
+protected:
+  /// @brief get the leaf_bbx_iterator pointing to the given cell
+  ///
+  /// @param id    opaque id returned in the contact information or distance result
+  /// @param point a point on or in the cell returned in the contact
+  ///              information or distance result
+  /// @param out   pointer to output iterator
+  /// @return true if the cell was found
+  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,14 @@ 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 opaque id of the cell (see fcl::OcTree::getNode and friends)
+  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 opaque id of the cell (see fcl::OcTree::getNode and friends)
+  intptr_t b2;
 
   /// @brief contact normal, pointing from o1 to o2
   Vector3<S> normal;

include/fcl/narrowphase/distance_result.h

@@ -77,14 +77,14 @@ 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 opaque id of the cell (see fcl::OcTree::getNode and friends)
+  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 opaque id of the cell (see fcl::OcTree::getNode and friends)
+  intptr_t b2;
 
   /// @brief invalid contact primitive information
   static const int NONE = -1;

test/test_fcl_octomap_collision.cpp

@@ -61,7 +61,7 @@ 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 +109,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 +329,13 @@ 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 +349,16 @@ 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);
+
+      fcl::AABB<S> temp_aabb;
+      const fcl::OcTree<S>* contact_tree = static_cast<const fcl::OcTree<S>*> (contact.o1);
+      EXPECT_TRUE(contact_tree->getNodeBV(contact.b1, contact.pos, temp_aabb));
+      octomap::OcTreeKey key;
+      unsigned int depth;
+      EXPECT_TRUE(contact_tree->getNodeKeyAndDepth(contact.b1, contact.pos, key, depth));
+      typename fcl::OcTree<S>::OcTreeNode* octree_node = octree->search(key, depth);
+      EXPECT_TRUE(octree_node == contact_tree->getNode(contact.b1));
+
       const fcl::BVHModel<fcl::OBBRSS<S>>* surface = static_cast<const fcl::BVHModel<fcl::OBBRSS<S>>*> (contact.o2);
       EXPECT_TRUE(surface->num_tris > contact.b2);
     }