Add pathing infrastructure

src/Dodge/Path.hs

@@ -9,6 +9,7 @@ module Dodge.Path
     , walkableNodeNear
     , nodesNearL
     , getNodePos
+    , addObstacleCrossing'
     ) where
 import Dodge.Data
 import Dodge.Base.Collide
@@ -21,7 +22,10 @@ import Data.Maybe
 import Data.List
 --import qualified Data.IntMap.Strict as IM
 import Data.Graph.Inductive hiding ((&))
+import Data.Set (Set)
 import qualified Data.Set as Set
+import Data.Map (Map)
+import qualified Data.Map.Strict as M
 import qualified Streaming.Prelude as S
 import StreamingHelp
 --import Data.Graph.Inductive.PatriciaTree
@@ -34,7 +38,10 @@ makePathBetween :: Point2 -> Point2 -> World -> Maybe [Int]
 makePathBetween a b w = do -- join $ sp <$> a' <*> b' <*> return (_pathGraph w)
     (na,_) <- walkableNodeNear a w
     (nb,_) <- walkableNodeNear b w
-    sp na nb (_pgGraph $ _pathGraph w)
+    sp na nb (second pathDist $ _pgGraph $ _pathGraph w)
+
+pathDist :: PathEdge -> Float
+pathDist pe = dist (_peStart pe) (_peEnd pe)
 
 walkableNodeNear :: Point2 -> World -> Maybe (Int,Point2)
 {-# INLINE walkableNodeNear #-}
@@ -43,7 +50,7 @@ walkableNodeNear p w = minStreamOn (dist p . snd)
 
 nodesNear :: Point2 -> World -> StreamOf (Int,Point2)
 {-# INLINE nodesNear #-}
-nodesNear = aroundPoint _phZoning
+nodesNear = aroundPoint _pnZoning
 
 nodesNearL :: Point2 -> World -> [(Int,Point2)]
 nodesNearL p = runIdentity . S.toList_ . nodesNear p
@@ -99,18 +106,45 @@ pointTowardsImpulse a b w = find (flip (isWalkable a) w) =<< makePathBetweenPs a
 --
 
 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 pairset = ngr'
+pairsToGraph'' pairset = PathGraph gr' nodemap ncount edgemap
   where
-    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)
+    (gr,nodemap,ncount) = insertNodes pairset
+    (gr',edgemap) = insertEdges toPathEdge pairset gr nodemap
+    
+
+toPathEdge :: Point2 -> Point2 -> PathEdge
+toPathEdge sp' ep = PathEdge sp' ep mempty
+
+insertEdges :: (Point2 -> Point2 -> b) 
+    -> Set (Point2,Point2) -> Gr Point2 b -> Map Point2 Int -> (Gr Point2 b,Map (V2 Point2) Int2)
+insertEdges efunc pairset gr nm = runIdentity $ S.fold_ insertedge (gr,mempty) id $ (S.each pairset)
+  where
+    insertedge (gr',em) (a,b) = (insEdge (f a,f b,efunc a b) gr'
+        , M.insert (V2 a b) (V2 (f a) (f b)) em)
+    f a = nm M.! a
+
+insertNodes :: Set (Point2,Point2) -> (Gr Point2 b, Map Point2 Int, Int)
+insertNodes pairset = runIdentity $ S.fold_ insertnode (Data.Graph.Inductive.empty,mempty,0) id nodestream
+  where
+    nodestream :: StreamOf Point2
+    nodestream = S.each . runIdentity $ S.fold_ getnode Set.empty id $ S.each pairset
+    getnode strm (x,y) = Set.insert x $ Set.insert y strm
+    insertnode :: (Gr Point2 b,Map Point2 Int,Int) -> Point2 -> (Gr Point2 b,Map Point2 Int,Int)
+    insertnode (gr,nm,i) n = case M.lookup n nm of
+        Nothing -> (insNode (j,n) gr, M.insert n j nm, j)
+        Just _  -> (gr,nm,i)
+      where
+        j = i + 1
+--pairsToGraph' :: (Ord a) => (a -> a -> b) -> Set (a,a) -> (Gr a b, Map a Int, Map (V2 a) Int2)
+--pairsToGraph' f pairset = ngr'
+--  where
+--    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
@@ -120,6 +154,26 @@ 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
 
+--findEdgesCrossing :: Point2 -> Point2 -> Gr Point2 PathEdge -> 
+--
+addObstacleCrossing :: World -> EdgeObstacle -> Point2 -> Point2 
+    -> Gr Point2 PathEdge
+    -> Gr Point2 PathEdge
+addObstacleCrossing w eo a b gr = runIdentity $ S.fold_ updateedge gr id paths
+  where
+    updateedge gr' (xi,yi,PathEdge x y obs) = insEdge (xi,yi,PathEdge x y (Set.insert eo obs))
+        $ delEdge (xi,yi) gr'
+    paths = S.filter f $ nearSeg _peZoning a b w
+    f (_,_,PathEdge x y _) = isJust $ intersectSegSeg x y a b
+
+addObstacleCrossing' :: EdgeObstacle -> Point2 -> Point2 
+    -> World
+    -> World
+addObstacleCrossing' eo a b w = w & pathGraph . pgGraph %~ addObstacleCrossing w eo a b
+  where
+    paths = S.filter f $ nearSeg _peZoning a b w
+    f (_,_,PathEdge x y _) = isJust $ intersectSegSeg x y a b
+
 removePathsCrossing :: Point2 -> Point2 -> World -> World
 removePathsCrossing a b w = w
 --    & pathGraph . pgGraph %~ deleteEdges
@@ -130,7 +184,7 @@ removePathsCrossing a b w = w
 --        (labNodes newGraph)
   where
     thegr = _pgGraph (_pathGraph w)
-    ns = nearSeg _phZoning a b w
+    ns = nearSeg _pnZoning 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