Initial Commit
Remove commented version to create new branch
BUG: Get Cost to be symmetric from a voxel to another
BUG: Remove unuseful declaration of functions