diff --git a/include/hpp/fcl/collision_data.h b/include/hpp/fcl/collision_data.h index be3c4e6a7ae1c6b4dcc69ee5e2fb4588052450ba..eec9c36e099b1d61c7772b490ae2f58ab6b9a76b 100644 --- a/include/hpp/fcl/collision_data.h +++ b/include/hpp/fcl/collision_data.h @@ -309,6 +309,10 @@ public: contacts.clear(); cost_sources.clear(); } + + /// @brief reposition Contact objects when fcl inverts them + /// during their construction. + friend void invertResults(CollisionResult& result); }; diff --git a/src/collision.cpp b/src/collision.cpp index 5178a45afa79701db2916f1ea0fb074767fd9907..d71c1231919afef403732b3f2046460d487a30a0 100644 --- a/src/collision.cpp +++ b/src/collision.cpp @@ -62,6 +62,23 @@ std::size_t collide(const CollisionObject* o1, const CollisionObject* o2, nsolver, request, result); } +// reorder collision results in the order the call has been made. +void invertResults(CollisionResult& result) +{ + const CollisionGeometry* otmp; + int btmp; + for(std::vector<Contact>::iterator it = result.contacts.begin(); + it != result.contacts.end(); ++it) + { + otmp = it->o1; + it->o1 = it->o2; + it->o2 = otmp; + btmp = it->b1; + it->b1 = it->b2; + it->b2 = btmp; + } +} + template<typename NarrowPhaseSolver> std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, @@ -96,7 +113,10 @@ std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1, res = 0; } else + { res = looktable.collision_matrix[node_type2][node_type1](o2, tf2, o1, tf1, nsolver, request, result); + invertResults(result); + } } else {