Detect collision with a body item
Added cost to move
Full 3D planning