Modified VFHPlus algorithm:
1. Adding collision detecting method
2. Take the spanning angle of free sector into account while calculating
   the candidate angles, which seperates the sector to narrow or wide.