Add unit tests for Box-Plane and Box-Halfspace collisions

[Kotochleb]

As in #520 I recreated the unit test for the Box-Plane collision on this branch. I also redid the same tests for the Box-Halfspace and basically cleaned the up.

Currently, this PR is a draft since it some of the tests are failing. In both cases (Plane and Halfspace) the accuracy of position of the contact point in the new implementation got worse to the point where error is of magnitude greater than 1e-3. This only fails if Box is not orthogonal to the Plane/Halfspace. This is nicely visible in the tests for Box-Plane, as those are the only failing tests. As a side note, those tests are passing with the old implementation (at least for the Box-Plane)

In the case of the Box-Halfspace there are two issues. When the Box is intersecting with the Halfspace, the nearest point for the Box is the one farthest away from the surface of the Halfspace. In case the Box is fully "submerged" in the Halfspace the nearest point jumps to the center of the Box.

The second problem is that in the same test, when the box is standing at its edge, Box-Plane will return projection of the COG onto the plane, being the center of the edge, while Box-Halfspace will return the vertex of the Box which actually can be seen in screenshots in the previous #520.

[jcarpent]

@lmontaut Could you have a look and provide an answer to @Kotochleb?

[lmontaut]

Hi @Kotochleb,

In short:

• could you check out the commits I just did on test/box_halfspace.cpp and rewrite the test/box_plane.cpp test to fit it?
• Everything is fine for box-plane and box-halfspace collisions, there is a priori nothing to change in the code. Also, these two tests are a bit redundant since all of this is already checked in test/normal_and_nearest_points.cpp, but I guess some redundancy does not hurt.

In details:

• Your tests are failing because you do not provide the right expected contact points. Please check the definition of contact points (also called witness points and wrongly named nearest_points in hpp-fcl) in the Contact struct in include/hpp/fcl/collision_data.h. The pos field is simply pos = (p1 + p2)/2 where p1 = Contact::nearest_points[0] and p2 = Contact::nearest_points[1]. So you should never have pos = p1 or pos = p2 except when you have a perfect contact between the shapes. As soon as the penetration distance is negative, we have pos != p1 != p2.

Some other details:

• The constructor of a box hpp::fcl::Box expects a vector (w, h, d). Sometimes your tests are written as if the box is (w/2, h/2, d/2)
• Please use BOOST_CHECK_CLOSE and EIGEN_VECTOR_IS_APPROX. These allow to easily see what is the expected result.