Added near matrix for quick neighboor computation and updated A* and MotionGraph to use it