Commit 177fb1fa authored by Steve Tonneau's avatar Steve Tonneau
Browse files

reordering collision results between shapes and meshes

parent fdd6d8df
......@@ -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);
};
......
......@@ -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
{
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment