Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- findPath :: (Ord a, Ord n, Eq a, Num n) => Pos a -> Pos a -> g -> Cost g a n -> Heuristic a n -> Neighbours g a -> Maybe (Path a)
- findPath start end grid cost heuristic neighbours = findPath' (MinQueue.singleton (Node 0 0 start)) (Map.singleton start (start, 0))
- where
- findPath' open closed = do
- minNode@(Node f g pos) <- MinQueue.getMin open
- let neighbourNodes = filterNeighbours pos f closed
- closed' = foldr (\p c -> Map.insert p (pos, f + cost pos p grid) c) closed neighbourNodes
- open' = foldr (\p c -> MinQueue.insert (Node (f + cost pos p grid) (heuristic pos p) p) c) (MinQueue.deleteMin open) neighbourNodes
- if pos == end then
- return $ flattenPath closed' [end]
- else
- findPath' open' closed' -- Keep going till we find a solution.
- filterNeighbours pos f closed = filter filterFunc $ neighbours pos grid
- where
- filterFunc nPos = case Map.lookup nPos closed of
- Nothing -> True
- Just (cameFrom, score) -> f + cost pos nPos grid < score
- flattenPath closed path@(p:ps) = case Map.lookup p closed of
- Nothing -> []
- Just (cameFrom, _) -> if cameFrom == start then
- cameFrom : path
- else
- flattenPath closed (cameFrom : path)
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement