Skip to content

Commit

Permalink
Merge pull request #114 from stonneau/master
Browse files Browse the repository at this point in the history
[BUG FIX]octree vs mesh CollisionResult now returns triangle id.
  • Loading branch information
sherm1 committed Apr 10, 2016
2 parents 0cd30df + 8934be7 commit 470cc2f
Show file tree
Hide file tree
Showing 2 changed files with 37 additions and 2 deletions.
4 changes: 2 additions & 2 deletions include/fcl/traversal/traversal_node_octree.h
Original file line number Diff line number Diff line change
Expand Up @@ -597,7 +597,7 @@ class OcTreeSolver
{
is_intersect = true;
if(cresult->numContacts() < crequest->num_max_contacts)
cresult->addContact(Contact(tree1, tree2, root1 - tree1->getRoot(), root2));
cresult->addContact(Contact(tree1, tree2, root1 - tree1->getRoot(), primitive_id));
}
}
else
Expand All @@ -610,7 +610,7 @@ class OcTreeSolver
{
is_intersect = true;
if(cresult->numContacts() < crequest->num_max_contacts)
cresult->addContact(Contact(tree1, tree2, root1 - tree1->getRoot(), root2, contact, normal, depth));
cresult->addContact(Contact(tree1, tree2, root1 - tree1->getRoot(), primitive_id, contact, normal, depth));
}
}

Expand Down
35 changes: 35 additions & 0 deletions test/test_fcl_octomap.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -41,6 +41,7 @@
#include "fcl/config.h"
#include "fcl/octree.h"
#include "fcl/traversal/traversal_node_octree.h"
#include "fcl/collision.h"
#include "fcl/broadphase/broadphase.h"
#include "fcl/shape/geometric_shape_to_BVH_model.h"
#include "fcl/math/transform.h"
Expand Down Expand Up @@ -89,6 +90,10 @@ void octomap_cost_test(double env_scale, std::size_t env_size, std::size_t num_m
/// @brief Octomap distance with an environment with 3 * env_size objects
void octomap_distance_test(double env_scale, std::size_t env_size, bool use_mesh, bool use_mesh_octomap);

/// @brief Octomap collision with an environment mesh with 3 * env_size objects, asserting that correct triangle ids
/// are returned when performing collision tests
void octomap_collision_test_mesh_triangle_id(double env_scale, std::size_t env_size, std::size_t num_max_contacts);


template<typename BV>
void octomap_collision_test_BVH(std::size_t n, bool exhaustive);
Expand Down Expand Up @@ -132,6 +137,12 @@ BOOST_AUTO_TEST_CASE(test_octomap_collision_mesh)
#endif
}

BOOST_AUTO_TEST_CASE(test_octomap_collision_mesh_triangle_id)
{
octomap_collision_test_mesh_triangle_id(1, 30, 100000);
}


BOOST_AUTO_TEST_CASE(test_octomap_collision_mesh_octomap_box)
{
#ifdef FCL_BUILD_TYPE_DEBUG
Expand Down Expand Up @@ -540,6 +551,30 @@ void octomap_collision_test(double env_scale, std::size_t env_size, bool exhaust
std::cout << "Note: octomap may need more collides when using mesh, because octomap collision uses box primitive inside" << std::endl;
}

void octomap_collision_test_mesh_triangle_id(double env_scale, std::size_t env_size, std::size_t num_max_contacts)
{
std::vector<CollisionObject*> env;
generateEnvironmentsMesh(env, env_scale, env_size);

OcTree* tree = new OcTree(std::shared_ptr<const octomap::OcTree>(generateOcTree()));
CollisionObject tree_obj((std::shared_ptr<CollisionGeometry>(tree)));

std::vector<CollisionObject*> boxes;
generateBoxesFromOctomap(boxes, *tree);
for(std::vector<CollisionObject*>::const_iterator cit = env.begin();
cit != env.end(); ++cit)
{
fcl::CollisionRequest req(num_max_contacts, true);
fcl::CollisionResult cResult;
fcl::collide(&tree_obj, *cit, req, cResult);
for(std::size_t index=0; index<cResult.numContacts(); ++index)
{
const Contact& contact = cResult.getContact(index);
const fcl::BVHModel<fcl::OBBRSS>* surface = static_cast<const fcl::BVHModel<fcl::OBBRSS>*> (contact.o2);
BOOST_CHECK(surface->num_tris > contact.b2);
}
}
}

void octomap_distance_test(double env_scale, std::size_t env_size, bool use_mesh, bool use_mesh_octomap)
{
Expand Down

0 comments on commit 470cc2f

Please sign in to comment.