diff --git a/src/Util.hs b/src/Util.hs index 2cf69c8..6d39bd2 100644 --- a/src/Util.hs +++ b/src/Util.hs @@ -133,11 +133,25 @@ astarAppl imgmat target start = aStar naviGraph :: Matrix (Maybe ImgId) -> V2 Int -> HS.HashSet (V2 Int) naviGraph imgmat (V2 r c) = - let list = foldl - (\acc (or, oc) -> if null (imgObstacle $ imgmat M.! (r + or, c + oc)) - then if or == 0 && oc == 0 then acc else V2 (r + or) (c + oc): acc - else acc - ) - [] - [(0, 1), (0, -1), (1, 0), (-1, 0)] + let list = + foldl + (\acc (or, oc) -> + if null (imgObstacle $ imgmat M.! (r + or, c + oc)) + then V2 (r + or) (c + oc): acc + else acc + ) + [] + [(0, 1), (0, -1), (1, 0), (-1, 0)] + ++ + foldl + (\acc (or, oc) -> + if all null + (map + (\(oor, ooc) -> imgObstacle $ imgmat M.! (r + oor, c + ooc)) + [(0, oc), (or, 0), (or, oc)]) + then V2 (r + or) (c + oc): acc + else acc + ) + [] + [(-1, -1), (-1, 1), (1, -1), (1, 1)] in HS.fromList list