187 lines
6.6 KiB
Haskell
187 lines
6.6 KiB
Haskell
module Dodge.Path where
|
|
import Dodge.Data
|
|
import Dodge.Base
|
|
import Dodge.Graph
|
|
import Geometry
|
|
|
|
import Data.List
|
|
import Data.Maybe
|
|
import Data.Function
|
|
import Data.Graph.Inductive.PatriciaTree
|
|
import Data.Graph.Inductive.Query.SP
|
|
import Data.Graph.Inductive.Graph hiding ((&))
|
|
import qualified Data.HashSet as HS
|
|
import qualified Data.Heap as HP
|
|
import qualified Data.Map as M
|
|
import Control.Monad
|
|
import Control.Monad.State
|
|
import System.Random
|
|
|
|
worldGraph :: World -> Point2 -> HS.HashSet Point2
|
|
worldGraph w p = HS.unions $ fmap (\q -> HS.fromList $ pointsAlong w p (p +.+ q))
|
|
[(200,0),(-200,0),(0,200),(0,-200)]
|
|
|
|
pointsAlong :: World -> Point2 -> Point2 -> [Point2]
|
|
pointsAlong w p q = divideLineFixed 50 p p'
|
|
where
|
|
p' = furthestPointWalkable p q $ wallsAlongLine p q w
|
|
|
|
divideLineFixed :: Float -> Point2 -> Point2 -> [Point2]
|
|
divideLineFixed x a b = fmap
|
|
( \i -> a +.+ i * x *.* normalizeV (b -.- a) )
|
|
ns
|
|
where
|
|
numPoints = floor $ dist a b / x
|
|
ns = map fromIntegral [1 .. numPoints]
|
|
|
|
-- ok, astar or something like it
|
|
|
|
type SearchedNodes = (HP.MinHeap (Float,(Float,[Point2])), [Point2])
|
|
|
|
stripRight :: Either a b -> b
|
|
stripRight (Right x) = x
|
|
|
|
stepPath :: (Point2 -> [Point2]) -> Point2 -> SearchedNodes -> Either [Point2] SearchedNodes
|
|
stepPath f p (nextNodes, seenNodes) = case HP.view nextNodes of
|
|
Nothing -> Left []
|
|
Just ((_,(cost,q:qs)), nextNodes')
|
|
| q == p -> Left (q:qs)
|
|
| otherwise ->
|
|
let rs' = f q
|
|
rs = rs' \\ seenNodes
|
|
newNodes = map (\r -> (cost + dist q r + dist r p , (cost + dist q r , r:q:qs))) rs
|
|
in Right (foldr HP.insert nextNodes' newNodes , rs ++ seenNodes)
|
|
|
|
stepPath' :: (Point2 -> [Point2]) -> Point2 -> SearchedNodes -> [Point2]
|
|
stepPath' f p s = case stepPath f p s of
|
|
Left ps -> ps
|
|
Right s' -> stepPath' f p s'
|
|
|
|
makePath' :: (Point2 -> [Point2]) -> Point2 -> Point2 -> [Point2]
|
|
makePath' f s e = stepPath' f e (HP.singleton (0,(0,[s])) , [])
|
|
|
|
makeNode :: Point2 -> SearchedNodes
|
|
makeNode e = (HP.singleton (0,(0,[e])) , [])
|
|
|
|
|
|
tp1,tp2,tp3 :: Point2
|
|
tp1 = (0,1)
|
|
tp2 = (0,20)
|
|
tp3 = (30,40)
|
|
|
|
f = incidenceToFunction $ pairsToIncidence
|
|
[(tp1,tp2)
|
|
,(tp2,tp3)
|
|
,(tp2,tp1)
|
|
,(tp1,tp3)
|
|
]
|
|
g = pairsToIncidence
|
|
[(tp1,tp2)
|
|
,(tp2,tp3)
|
|
,(tp2,tp1)
|
|
,(tp1,tp3)
|
|
]
|
|
|
|
pathBetween :: Point2 -> Point2 -> World -> Maybe [Point2]
|
|
pathBetween a b w = (makePath' $ \p -> _pathInc w M.! p) <$> a' <*> b'
|
|
where
|
|
nsa :: [Point2]
|
|
nsa = map snd $ concat $ lookLookups (zoneAroundPoint a) (_pathPoints w)
|
|
nsb = map snd $ concat $ lookLookups (zoneAroundPoint b) (_pathPoints w)
|
|
a' = find (flip (isWalkable a) w) nsa
|
|
b' = find (flip (isWalkable b) w) nsb
|
|
|
|
makePathBetween :: Point2 -> Point2 -> World -> Maybe [Int]
|
|
makePathBetween a b w = join $ sp <$> fmap fst a' <*> fmap fst b' <*> return g
|
|
where
|
|
g = _pathGraph w
|
|
nsa = concat $ lookLookups (zoneAroundPoint a) (_pathPoints w)
|
|
nsb = concat $ lookLookups (zoneAroundPoint b) (_pathPoints w)
|
|
a' = find (flip (isWalkable a) w . snd) nsa
|
|
b' = find (flip (isWalkable b) w . snd) nsb
|
|
|
|
ezipWith :: Monoid a => (b -> c -> d) -> Either a b -> Either a c -> Either a d
|
|
ezipWith f (Right x) (Right y) = Right (f x y)
|
|
ezipWith f (Left x) (Right _) = Left x
|
|
ezipWith f (Right _) (Left y) = Left y
|
|
ezipWith f (Left x) (Left y) = Left (mappend x y)
|
|
|
|
makePathBetween' :: Point2 -> Point2 -> World -> Either String [Int]
|
|
makePathBetween' a b w =
|
|
let g = _pathGraph w
|
|
ns = labNodes g
|
|
nsa = _pathPoints w `ixNZ` a
|
|
nsb = _pathPoints w `ixNZ` b
|
|
a' = case listToMaybe $ sortBy (compare `on` dist a . snd) $ filter (flip (isWalkable a) w . snd) ns of
|
|
Just p -> Right $ fst p
|
|
_ -> Left "FIRST POINT UNSEEN"
|
|
b' = case listToMaybe $ sortBy (compare `on` dist b . snd) $ filter (flip (isWalkable b) w . snd) ns of
|
|
Just p -> Right $ fst p
|
|
_ -> Left $ "SECOND POINT UNSEEN" ++ show b
|
|
in case ezipWith (\x y -> sp x y g) a' b' of
|
|
Right (Just xs) -> Right xs
|
|
Right Nothing -> Left $ "NO PATH" ++ show a ++ show b ++ show a' ++ show b'
|
|
Left m -> Left m
|
|
|
|
makePathBetweenPs :: Point2 -> Point2 -> World -> Maybe [Point2]
|
|
makePathBetweenPs a b w = mapMaybe (lab g) <$> makePathBetween b a w
|
|
where
|
|
g = _pathGraph w
|
|
|
|
makePathBetweenPs' :: Point2 -> Point2 -> World -> Either String [Point2]
|
|
makePathBetweenPs' a b w = mapMaybe (lab g) <$> makePathBetween' a b w
|
|
where
|
|
g = _pathGraph w
|
|
|
|
pointTowardsImpulse :: Point2 -> Point2 -> World -> Maybe Point2
|
|
pointTowardsImpulse a b w = find (flip (isWalkable a) w) =<< makePathBetweenPs a b w
|
|
|
|
pointTowardsImpulse' :: Point2 -> Point2 -> World -> Either String Point2
|
|
pointTowardsImpulse' a b w = (maybeToEither "NOSEEPATH" . find (flip (isWalkable a) w)) =<< makePathBetweenPs' b a w
|
|
|
|
maybeToEither :: a -> Maybe b -> Either a b
|
|
maybeToEither _ (Just x) = Right x
|
|
maybeToEither y Nothing = Left y
|
|
|
|
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
|
|
|
|
---- 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
|
|
|
|
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)
|