Unitest that checks Octree Contact location

Added a unit test that verifies the rigidLocalPosition is valid when
doing Octree collision detection. This highlights the bug fixed in
58d180df3437c3dcf042dbcb8e0073bce47acc4e.
Unit test for contact contraint generation and local poses

This unit test checks to make sure that the position of the location
produced by collision detection matches the position in the localization
produced by contact contraint generation. This highlights a bug fixed in
58d180df3437c3dcf042dbcb8e0073bce47acc4e and
191a21675211dc8cae14166bb10741b541e20650.
Updated comments in ContactConstraintGeneration

Updated the doxygen comments in ContactConstraintGeneration and added
some debug info to the ContactConstraintGenerationTests.