Change distanceTo to require points diagonal or orthogonal
Implement isClearPath function