Add various capabilities for generating graphs, fixes to lasgun

2022-07-06 11:54:29 +01:00
parent cd3463d3d8
commit d63495e335
29 changed files with 1801 additions and 178 deletions
@@ -0,0 +1,250 @@
+--{-# LANGUAGE TupleSections #-}
+module Dodge.Path 
+    ( pointTowardsImpulse
+    , makePathBetween
+    , makePathBetweenPs
+<<<<<<< HEAD
+    , pairsToGraph
+    , walkableNodeNear
+    , nodesNearL
+    , getNodePos
+    , addObstacleCrossing'
+=======
+--    , removePathsCrossing
+    , obstructPathsCrossing
+    , pairsToGraph
+    , getNodePos
+    , walkableNodeNear
+>>>>>>> efficientRuntime
+    ) where
+import Dodge.Data
+import Dodge.Base.Collide
+import Dodge.Zone
+import Geometry.Data
+import Geometry
+
+import Control.Lens
+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
+<<<<<<< HEAD
+import Data.Map.Strict (Map)
+import qualified Data.Map.Strict as M
+=======
+import qualified Data.Map.Strict as M
+import Data.Map.Strict (Map)
+import StreamingHelp
+>>>>>>> efficientRuntime
+import qualified Streaming.Prelude as S
+import StreamingHelp
+--import Data.Graph.Inductive.PatriciaTree
+--import Data.Graph.Inductive.Graph hiding ((&))
+
+getNodePos :: Int -> World -> Maybe Point2
+<<<<<<< HEAD
+getNodePos i w = _pgGraph (_pathGraph w) `lab` i
+
+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 (second pathDist $ efilter noobstacle $ _pgGraph $ _pathGraph w)
+  where
+    noobstacle (_,_,PathEdge _ _ s) = null s
+
+pathDist :: PathEdge -> Float
+pathDist pe = dist (_peStart pe) (_peEnd pe)
+
+walkableNodeNear :: Point2 -> World -> Maybe (Int,Point2)
+{-# INLINE walkableNodeNear #-}
+walkableNodeNear p w = minStreamOn (dist p . snd) 
+    . S.filter (flip (isWalkable p) w . snd) $ nodesNear p w
+
+nodesNear :: Point2 -> World -> StreamOf (Int,Point2)
+{-# INLINE nodesNear #-}
+nodesNear = aroundPoint _pnZoning
+
+nodesNearL :: Point2 -> World -> [(Int,Point2)]
+nodesNearL p = runIdentity . S.toList_ . nodesNear p
+=======
+getNodePos i w = _pathGraph w `lab` i
+
+makePathBetween :: Point2 -> Point2 -> World -> Maybe [Int]
+makePathBetween a b w = do -- join $ sp <$> a' <*> b' <*> return (_pathGraph w)
+    na <- walkableNodeNear w a
+    nb <- walkableNodeNear w b
+    sp na nb (second _peDist (_pathGraph w))
+  where
+    --nodesNear p = concat $ lookLookups (zoneNearPointIP p) (_pathPoints w)
+--    nodesNear p = runIdentity . S.toList_ $ nearPoint _pnZoning p w
+--    walkableNodeNear p = fmap fst . find (flip (isWalkable p) w . snd) $ nodesNear p
+
+walkableNodeNear :: World -> Point2 -> Maybe Int
+{-# INLINE walkableNodeNear #-}
+walkableNodeNear w p = fmap fst . find (flip (isWalkable p) w . snd) $ nodesNear
+  where
+    --nodesNear = runIdentity . S.toList_ $ nearPoint _pnZoning p w
+    nodesNear = runIdentity . S.toList_ $ aroundPoint _pnZoning p w
+>>>>>>> efficientRuntime
+
+makePathBetweenPs :: Point2 -> Point2 -> World -> Maybe [Point2]
+makePathBetweenPs a b w = mapMaybe (lab $ _pgGraph $ _pathGraph w) <$> makePathBetween a b w
+
+pointTowardsImpulse :: Point2 -> Point2 -> World -> Maybe Point2
+pointTowardsImpulse a b w = find (flip (isWalkable a) w) =<< makePathBetweenPs a b w
+
+------ continues a walk from a list of points, without repetitions
+------ supposes that the list is non-empty
+--randomGraphWalk :: RandomGen g => [Int] -> Gr a b -> State g [Int]
+--randomGraphWalk (n:ns) g = do 
+--    next' <- randomGraphStepRestricted n ns g
+--    case next' of 
+--        Nothing -> return (n:ns)
+--        Just n' -> randomGraphWalk (n':n:ns) g
+--randomGraphWalk _ _ = error "Trying to walk in an empty list"
+--
+--randomPointXStepsFrom :: Int -> Point2 -> World -> Point2
+--randomPointXStepsFrom i p w = 
+--    let g  = _pathGraph w
+--        ns = labNodes g
+--        mp = listToMaybe $ sortBy (compare `on` dist p . snd) $ filter (flip (isWalkable p) w . snd) ns
+--    in case mp of 
+--        Nothing -> p
+--        Just (n,_) -> fromJust 
+--            $ lab g (last $ take i $ randomGraphWalk [n] g Data.Function.& evalState  $ _randGen w)
+--
+--randomPointsXStepsFrom :: Int -> Point2 -> World -> [Point2]
+--randomPointsXStepsFrom i p w = 
+--    let g  = _pathGraph w
+--        ns = labNodes g
+--        mp = listToMaybe $ sortBy (compare `on` dist p . snd) $ filter (flip (isWalkable p) w . snd) ns
+--    in case mp of 
+--        Nothing -> [p]
+--        Just (n,_) -> mapMaybe (lab g) (take i $ randomGraphWalk [n] g Data.Function.& evalState $ _randGen w)
+--
+--randomGraphStep :: RandomGen g => Int -> Gr a b -> State g (Maybe Int)
+--randomGraphStep n g = 
+--    do let ns = neighbors g n
+--       i  <- state $ randomR (0,length ns - 1)
+--       case ns of [] -> return Nothing
+--                  _  -> return $ Just $ ns !! i
+--randomGraphStepRestricted :: RandomGen g => Int -> [Int] -> Gr a b -> State g (Maybe Int)
+--randomGraphStepRestricted n notns g = do 
+--    let ns = neighbors g n \\ notns
+--    i  <- state $ randomR (0,length ns - 1)
+--    case ns of 
+--        [] -> return Nothing
+--        _  -> return $ Just $ ns !! i
+--
+<<<<<<< HEAD
+
+pairsToGraph :: Set.Set (Point2,Point2) -> PathGraph
+pairsToGraph pairset = PathGraph gr' nodemap ncount edgemap
+  where
+    (gr,nodemap,ncount) = insertNodes pairset
+    (gr',edgemap) = insertEdges toPathEdge gr nodemap pairset
+
+toPathEdge :: Point2 -> Point2 -> PathEdge
+toPathEdge sp' ep = PathEdge sp' ep mempty
+
+insertEdges :: (Point2 -> Point2 -> b) 
+    -> Gr Point2 b -> Map Point2 Int 
+    -> Set (Point2,Point2) 
+    -> (Gr Point2 b, Map (V2 Point2) (Int,Int,b))
+insertEdges efunc gr nm = runIdentity . S.fold_ (insertEdge efunc nm) (gr,mempty) id . S.each
+
+insertEdge :: Ord a => (a -> a -> c) -> Map a Int -> (Gr b c,Map (V2 a) (Int,Int,c))
+    -> (a,a)
+    -> (Gr b c,Map (V2 a) (Int,Int,c))
+insertEdge efunc nm (gr',em) (a,b) = (insEdge (f a,f b,efunc a b) gr'
+    , M.insert (V2 a b) (f a,f b,efunc a b) em)
+  where 
+    f x = nm M.! x
+
+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, Ord b) => (a -> a -> b) -> Set.Set (a,a) -> Gr a b
+--pairsToGraph f pairs = undir
+--    $ run_ Data.Graph.Inductive.empty 
+--    $ insMapNodesM (Set.toList nodes') >> insMapEdgesM (Set.toList pairs')
+--  where
+--    nodes' = Set.map fst pairs `Set.union` Set.map snd pairs
+--    pairs' = Set.map (\(x,y)->(x,y,f x y)) pairs
+
+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
+=======
+pairsToGraph :: Set.Set (Point2,Point2) -> (Map (V2 Point2) (Int,Int,PathEdge),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) 
+    -> (Map (V2 Point2) (Int,Int,PathEdge) , Gr Point2 PathEdge)
+addEdges nodemap gr = runIdentity . S.fold_ f (mempty,gr) id
+  where
+    f (edgemap,gr') (a,b) = (M.insert (V2 a b) theedge edgemap
+        , insEdge theedge gr'
+        )
+      where
+        theedge = (g a,g b,PathEdge a b (dist a b) mempty)
+    g a = nodemap M.! a
+
+obstructPathsCrossing :: Point2 -> Point2 -> World -> ( World, [(Int,Int,PathEdge)])
+obstructPathsCrossing sp ep w = 
+    ( w & pathGraph %~ updateedges
+    , runIdentity $ S.toList_ edges
+    )
+  where
+    edges = S.filter edgecrosses $ nearSeg _peZoning sp ep w
+    edgecrosses (_,_,pe) = isJust $ intersectSegSeg sp ep (_peStart pe) (_peEnd pe)
+    updateedges gr = runIdentity $ S.fold_ updateedge gr id edges
+    updateedge gr (x,y,pe) 
+        = insEdge (x,y,pe & peObstacles . at BlockObstacle .~ Just ()) $ delEdge (x,y) gr
+
+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 pg'
+>>>>>>> efficientRuntime