Tweak pathfinding

src/Dodge/Path.hs

@@ -1,8 +1,9 @@
+{-# LANGUAGE TupleSections #-}
 module Dodge.Path 
     ( pointTowardsImpulse
     , makePathBetween
     , makePathBetweenPs
---    , removePathsCrossing
+    , removePathsCrossing
     , pairsToGraph
     , pairsToGraph''
     , walkableNodeNear
@@ -101,14 +102,15 @@ pairsToGraph'' :: Set.Set (Point2,Point2) -> PathGraph
 pairsToGraph'' = uncurry PathGraph . pairsToGraph' dist
 
 pairsToGraph' :: Ord a => (a -> a -> b) -> Set.Set (a,a) -> (Gr a b, NodeMap a)
-pairsToGraph' f = runIdentity . S.fold_ g (Data.Graph.Inductive.empty,new) id . S.each
+pairsToGraph' f pairset = ngr'
   where
-    g grnm (x,y) = h' $ h x $ h y grnm
-      where
-        h' (gr,nm) = (insMapEdge nm (x,y,f x y) gr , nm)
-        h n (gr,nm) = (gr',nm')
-          where
-            (gr',nm',_) = insMapNode nm n gr
+    nodeset = S.each . runIdentity $ S.fold_ getnode Set.empty id $ S.each pairset
+    getnode nodeset' (x,y) = Set.insert x $ Set.insert y nodeset'
+    ngr = runIdentity $ S.fold_ insertnode (Data.Graph.Inductive.empty,new) id nodeset
+    ngr' = runIdentity $ S.fold_ insertedge ngr id $ S.each pairset
+    insertedge (gr,nm) (x,y) = (insMapEdge nm (x,y,f x y) gr, nm)
+    insertnode (gr,nm) n = fstsnd $ insMapNode nm n gr
+    fstsnd (a,b,_) = (a,b)
 
 pairsToGraph :: (Ord a, Ord b) => (a -> a -> b) -> Set.Set (a,a) -> Gr a b
 pairsToGraph f pairs = undir
@@ -118,13 +120,30 @@ pairsToGraph f pairs = undir
     nodes' = Set.map fst pairs `Set.union` Set.map snd pairs
     pairs' = Set.map (\(x,y)->(x,y,f x y)) pairs
 
---removePathsCrossing :: Point2 -> Point2 -> World -> World
---removePathsCrossing a b w = w
+removePathsCrossing :: Point2 -> Point2 -> World -> World
+removePathsCrossing a b w = w
+--    & pathGraph . pgGraph %~ deleteEdges
+--    & pathGraphP %~ deletePairs
 --    & pathGraph  .~ newGraph 
 --    & pathGraphP .~ pg'
 --    & phZoning   %~ \zn -> foldl' (flip $ updateZoning (:)) (zn & znObjects .~ mempty)
 --        (labNodes newGraph)
---  where 
+  where
+    thegr = _pgGraph (_pathGraph w)
+    ns = nearSeg _phZoning a b w
+    pairs = S.filter testEdge $ S.for ns (nodePairs thegr)
+    testEdge ((_,p),(_,q)) = isJust $ intersectSegSeg a b p q
+    deleteEdges gr = runIdentity $ S.fold_ deleteEdge gr id pairs
+    deleteEdge gr ((x,_),(y,_)) = delEdge (x,y) $ delEdge (y,x) gr
+--    deletePairs s = runIdentity $ S.fold_ deletePair s id pairs
+--    deletePair s (x,y) = Set.delete (x,y) $ Set.delete (y,x) s
+
+nodePairs :: Gr a b -> (Int,a) -> StreamOf ((Int,a),(Int,a))
+nodePairs gr (n,p) = S.mapMaybe (fmap ((n,p),) . nodeLabel gr) . S.each $ neighbors gr n
+
+nodeLabel :: Gr a b -> Int -> Maybe (Int, a)
+nodeLabel gr n = (n,) <$> lab gr n
+
 --    pg' = Set.filter (isNothing . uncurry (intersectSegSeg a b)) $ _pathGraphP w
 --   -- insertPoint pp@(_,p) = insertInZoneWith (wlZoneOfPoint p) (++) [pp]
 --    newGraph = pairsToGraph'' dist pg'