196 lines
6.6 KiB
Haskell
196 lines
6.6 KiB
Haskell
--{-# LANGUAGE TupleSections #-}
|
|
module Dodge.Path (
|
|
pointTowardsImpulse,
|
|
makePathBetween,
|
|
makePathBetweenPs,
|
|
-- , removePathsCrossing
|
|
obstructPathsCrossing,
|
|
pairsToGraph,
|
|
getNodePos,
|
|
walkableNodeNear,
|
|
bfsNodePoints,
|
|
snapToGrid,
|
|
pairsToIncGraph,
|
|
) where
|
|
|
|
import qualified Data.Vector as V
|
|
import qualified Data.Vector.Unboxed as UV
|
|
import Control.Lens
|
|
import Data.Bifunctor
|
|
import Data.Foldable
|
|
import Data.Graph.Inductive hiding ((&))
|
|
import Data.List (sortOn)
|
|
import Data.Map.Strict (Map)
|
|
import qualified Data.Map.Strict as M
|
|
import Data.Maybe
|
|
import Data.Set (Set)
|
|
import qualified Data.Set as Set
|
|
import Dodge.Base.Collide
|
|
import Dodge.Data.World
|
|
import Dodge.Zoning.Base
|
|
import Dodge.Zoning.Pathing
|
|
import Geometry
|
|
import Linear
|
|
import qualified IntMapHelp as IM
|
|
|
|
getNodePos :: Int -> World -> Maybe Point2
|
|
getNodePos i w = (w ^. cWorld . pathGraph) `lab` i
|
|
|
|
makePathUsing :: (PathEdge -> Bool) -> Point2 -> Point2 -> World -> Maybe [Int]
|
|
makePathUsing t s e w = do
|
|
na <- walkableNodeNear w s
|
|
nb <- walkableNodeNear w e
|
|
sp na nb . second _peDist . efilter (^. _3 . to t) $ w ^. cWorld . pathGraph
|
|
|
|
makePathBetween :: Point2 -> Point2 -> World -> Maybe [Int]
|
|
makePathBetween = makePathUsing $ not . pathEdgeObstructed
|
|
|
|
pathEdgeObstructed :: PathEdge -> Bool
|
|
pathEdgeObstructed pe =
|
|
any
|
|
(`Set.member` _peObstacles pe)
|
|
[DoorObstacle, BlockObstacle, ChasmObstacle]
|
|
|
|
walkableNodeNear :: World -> Point2 -> Maybe Int
|
|
{-# INLINE walkableNodeNear #-}
|
|
walkableNodeNear w p = fmap fst . find (flip (isWalkable p) w . snd) $ nodesNear
|
|
where
|
|
nodesNear = zonesExtract (w ^. pnZoning) . snailAround $ zoneOfPoint pnZoneSize p
|
|
|
|
snailAround :: Int2 -> [Int2]
|
|
snailAround x = (x +) <$> smallSnailInt2
|
|
|
|
smallSnailInt2 :: [Int2]
|
|
smallSnailInt2 =
|
|
sortOn
|
|
(distance (V2 0 (0 :: Float)) . fmap fromIntegral)
|
|
[V2 x y | x <- [-2 .. 2], y <- [-2 .. 2]]
|
|
|
|
makePathBetweenPs :: Point2 -> Point2 -> World -> Maybe [Point2]
|
|
makePathBetweenPs a b w = mapMaybe (lab $ w ^. cWorld . pathGraph) <$> makePathBetween a b w
|
|
|
|
bfsNodePoints :: Int -> World -> [Point2]
|
|
bfsNodePoints n w = mapMaybe (lab g) $ bfs n g
|
|
where
|
|
g = w ^. cWorld . pathGraph
|
|
|
|
pointTowardsImpulse :: Point2 -> Point2 -> World -> Maybe Point2
|
|
pointTowardsImpulse a b w =
|
|
-- (find (flip (isWalkable a) w) . reverse)
|
|
-- =<< (makePathBetweenPs a b w <&> (<> [b]))
|
|
(find (flip (isWalkable a) w) . reverse)
|
|
=<< makePathBetweenPs a b w -- <&> (<> [b]))
|
|
|
|
------ 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
|
|
--
|
|
|
|
pairsToIncGraph :: Set.Set (Point2,Point2)
|
|
-> (UV.Vector Point2
|
|
, V.Vector [(Int,SimpleEdge)]
|
|
, [(Int,Int)]
|
|
)
|
|
pairsToIncGraph pairs = (inodes,incgraph,undefined)
|
|
where
|
|
incgraph = V.generate (length im) (\i -> im ^?! ix i)
|
|
im = IM.fromListWith (<>) . fmap toedge $ Set.toList pairs
|
|
toedge (x,y) = (pstons ^?! ix x . _head
|
|
, [(pstons ^?! ix y . _head, SimpleEdge (distance x y) mempty)])
|
|
pstons = IM.invertIntMap . IM.fromList $ zip [0..] ps
|
|
inodes = UV.generate (length ps) (ps !!)
|
|
ps = Set.toList $ Set.map fst pairs <> Set.map snd pairs
|
|
|
|
pairsToGraph ::
|
|
Set.Set (Point2, Point2) ->
|
|
(Map (V2 Point2) PathEdgeNodes, Gr Point2 PathEdge)
|
|
pairsToGraph pairs = addEdges nodemap gr pairs
|
|
where
|
|
(nodemap, _, gr) = addNodes $ Set.toList $ Set.map fst pairs <> Set.map snd pairs
|
|
|
|
addNodes :: [Point2] -> (Map Point2 Int, Int, Gr Point2 PathEdge)
|
|
addNodes = foldl' f (mempty, 0, Data.Graph.Inductive.empty)
|
|
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 ->
|
|
Set.Set (Point2, Point2) ->
|
|
(Map (V2 Point2) PathEdgeNodes, Gr Point2 PathEdge)
|
|
addEdges nodemap gr = foldl' f (mempty, gr)
|
|
where
|
|
f (edgemap, gr') (a, b) =
|
|
( M.insert (V2 a b) theedgedata edgemap
|
|
, insEdge theedgetup gr'
|
|
)
|
|
where
|
|
theedgetup = (g a, g b, PathEdge a b (dist a b) mempty)
|
|
theedgedata = PathEdgeNodes (g a) (g b) (PathEdge a b (dist a b) mempty)
|
|
g a = nodemap M.! a
|
|
|
|
obstructPathsCrossing ::
|
|
EdgeObstacle ->
|
|
Point2 ->
|
|
Point2 ->
|
|
World ->
|
|
(World, Set PathEdgeNodes)
|
|
obstructPathsCrossing obstacletype sp' ep w =
|
|
( w & cWorld . pathGraph %~ updateedges
|
|
, es
|
|
)
|
|
where
|
|
es = Set.filter edgecrosses $ pesNearSeg sp' ep w
|
|
edgecrosses (PathEdgeNodes _ _ pe) =
|
|
isJust $ intersectSegSeg sp' ep (_peStart pe) (_peEnd pe)
|
|
updateedges gr = foldl' updateedge gr es
|
|
updateedge gr (PathEdgeNodes x y pe) =
|
|
insEdge (x, y, pe & peObstacles . at obstacletype ?~ ()) $ delEdge (x, y) gr
|
|
|
|
snapToGrid :: Set (Point2, Point2) -> Set (Point2, Point2)
|
|
snapToGrid = Set.map (over each (fmap (fromIntegral . f)))
|
|
where
|
|
f :: Float -> Int
|
|
f = round
|