Continue to work on pathfinding
This commit is contained in:
+103
-87
@@ -5,19 +5,18 @@ module Dodge.Path (
|
||||
makePathBetweenPs,
|
||||
-- , removePathsCrossing
|
||||
obstructPathsCrossing,
|
||||
pairsToGraph,
|
||||
-- pairsToGraph,
|
||||
getNodePos,
|
||||
walkableNodeNear,
|
||||
bfsNodePoints,
|
||||
-- bfsNodePoints,
|
||||
snapToGrid,
|
||||
pairsToIncGraph,
|
||||
updateEdge,
|
||||
getEdgesCrossing,
|
||||
) where
|
||||
|
||||
import Control.Lens
|
||||
import qualified Algorithm.Search as AS
|
||||
import Dodge.Zoning.Common
|
||||
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 ((&))
|
||||
@@ -27,22 +26,36 @@ import qualified Data.Map.Strict as M
|
||||
import Data.Maybe
|
||||
import Data.Set (Set)
|
||||
import qualified Data.Set as Set
|
||||
import qualified Data.Vector as V
|
||||
import qualified Data.Vector.Unboxed as UV
|
||||
import Dodge.Base.Collide
|
||||
import Dodge.Data.World
|
||||
import Dodge.Zoning.Base
|
||||
import Dodge.Zoning.Common
|
||||
import Dodge.Zoning.Pathing
|
||||
import Geometry
|
||||
import Linear
|
||||
import qualified IntMapHelp as IM
|
||||
import Linear
|
||||
|
||||
getNodePos :: Int -> World -> Maybe Point2
|
||||
getNodePos i w = (w ^. cWorld . pathGraph) `lab` i
|
||||
getNodePos i w = w ^? cWorld . incNode . ix 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
|
||||
getEdgesCrossing :: Point2 -> Point2 -> World -> [(Int,Int)]
|
||||
getEdgesCrossing s e w = filter inedgecrosses $ nearSeg peZoneSize _incEdgeZoning s e w
|
||||
where
|
||||
inedgecrosses (i, j) = isJust $ intersectSegSeg s e (f i) (f j)
|
||||
f i = w ^?! cWorld . incNode . ix i
|
||||
|
||||
updateEdge ::
|
||||
(Set.Set EdgeObstacle -> Set.Set EdgeObstacle) ->
|
||||
(Int, Int) ->
|
||||
V.Vector [(Int, SimpleEdge)] ->
|
||||
V.Vector [(Int, SimpleEdge)]
|
||||
updateEdge f (i, j) = ix i . each %~ g
|
||||
where
|
||||
g (k, o)
|
||||
| j == k = (k, o & seObstacles %~ f)
|
||||
| otherwise = (k, o)
|
||||
|
||||
makePathUsing :: (Set.Set EdgeObstacle -> Bool) -> Point2 -> Point2 -> World -> Maybe [Int]
|
||||
makePathUsing t s e w = do
|
||||
@@ -50,24 +63,24 @@ makePathUsing t s e w = do
|
||||
nb <- walkableNodeNear w e
|
||||
let f i = getes i
|
||||
h i = distance (getn nb) (getn i)
|
||||
(na :) . snd <$> AS.aStarAssoc f h (==nb) na
|
||||
(na :) . snd <$> AS.aStarAssoc f h (== nb) na
|
||||
where
|
||||
g (i,SimpleEdge c o)
|
||||
| t o = Just (i,c)
|
||||
g (i, SimpleEdge c o)
|
||||
| t o = Just (i, c)
|
||||
| otherwise = Nothing
|
||||
getes i = mapMaybe g $ w ^?! cWorld . incGraph . ix i
|
||||
getn i = w ^?! cWorld . incNode . ix i
|
||||
|
||||
|
||||
-- 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]
|
||||
--pathEdgeObstructed' :: PathEdge -> Bool
|
||||
--pathEdgeObstructed' pe =
|
||||
-- any
|
||||
-- (`Set.member` _peObstacles pe)
|
||||
-- [DoorObstacle, BlockObstacle, ChasmObstacle]
|
||||
|
||||
pathEdgeObstructed :: Set.Set EdgeObstacle -> Bool
|
||||
pathEdgeObstructed pe =
|
||||
@@ -81,11 +94,11 @@ walkableNodeNear w p = fmap fst . find (flip (isWalkable p) w . snd) $ nodesNear
|
||||
where
|
||||
nodesNear = zonesExtract (w ^. incNodeZoning) . snailAround $ zoneOfPoint pnZoneSize p
|
||||
|
||||
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
|
||||
--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
|
||||
@@ -97,17 +110,18 @@ smallSnailInt2 =
|
||||
[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
|
||||
--makePathBetweenPs a b w = mapMaybe (lab $ w ^. cWorld . pathGraph) <$> makePathBetween a b w
|
||||
makePathBetweenPs a b w = mapMaybe (flip getNodePos w) <$> makePathBetween a b w
|
||||
|
||||
bfsNodePoints :: Int -> World -> [Point2]
|
||||
bfsNodePoints n w = mapMaybe (lab g) $ bfs n g
|
||||
where
|
||||
g = w ^. cWorld . pathGraph
|
||||
--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]))
|
||||
(find (flip (isWalkable a) w) . reverse)
|
||||
=<< makePathBetweenPs a b w -- <&> (<> [b]))
|
||||
|
||||
@@ -155,78 +169,80 @@ pointTowardsImpulse a b w =
|
||||
-- _ -> return $ Just $ ns !! i
|
||||
--
|
||||
|
||||
pairsToIncGraph :: Set.Set (Point2,Point2)
|
||||
-> (UV.Vector Point2
|
||||
, V.Vector [(Int,SimpleEdge)]
|
||||
, [(Int,Int)]
|
||||
pairsToIncGraph ::
|
||||
Set.Set (Point2, Point2) ->
|
||||
( UV.Vector Point2
|
||||
, V.Vector [(Int, SimpleEdge)]
|
||||
, [(Int, Int)]
|
||||
)
|
||||
pairsToIncGraph pairs =
|
||||
( inodes
|
||||
, incgraph
|
||||
, Set.toList pairs & each . each %~ (\i -> pstons ^?! ix i . _head)
|
||||
)
|
||||
pairsToIncGraph pairs = (inodes,incgraph
|
||||
,Set.toList pairs & each . each %~ (\i -> pstons ^?! ix i . _head))
|
||||
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
|
||||
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
|
||||
--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)
|
||||
--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
|
||||
--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
|
||||
& cWorld . incGraph %~ updateincedges
|
||||
, es
|
||||
(World, [(Int,Int)])
|
||||
obstructPathsCrossing obstacletype s e w =
|
||||
( w & cWorld . incGraph %~ updateincedges
|
||||
, inces
|
||||
)
|
||||
where
|
||||
updateincedge :: V.Vector [(Int,SimpleEdge)] -> (Int,Int) -> V.Vector [(Int,SimpleEdge)]
|
||||
updateincedge gr (i,j) = gr & ix i . each %~ g j
|
||||
g j (k,o) | j == k = (k,o & seObstacles . at obstacletype ?~ ())
|
||||
| otherwise = (k,o)
|
||||
updateincedge = flip $ updateEdge (at obstacletype ?~ ())
|
||||
updateincedges gr = foldl' updateincedge gr inces
|
||||
inces = filter inedgecrosses $ nearSeg peZoneSize _incEdgeZoning sp' ep w
|
||||
inedgecrosses (i,j) = isJust $ intersectSegSeg sp' ep (f i) (f j)
|
||||
inces = filter inedgecrosses $ nearSeg peZoneSize _incEdgeZoning s e w
|
||||
inedgecrosses (i, j) = isJust $ intersectSegSeg s e (f i) (f j)
|
||||
f i = w ^?! cWorld . incNode . ix i
|
||||
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
|
||||
--es = Set.filter edgecrosses $ pesNearSeg sp ep w
|
||||
-- edgecrosses (PathEdgeNodes _ _ pe) =
|
||||
-- isJust $ intersectSegSeg s e (_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)))
|
||||
|
||||
Reference in New Issue
Block a user