search

flexible-collision-library / fcl

Flexible Collision Library
https://flexible-collision-library.github.io/
Other
1.35k stars 415 forks source link

The order of collision geometry in fcl::Contact #601

Open wuyingnan opened 1 year ago

wuyingnan commented 1 year ago

Hello,

I'm wondering if there is any guarantee of order consistency between fcl::collide and returned fcl::CollisionResult. I.e., if I call fcl::collide(o1, o2, request, result), does the result.contact.o1 is always equal to o1.getCollisionGeometry()?

If this is not the case, how can I get the correct correspondence between fcl::CollisionObject and fcl::CollsionGeometry? If the collision geometry is shared by two CollisionObject, the result.contact will have same CollisionGeometry address for both o1 and o2.

SeanCurtis-TRI commented 1 year ago

With 95% confidence, if you call either variant of fcl::collide located in this file, the CollisionResults output "objects" should have the same ordering as the input objects.

If you need to harden your code to make sure this is definitively true, the answer is actually hiding in your question. Simply compare o1.getCollisionGeometry() to Contact::o1 (and the same for o2).

For the record, if those objects lie in a broadphase structure, you are not guaranteed that if they do collide that one will always be o1 and the other o2. They can swap roles from evaluation to evaluation (even if they don't move).

wuyingnan commented 1 year ago

@SeanCurtis-TRI Thank you for the reply.

The problem is, if I have two collision objects o1 and o2 that share the same collision geometry, the assertion o1.getCollisionGeometry() == Contact::o1 && o2.getCollisionGeometry() == Contact::o1 && Contact::o1 == Contact::o2 is always evaluated as true. However, the returned normal vector may be inverted in this case. My code can do nothing about that.

It seems that ShapeOcTreeCollisionTraversalNode does not respect the order. The order is inverted in this line. Also, the OcTreeSolver::ShapeOcTreeIntersect does not respect the order in this line. And finally, we do not have a corresponding OcTreeSolver::ShapeOcTreeIntersectRecurse for OcTreeSolver::ShapeOcTreeIntersect.

SeanCurtis-TRI commented 1 year ago

Ah....I've never used the OcTree code and haven't even looked at it. So, all bets are off for that. :) As I indicated, there are inversions due to broadphase traversal, so I can't say I'm surprised they might exist elsewhere.

For other shapes, I'd predict that the normals and what not are better behaved. Feel free to submit a PR to respect the ordering of the parameters. I'll be happy to review it. It'll give me a chance to look at a corner of the code I've otherwise ignored so far.

wuyingnan commented 1 year ago

OK. I'm happy to provide a PR