Add PathEdge data to pathfinding graph

src/Dodge/Path.hs

@@ -17,6 +17,9 @@ import Data.List
 --import qualified Data.IntMap.Strict as IM
 import Data.Graph.Inductive hiding ((&))
 import qualified Data.Set as Set
+import qualified Data.Map.Strict as M
+import Data.Map.Strict (Map)
+import StreamingHelp
 import qualified Streaming.Prelude as S
 --import Data.Graph.Inductive.PatriciaTree
 --import Data.Graph.Inductive.Graph hiding ((&))
@@ -25,7 +28,7 @@ makePathBetween :: Point2 -> Point2 -> World -> Maybe [Int]
 makePathBetween a b w = do -- join $ sp <$> a' <*> b' <*> return (_pathGraph w)
     na <- walkableNodeNear a
     nb <- walkableNodeNear b
-    sp na nb (_pathGraph w)
+    sp na nb (second _peDist (_pathGraph w))
   where
     --nodesNear p = concat $ lookLookups (zoneNearPointIP p) (_pathPoints w)
     nodesNear p = runIdentity . S.toList_ $ nearPoint _phZoning p w
@@ -83,19 +86,33 @@ pointTowardsImpulse a b w = find (flip (isWalkable a) w) =<< makePathBetweenPs a
 --        [] -> return Nothing
 --        _  -> return $ Just $ ns !! i
 --
-pairsToGraph :: (Ord a, Ord b) => (a -> a -> b) -> Set.Set (a,a) -> Gr a b
-pairsToGraph f pairs = 
-    let nodes' = Set.map fst pairs `Set.union` Set.map snd pairs
-        pairs' = Set.map (\(x,y)->(x,y,f x y)) pairs
-    in undir $ run_ Data.Graph.Inductive.empty $ insMapNodesM (Set.toList nodes') >> insMapEdgesM (Set.toList pairs')
+pairsToGraph :: Set.Set (Point2,Point2) -> Gr Point2 PathEdge
+pairsToGraph pairs = addEdges nodemap gr $ S.each pairs
+  where
+    (nodemap,_,gr) = addNodes $ S.map fst (S.each pairs) <> S.map snd (S.each pairs)
+--    let nodes' = Set.map fst pairs `Set.union` Set.map snd pairs
+--        pairs' = Set.map (\(x,y)->(x,y,f x y)) pairs
+--    in undir $ run_ Data.Graph.Inductive.empty $ insMapNodesM (Set.toList nodes') >> insMapEdgesM (Set.toList pairs')
+
+addNodes :: StreamOf Point2 -> (Map Point2 Int,Int,Gr Point2 PathEdge)
+addNodes = runIdentity . S.fold_ f (mempty,0,Data.Graph.Inductive.empty) id
+  where
+    f (nodemap,i,gr) p = case nodemap M.!? p of
+        Just _  -> (nodemap,i,gr)
+        Nothing -> (nodemap & at p ?~i ,i+1, insNode (i,p) gr)
+addEdges :: Map Point2 Int -> Gr Point2 PathEdge -> StreamOf (Point2,Point2) -> Gr Point2 PathEdge
+addEdges nodemap gr = runIdentity . S.fold_ f gr id
+  where
+    f gr' (a,b) = insEdge (g a,g b,PathEdge a b (dist a b) mempty) gr'
+    g a = nodemap M.! a
 
 removePathsCrossing :: Point2 -> Point2 -> World -> World
 removePathsCrossing a b w = w
-    & pathGraph  .~ newGraph 
-    & pathGraphP .~ pg'
-    & phZoning   %~ \zn -> foldl' (flip $ updateZoning (:)) (zn & znObjects .~ mempty)
-        (labNodes newGraph)
-  where 
-    pg' = Set.filter (isNothing . uncurry (intersectSegSeg a b)) $ _pathGraphP w
-   -- insertPoint pp@(_,p) = insertInZoneWith (wlZoneOfPoint p) (++) [pp]
-    newGraph = pairsToGraph dist pg'
+--    & pathGraph  .~ newGraph 
+--    & pathGraphP .~ pg'
+--    & phZoning   %~ \zn -> foldl' (flip $ updateZoning (:)) (zn & znObjects .~ mempty)
+--        (labNodes newGraph)
+--  where 
+--    pg' = Set.filter (isNothing . uncurry (intersectSegSeg a b)) $ _pathGraphP w
+--   -- insertPoint pp@(_,p) = insertInZoneWith (wlZoneOfPoint p) (++) [pp]
+--    newGraph = pairsToGraph pg'