From a64447b39b10b44072ca6364bdac357e1e1cdf4d Mon Sep 17 00:00:00 2001 From: "C. Andy Martin" Date: Wed, 6 May 2020 17:00:04 -0900 Subject: [PATCH] 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 | 2 + include/fcl/geometry/octree/octree-inl.h | 86 +++++++++++++++++++++++ include/fcl/geometry/octree/octree.h | 31 ++++++++ include/fcl/narrowphase/contact.h | 8 +-- include/fcl/narrowphase/distance_result.h | 8 +-- test/test_fcl_octomap_collision.cpp | 29 +++++--- 6 files changed, 147 insertions(+), 17 deletions(-) diff --git a/include/fcl/common/types.h b/include/fcl/common/types.h index 53f49d711..892e2b07f 100644 --- a/include/fcl/common/types.h +++ b/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 using Vector2 = Eigen::Matrix; diff --git a/include/fcl/geometry/octree/octree-inl.h b/include/fcl/geometry/octree/octree-inl.h index b22b1e17d..5f11dca56 100644 --- a/include/fcl/geometry/octree/octree-inl.h +++ b/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& root_bv, unsigned int i, AABB& child_bv) } } +//============================================================================== +template +const typename OcTree::OcTreeNode* OcTree::getNode(intptr_t id) const +{ + return getRoot() + id; +} + +//============================================================================== +template +bool OcTree::getNodeBV(intptr_t id, const Vector3& point, AABB& aabb) const +{ + octomap::OcTree::leaf_bbx_iterator it; + if (getOctomapIterator(id, point, &it)) + { + Vector3 center(it.getX(), it.getY(), it.getZ()); + double half_size = it.getSize() / 2.0; + Vector3 half_extent(half_size, half_size, half_size); + aabb.min_ = center - half_extent; + aabb.max_ = center + half_extent; + return true; + } + return false; +} + +//============================================================================== +template +bool OcTree::getNodeBox(intptr_t id, const Vector3& point, Box& box, Transform3& box_tf) const +{ + AABB aabb; + if (getNodeBV(id, point, aabb)) + { + constructBox(aabb, box, box_tf); + return true; + } + return false; +} + +template +bool OcTree::getNodeKeyAndDepth(intptr_t id, const Vector3& 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 +bool OcTree::getOctomapIterator(intptr_t id, const Vector3& 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::min() ? point_key[i] - 1 : 0; + max_key[i] = point_key[i] < std::numeric_limits::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 diff --git a/include/fcl/geometry/octree/octree.h b/include/fcl/geometry/octree/octree.h index 58f73645d..7d440be80 100644 --- a/include/fcl/geometry/octree/octree.h +++ b/include/fcl/geometry/octree/octree.h @@ -47,6 +47,7 @@ #include #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 /// @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& point, AABB& 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& point, Box& box, Transform3& 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& 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& point, octomap::OcTree::leaf_bbx_iterator* out) const; }; using OcTreef = OcTree; diff --git a/include/fcl/narrowphase/contact.h b/include/fcl/narrowphase/contact.h index 0994036e4..a8da95019 100644 --- a/include/fcl/narrowphase/contact.h +++ b/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 normal; diff --git a/include/fcl/narrowphase/distance_result.h b/include/fcl/narrowphase/distance_result.h index 045d2c80d..f9d36c6eb 100644 --- a/include/fcl/narrowphase/distance_result.h +++ b/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; diff --git a/test/test_fcl_octomap_collision.cpp b/test/test_fcl_octomap_collision.cpp index 3c711884a..7d60af019 100644 --- a/test/test_fcl_octomap_collision.cpp +++ b/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 -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 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 -void test_octomap_collision_mesh_triangle_id() +void test_octomap_collision_contact_primitive_id() { #ifdef NDEBUG - octomap_collision_test_mesh_triangle_id(1, 30, 100000); + octomap_collision_test_contact_primitive_id(1, 30, 100000); #else - octomap_collision_test_mesh_triangle_id(1, 10, 10000, 1.0); + octomap_collision_test_contact_primitive_id(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(); - test_octomap_collision_mesh_triangle_id(); +// test_octomap_collision_contact_primitive_id(); + test_octomap_collision_contact_primitive_id(); } template @@ -329,12 +329,13 @@ void octomap_collision_test(S env_scale, std::size_t env_size, bool exhaustive, } template -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*> env; test::generateEnvironmentsMesh(env, env_scale, env_size); - OcTree* tree = new OcTree(std::shared_ptr(test::generateOcTree(resolution))); + std::shared_ptr octree(test::generateOcTree(resolution)); + OcTree* tree = new OcTree(octree); CollisionObject tree_obj((std::shared_ptr>(tree))); std::vector*> 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& contact = cResult.getContact(index); + + fcl::AABB temp_aabb; + const fcl::OcTree* contact_tree = static_cast*> (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::OcTreeNode* octree_node = octree->search(key, depth); + EXPECT_TRUE(octree_node == contact_tree->getNode(contact.b1)); + const fcl::BVHModel>* surface = static_cast>*> (contact.o2); EXPECT_TRUE(surface->num_tris > contact.b2); }