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..8069ac273 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,74 @@ void computeChildBV(const AABB& root_bv, unsigned int i, AABB& child_bv) } } +//============================================================================== +template +const typename OcTree::OcTreeNode* OcTree::getNodeByQueryCellId( + intptr_t id, + const Vector3& point, + AABB* 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 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; + } + if (key != nullptr) + *key = it.getKey(); + if (depth != nullptr) + *depth = it.getDepth(); + return &(*it); +} + +//============================================================================== +template +bool OcTree::getOctomapIterator( + intptr_t id, + const Vector3& 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::min() ? point_key[i] - 1 : point_key[i]; + 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); + const octomap::OcTree::leaf_bbx_iterator end = tree->end_leafs_bbx(); + const OcTreeNode* const node = getRoot() + 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 2af744200..f36c468fe 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 @@ -135,6 +136,47 @@ class FCL_EXPORT OcTree : public CollisionGeometry /// @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 a query result. + /// @param[in] point A point in or on the given cell id, from a query result. + /// @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& point, + AABB* 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& 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..ac2f7435d 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 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 normal; diff --git a/include/fcl/narrowphase/distance_result.h b/include/fcl/narrowphase/distance_result.h index 045d2c80d..e7ad724ee 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 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; diff --git a/test/test_fcl_octomap_collision.cpp b/test/test_fcl_octomap_collision.cpp index 3c711884a..1636ba11d 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,28 @@ 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); + + const fcl::OcTree* contact_tree = static_cast*> (contact.o1); + fcl::AABB 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>* surface = static_cast>*> (contact.o2); EXPECT_TRUE(surface->num_tris > contact.b2); }