251 lines
9.5 KiB
Plaintext
251 lines
9.5 KiB
Plaintext
--{-# 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
|