Templatize FCL for scalar type

[jslee02]

This PR templatize FCL objects on the scalar type so that we can take advantage of Eigen's automatic differentiation module as suggested by @sherm1 . A simple unit test that computes the gradient of distance w.r.t. the object position using Eigen's AutoDiff Module is included.

To make FCL working fine with Eigen::AutoDiffScalar, I had to templatize most of the FCL classes and global functions. Now there are only 7 files not templatized over ~180 files, which are not used anyways. I would suggest making FCL header only project as future change.

In this work, the major challenge was the resolving circular dependencies as all the implementations of templated classes should be placed in the same file with the declarations. To resolve the problem, I split those header files in circular dependencies into several files. Also, some files are split for easier maintenance (e.g., geometric_shapes.h is split per each shape class).

One caveat is the increased compilation time, which is the nature of header only project.

[sherm1]

Win64 VS 2015 RelWithDebInfo

Re-run with your latest changes (had to fix a typo in spatial_hash.h). Results from above repeated; new ones marked with **.

                                             FCL 0.5/master/templatized/**optimized**
Test  #1: test_fcl_collision ........ Passed    4.34/4.78/5.72/**4.93**
Test  #2: test_fcl_distance ......... Passed    3.78/4.33/4.73/**3.84**
Test  #4: test_fcl_broadphase ....... Passed  112.92/143.10/159.19/**135.06** (breakdown below)
Test  #6: test_fcl_frontlist ........ Passed    2.94/3.43/4.17/**3.77**

Test  #2: test_fcl_broadphase_collision_1 ...   Passed   63.88/**51.90**
Test  #3: test_fcl_broadphase_collision_2 ...   Passed   55.22/**57.87**
Test  #4: test_fcl_broadphase_distance ......    Passed   40.09/**25.29**

Not back to 0.5 levels, but generally much better.

[jslee02]

Thanks for the comparisons! For fair comparisons, FCL 0.5 build should be updated with this, but I don't it would make a big difference, though. Hm, I don't know why this PR is slower than 0.5 on Windows.. I could to some investigation on my virtual Windows machine, but I would like to save it for future work.

The typo is fixed in #156.

[sherm1]

See issue #160 for a subsequent change we could make to lighten the compilation load created by templatizing.

[jslee02]

The idea sounds good to me. I would like to save it for a future pull request, though.

Also, I believe this PR is ready to merge.

[sherm1]

The idea sounds good to me. I would like to save it for a future pull request, though.

Definitely! Could be done any time later.

Also, I believe this PR is ready to merge.

I agree.