Fix inconsistent normal directions

[jslee02]

According to the comment, contact normal should point from object 1 to object 2. However, shape-shape and shape-mesh collision algorithms doesn't follow the convention. This pull request fixes it.

A bunch of tests in test/test_fcl_geometric_shapes.cpp are updated to be able to check the normals.

[jslee02]

This PR is ready to merge. I'll wait another one day for any comments.

[scpeters]

@jslee02 I think you should get at least one review. I started to look at this before, but there are thousands of changed lines in one of the tests. Can you summarize the changes in that file?

[jslee02]

The most changes are made in test_fcl_geometric_shapes.cpp. It has a bunch of collision/distance tests for shapes (e.g., box, sphere, triangle, plane, and so on) with two GJK solvers: libccd's and built-in solver (called GJKSolver_indep). I changed only collision tests since this PR is related to normal, which is a result of collision query but not distance query.

Previously, the collision tests checked only whether shape 1 and shape 2 are in contact or not in various relative transformation cases, but not contact information: normal, contact point, penetration depth. That's why the tests couldn't catch the fact that the normal directions didn't follow the convention.

I first wrote a test function, testShapeInersection(~) (and some utility functions for it), that can check the contact information. It's a general function in terms of the type of shapes, transformations of the shapes, GJK solvers, numerical tolerance so that it can be used in all the collision tests for various cases. It is still possible to check only whether the shapes are in contact or not as before by passing null pointers for the expected contact information parameters.

So I modified the previous test function to use the new test function. The expected normal directions are based on the convention.

Even though the test function I wrote is able to check contact points and penetration depth, I used it only for checking the normal because this PR is for resolving the normal issue. If this can be a matter, then I can change the function only for normal. But it seems useful to make it to check the other contact information to me.

Also, I added new test set for some missing collision tests for specific shape-shape combination: halfspace-triangle, plane-triangle tests, (GJK) sphere-capsule, (GJK) sphere-triangle, (GJK) halfspace-triangle and plane-triangle. (GJK) means it used the built-in GJK solver.

[scpeters]

I'll wait to review this until you've merged #60 and resolved conflicts, since I expect there would be some conflicts?

[jslee02]

Yes, there would be conflicts. I'll resolve the conflicts once #60 is merged.

[jslee02]

#60 is merged and conflicts are resolved as well. So ready to get reviewed.

[scpeters]

There are too many changes in the test for me to review them all, but the other code changes make sense. 👍

[jslee02]

The changes in tests are similar to the changes in #60. Applied the generalized tests to other primitive collision shapes.

[adnanmunawar]

I have a bit of confusion regarding the normals that are computed at collision. So for two objects, 1 and 2, is the normal the surface normal of object 1 at the collision point? Or, is it the surface normal of object 2 at collision point? What makes it confusing is when you say that the contact normal points from object 1 to object 2. At the computed collision points, the two objects may have different direction of surface normals and not just opposite to each other.

[jslee02]

You're right. In many cases, the normals at the contact point of the two colliding objects (assuming the contact point is on the surface) can be different. Moreover, the contact point is possibly not on the surfaces when there is non-zero penetration depth. In this case, the normal is not easy to define. For all these cases, most collision detection libraries (at least fcl and bullet from my experience) returns one compromised normal. The definition of compromised normal is totally dependent on the collision detection libraries. It also can be different depending on the shape types of the objects in the same library.

One thing always should be consistent is the pointing direction of normal. In other words, the normal should be always one of normal on object 1 and normal on object 2. This PR fixed the inconsistency.