add FCL and find a octree implementation octomap
http://octomap.github.com