Replaced findPath with findPath2. TODO: Replace all vectors because they are too slow.