module Swarm.Game.Step.Pathfinding where
import Control.Carrier.State.Lazy
import Control.Effect.Lens
import Control.Monad (filterM, guard)
import Control.Monad.Trans.Class (lift)
import Control.Monad.Trans.Maybe (MaybeT (..), runMaybeT)
import Data.Graph.AStar (aStarM)
import Data.HashSet (HashSet)
import Data.HashSet qualified as HashSet
import Data.Int (Int32)
import Swarm.Game.Entity
import Swarm.Game.Location
import Swarm.Game.State
import Swarm.Game.Step.Util
import Swarm.Game.Universe
import Swarm.Language.Syntax
import Swarm.Util (hoistMaybe)
data PathfindingTarget
= LocationTarget Location
|
EntityTarget EntityName
pathCommand ::
(HasRobotStepState sig m, Has (State GameState) sig m) =>
Maybe Integer ->
Cosmic Location ->
PathfindingTarget ->
m (Maybe Direction)
pathCommand :: forall (sig :: (* -> *) -> * -> *) (m :: * -> *).
(HasRobotStepState sig m, Has (State GameState) sig m) =>
Maybe Integer
-> Cosmic Location -> PathfindingTarget -> m (Maybe Direction)
pathCommand Maybe Integer
maybeLimit (Cosmic SubworldName
currentSubworld Location
robotLoc) PathfindingTarget
target = do
Bool
isGoalLocWalkable <- case PathfindingTarget
target of
LocationTarget Location
loc -> forall (t :: * -> *) a. Foldable t => t a -> Bool
null forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> forall (sig :: (* -> *) -> * -> *) (m :: * -> *).
HasRobotStepState sig m =>
Cosmic Location -> m (Maybe MoveFailureDetails)
checkMoveFailure (forall a. SubworldName -> a -> Cosmic a
Cosmic SubworldName
currentSubworld Location
loc)
EntityTarget EntityName
_ -> forall (m :: * -> *) a. Monad m => a -> m a
return Bool
True
forall (m :: * -> *) a. MaybeT m a -> m (Maybe a)
runMaybeT forall a b. (a -> b) -> a -> b
$ do
forall (f :: * -> *). Alternative f => Bool -> f ()
guard Bool
isGoalLocWalkable
Maybe [Location]
maybeFoundPath <- forall (t :: (* -> *) -> * -> *) (m :: * -> *) a.
(MonadTrans t, Monad m) =>
m a -> t m a
lift m (Maybe [Location])
computePath
[Location]
foundPath <- forall (m :: * -> *) b. Applicative m => Maybe b -> MaybeT m b
hoistMaybe Maybe [Location]
maybeFoundPath
forall (m :: * -> *) a. Monad m => a -> m a
return forall a b. (a -> b) -> a -> b
$ [Location] -> Direction
nextDir [Location]
foundPath
where
computePath :: m (Maybe [Location])
computePath =
forall (m :: * -> *) a c.
(Monad m, Hashable a, Ord a, Ord c, Num c) =>
(a -> m (HashSet a))
-> (a -> a -> m c)
-> (a -> m c)
-> (a -> m Bool)
-> m a
-> m (Maybe [a])
aStarM
(forall (sig :: (* -> *) -> * -> *) (m :: * -> *).
HasRobotStepState sig m =>
(Location -> Bool) -> Cosmic Location -> m (HashSet Location)
neighborFunc Location -> Bool
withinDistanceLimit forall b c a. (b -> c) -> (a -> b) -> a -> c
. forall a. SubworldName -> a -> Cosmic a
Cosmic SubworldName
currentSubworld)
(forall a b. a -> b -> a
const forall a b. (a -> b) -> a -> b
$ forall a b. a -> b -> a
const forall a b. (a -> b) -> a -> b
$ forall (m :: * -> *) a. Monad m => a -> m a
return Int32
1)
(forall (m :: * -> *) a. Monad m => a -> m a
return forall b c a. (b -> c) -> (a -> b) -> a -> c
. Location -> Int32
distHeuristic)
forall (sig :: (* -> *) -> * -> *) (m :: * -> *).
Has (State GameState) sig m =>
Location -> m Bool
goalReachedFunc
(forall (m :: * -> *) a. Monad m => a -> m a
return Location
robotLoc)
withinDistanceLimit :: Location -> Bool
withinDistanceLimit :: Location -> Bool
withinDistanceLimit = (forall a. Ord a => a -> a -> Bool
<= Integer
distanceLimit) forall b c a. (b -> c) -> (a -> b) -> a -> c
. forall a b. (Integral a, Num b) => a -> b
fromIntegral forall b c a. (b -> c) -> (a -> b) -> a -> c
. Location -> Location -> Int32
manhattan Location
robotLoc
nextDir :: [Location] -> Direction
nextDir :: [Location] -> Direction
nextDir [Location]
pathLocs = case [Location]
pathLocs of
[] -> RelativeDir -> Direction
DRelative RelativeDir
DDown
(Location
nextLoc : [Location]
_) -> AbsoluteDir -> Direction
DAbsolute forall a b. (a -> b) -> a -> b
$ V2 Int32 -> AbsoluteDir
nearestDirection forall a b. (a -> b) -> a -> b
$ Location
nextLoc forall (p :: * -> *) a. (Affine p, Num a) => p a -> p a -> Diff p a
.-. Location
robotLoc
neighborFunc ::
HasRobotStepState sig m =>
(Location -> Bool) ->
Cosmic Location ->
m (HashSet Location)
neighborFunc :: forall (sig :: (* -> *) -> * -> *) (m :: * -> *).
HasRobotStepState sig m =>
(Location -> Bool) -> Cosmic Location -> m (HashSet Location)
neighborFunc Location -> Bool
isWithinRange Cosmic Location
loc = do
[Cosmic Location]
locs <- forall (m :: * -> *) a.
Applicative m =>
(a -> m Bool) -> [a] -> m [a]
filterM forall {sig :: (* -> *) -> * -> *} {m :: * -> *}.
(Algebra sig m, Member (State Robot) sig,
Member (State GameState) sig, Member (Throw Exn) sig) =>
Cosmic Location -> m Bool
isWalkableLoc [Cosmic Location]
neighborLocs
forall (m :: * -> *) a. Monad m => a -> m a
return forall a b. (a -> b) -> a -> b
$ forall a. (Eq a, Hashable a) => [a] -> HashSet a
HashSet.fromList forall a b. (a -> b) -> a -> b
$ forall a b. (a -> b) -> [a] -> [b]
map (forall r a (sig :: (* -> *) -> * -> *) (m :: * -> *).
Has (Reader r) sig m =>
Getting a r a -> m a
view forall a1 a2. Lens (Cosmic a1) (Cosmic a2) a1 a2
planar) [Cosmic Location]
locs
where
neighborLocs :: [Cosmic Location]
neighborLocs = Cosmic Location -> [Cosmic Location]
getNeighborLocs Cosmic Location
loc
isWalkableLoc :: Cosmic Location -> m Bool
isWalkableLoc Cosmic Location
someLoc =
if Bool -> Bool
not forall a b. (a -> b) -> a -> b
$ Location -> Bool
isWithinRange forall a b. (a -> b) -> a -> b
$ forall r a (sig :: (* -> *) -> * -> *) (m :: * -> *).
Has (Reader r) sig m =>
Getting a r a -> m a
view forall a1 a2. Lens (Cosmic a1) (Cosmic a2) a1 a2
planar Cosmic Location
someLoc
then forall (m :: * -> *) a. Monad m => a -> m a
return Bool
False
else do
Bool
isGoal <- forall (sig :: (* -> *) -> * -> *) (m :: * -> *).
Has (State GameState) sig m =>
Location -> m Bool
goalReachedFunc forall a b. (a -> b) -> a -> b
$ forall r a (sig :: (* -> *) -> * -> *) (m :: * -> *).
Has (Reader r) sig m =>
Getting a r a -> m a
view forall a1 a2. Lens (Cosmic a1) (Cosmic a2) a1 a2
planar Cosmic Location
someLoc
if Bool
isGoal
then forall (m :: * -> *) a. Monad m => a -> m a
return Bool
True
else forall (t :: * -> *) a. Foldable t => t a -> Bool
null forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> forall (sig :: (* -> *) -> * -> *) (m :: * -> *).
HasRobotStepState sig m =>
Cosmic Location -> m (Maybe MoveFailureDetails)
checkMoveFailureUnprivileged Cosmic Location
someLoc
distHeuristic :: Location -> Int32
distHeuristic :: Location -> Int32
distHeuristic = case PathfindingTarget
target of
LocationTarget Location
gLoc -> Location -> Location -> Int32
manhattan Location
gLoc
EntityTarget EntityName
_eName -> forall a b. a -> b -> a
const Int32
0
goalReachedFunc :: Has (State GameState) sig m => Location -> m Bool
goalReachedFunc :: forall (sig :: (* -> *) -> * -> *) (m :: * -> *).
Has (State GameState) sig m =>
Location -> m Bool
goalReachedFunc Location
loc = case PathfindingTarget
target of
LocationTarget Location
gLoc -> forall (m :: * -> *) a. Monad m => a -> m a
return forall a b. (a -> b) -> a -> b
$ Location
loc forall a. Eq a => a -> a -> Bool
== Location
gLoc
EntityTarget EntityName
eName -> do
Maybe Entity
me <- forall (sig :: (* -> *) -> * -> *) (m :: * -> *).
Has (State GameState) sig m =>
Cosmic Location -> m (Maybe Entity)
entityAt forall a b. (a -> b) -> a -> b
$ forall a. SubworldName -> a -> Cosmic a
Cosmic SubworldName
currentSubworld Location
loc
forall (m :: * -> *) a. Monad m => a -> m a
return forall a b. (a -> b) -> a -> b
$ (forall r a (sig :: (* -> *) -> * -> *) (m :: * -> *).
Has (Reader r) sig m =>
Getting a r a -> m a
view Lens' Entity EntityName
entityName forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> Maybe Entity
me) forall a. Eq a => a -> a -> Bool
== forall a. a -> Maybe a
Just EntityName
eName
distanceLimit :: Integer
distanceLimit = forall b a. b -> (a -> b) -> Maybe a -> b
maybe Integer
maxPathRange (forall a. Ord a => a -> a -> a
min Integer
maxPathRange) Maybe Integer
maybeLimit