Merge branch 'efficientRuntime'
This commit is contained in:
+46
-74
@@ -3,11 +3,10 @@ module Dodge.Path
|
||||
( pointTowardsImpulse
|
||||
, makePathBetween
|
||||
, makePathBetweenPs
|
||||
, obstructPathsCrossing
|
||||
, pairsToGraph
|
||||
, walkableNodeNear
|
||||
, nodesNearL
|
||||
, getNodePos
|
||||
, addObstacleCrossing'
|
||||
, walkableNodeNear
|
||||
) where
|
||||
import Dodge.Data
|
||||
import Dodge.Base.Collide
|
||||
@@ -22,38 +21,33 @@ import Data.List
|
||||
import Data.Graph.Inductive hiding ((&))
|
||||
import Data.Set (Set)
|
||||
import qualified Data.Set as Set
|
||||
import Data.Map.Strict (Map)
|
||||
import qualified Data.Map.Strict as M
|
||||
import Data.Map.Strict (Map)
|
||||
import StreamingHelp
|
||||
import qualified Streaming.Prelude as S
|
||||
import StreamingHelp
|
||||
--import Data.Graph.Inductive.PatriciaTree
|
||||
--import Data.Graph.Inductive.Graph hiding ((&))
|
||||
|
||||
getNodePos :: Int -> World -> Maybe Point2
|
||||
getNodePos i w = _pgGraph (_pathGraph w) `lab` i
|
||||
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 a w
|
||||
(nb,_) <- walkableNodeNear b w
|
||||
sp na nb (second pathDist $ efilter noobstacle $ _pgGraph $ _pathGraph w)
|
||||
na <- walkableNodeNear w a
|
||||
nb <- walkableNodeNear w b
|
||||
sp na nb (second _peDist (_pathGraph w))
|
||||
where
|
||||
noobstacle (_,_,PathEdge _ _ s) = null s
|
||||
--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
|
||||
|
||||
pathDist :: PathEdge -> Float
|
||||
pathDist pe = dist (_peStart pe) (_peEnd pe)
|
||||
|
||||
walkableNodeNear :: Point2 -> World -> Maybe (Int,Point2)
|
||||
walkableNodeNear :: World -> Point2 -> Maybe Int
|
||||
{-# 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
|
||||
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
|
||||
|
||||
makePathBetweenPs :: Point2 -> Point2 -> World -> Maybe [Point2]
|
||||
makePathBetweenPs a b w = mapMaybe (lab $ _pgGraph $ _pathGraph w) <$> makePathBetween a b w
|
||||
@@ -104,62 +98,40 @@ pointTowardsImpulse a b w = find (flip (isWalkable a) w) =<< makePathBetweenPs a
|
||||
-- [] -> return Nothing
|
||||
-- _ -> return $ Just $ ns !! i
|
||||
--
|
||||
|
||||
pairsToGraph :: Set.Set (Point2,Point2) -> PathGraph
|
||||
pairsToGraph pairset = PathGraph gr' nodemap ncount edgemap
|
||||
pairsToGraph :: Set.Set (Point2,Point2) -> (Map (V2 Point2) (Int,Int,PathEdge),Gr Point2 PathEdge)
|
||||
pairsToGraph pairs = addEdges nodemap gr $ S.each pairs
|
||||
where
|
||||
(gr,nodemap,ncount) = insertNodes pairset
|
||||
(gr',edgemap) = insertEdges toPathEdge gr nodemap pairset
|
||||
(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')
|
||||
|
||||
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
|
||||
addNodes :: StreamOf Point2 -> (Map Point2 Int,Int,Gr Point2 PathEdge)
|
||||
addNodes = runIdentity . S.fold_ f (mempty,0,Data.Graph.Inductive.empty) id
|
||||
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)
|
||||
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
|
||||
j = i + 1
|
||||
theedge = (g a,g b,PathEdge a b (dist a b) mempty)
|
||||
g a = nodemap M.! a
|
||||
|
||||
--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
|
||||
obstructPathsCrossing :: Point2 -> Point2 -> World -> ( World, [(Int,Int,PathEdge)])
|
||||
obstructPathsCrossing sp ep w =
|
||||
( w & pathGraph %~ updateedges
|
||||
, runIdentity $ S.toList_ edges
|
||||
)
|
||||
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
|
||||
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
|
||||
|
||||
addObstacleCrossing' :: EdgeObstacle -> Point2 -> Point2
|
||||
-> World
|
||||
-> World
|
||||
addObstacleCrossing' eo a b w = w & pathGraph . pgGraph %~ addObstacleCrossing w eo a b
|
||||
|
||||
Reference in New Issue
Block a user