Added a new hit level function

Still need to find a way of factor kdtree traversal between getFirstCollision and getHitLevel
Updated getFirstCollision to use several policies
Merge branch 'feature/hitlevel' into develop