Implemented A* for navigation in navmesh