From 177fb1fab82cdb27435d9ca590d14b6169235730 Mon Sep 17 00:00:00 2001
From: Steve Tonneau <stonneau@axle.laas.fr>
Date: Wed, 19 Aug 2015 11:10:14 +0200
Subject: [PATCH] reordering collision results between shapes and meshes
---
include/hpp/fcl/collision_data.h | 4 ++++
src/collision.cpp | 20 ++++++++++++++++++++
2 files changed, 24 insertions(+)
diff --git a/include/hpp/fcl/collision_data.h b/include/hpp/fcl/collision_data.h
index be3c4e6a..eec9c36e 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 5178a45a..d71c1231 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
{
--
GitLab