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); }