-
Notifications
You must be signed in to change notification settings - Fork 0
/
Solver.hs
204 lines (192 loc) · 8.05 KB
/
Solver.hs
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
{-# LANGUAGE DeriveAnyClass #-}
{-# LANGUAGE DeriveFunctor #-}
{-# LANGUAGE DeriveGeneric #-}
{-# LANGUAGE DuplicateRecordFields #-}
{-# LANGUAGE FlexibleContexts #-}
{-# LANGUAGE GADTs #-}
{-# LANGUAGE GeneralizedNewtypeDeriving #-}
{-# LANGUAGE InstanceSigs #-}
{-# LANGUAGE MultiParamTypeClasses #-}
{-# LANGUAGE OverloadedStrings #-}
{-# LANGUAGE QuasiQuotes #-}
{-# LANGUAGE RecordWildCards #-}
{-# LANGUAGE ScopedTypeVariables #-}
{-# LANGUAGE Strict #-}
{-# LANGUAGE TupleSections #-}
{-# LANGUAGE TypeFamilies #-}
{-# LANGUAGE UnboxedTuples #-}
{-# OPTIONS_GHC -fno-warn-orphans #-}
module Sokoban.Solver where
import Control.Monad (filterM, forM_)
import Control.Monad.Primitive (PrimMonad (..), PrimState)
import qualified Data.HashMap.Mutable.Basic as HM
import Data.Hashable (Hashable)
import qualified Data.Heap.Mutable.ModelD as HP
import Data.Maybe (fromMaybe, isJust)
import Data.Primitive (MutVar, modifyMutVar', newMutVar)
import Distribution.SPDX (LicenseId (HPND))
import Text.InterpolatedString.QM (qm)
import Prelude hiding (Left, Right, id)
newtype Min
= Min Int
deriving (Show, Read, Eq, Ord)
instance Semigroup Min where
(<>) :: Min -> Min -> Min
(<>) (Min a) (Min b) = Min (min a b)
instance Monoid Min where
mempty :: Min
mempty = Min maxBound
data AStarSolver m p where
AStarSolver ::
(Monad m, Hashable p, Eq p) =>
{ neighbors :: p -> m [p],
distance :: p -> p -> Int, -- for adjacent points only
heuristic :: p -> m Int, -- calculate heuristic distance from first to destination
stopCond :: p -> Bool, -- condition, when the destination is reached
projection :: p -> Int, -- a function to convert points to integer for Heap data structure
injection :: Int -> p, -- an opposite function to projection
nodesBound :: Int -- upper bound for the number of nodes
} ->
AStarSolver m p
aStarFind :: forall m p. (PrimMonad m, Hashable p, Eq p, Show p) => AStarSolver m p -> p -> m [p]
aStarFind AStarSolver {..} src = do
let p2i = projection
let maxCount = nodesBound
maxSize <- newMutVar (0 :: Int)
openHeap <- HP.new maxCount :: m (HP.Heap (PrimState m) Min)
openList <- HM.new :: m (HM.MHashMap (PrimState m) Int (p, p, Int))
closedList <- HM.new :: m (HM.MHashMap (PrimState m) p p)
let isrc = p2i src
HP.unsafePush (Min 0) isrc openHeap
HM.insert openList isrc (src, src, 0)
-- returns path
aStarFindRec openHeap openList closedList p2i maxSize
where
aStarFindRec ::
HP.Heap (PrimState m) Min ->
HM.MHashMap (PrimState m) Int (p, p, Int) ->
HM.MHashMap (PrimState m) p p ->
(p -> Int) ->
MutVar (PrimState m) Int ->
m [p]
aStarFindRec openHeap openList closedList p2i maxSize = do
top' <- HP.pop openHeap -- remove the minimum and return
case top' of
Nothing -> return []
Just (_fscore, ip0) -> do
(p0, parent0, gscore0) <- fromMaybe (error [qm| {ip0} is not found in openList |]) <$> HM.lookup openList ip0
HM.insert closedList p0 parent0
if stopCond p0
then do
backtraceST closedList p0
else do
neighCandidates <- neighbors p0
let isAcc p = do
mc <- member closedList p
mo <- member openList (p2i p)
return $ not mc && not mo
neighbors <- filterM isAcc neighCandidates
forM_ neighbors $ \np -> do
let inp = p2i np
hue <- heuristic np
let dist = distance np p0
let gscoreNp = gscore0 + dist
let fscoreNp = Min (gscore0 + dist + hue)
pg' <- HM.lookup openList inp
case pg' of
Just (p, parent, gscore) | gscoreNp < gscore -> do
-- the neighbour can be reached with smaller cost - change priority
-- otherwise don't touch the neighbour, it will be taken by open_list.pop()
-- openList .= Q.insert np f1 w1 openList0
modifyMutVar' maxSize (+ 1)
HP.push fscoreNp (p2i p) openHeap
HM.insert openList (p2i p) (p, parent, gscoreNp)
Nothing -> do
-- the neighbour is new
-- openList .= Q.insert np f1 w1 openList0
modifyMutVar' maxSize (+ 1)
HP.push fscoreNp inp openHeap
HM.insert openList inp (np, p0, gscoreNp)
_ -> return ()
aStarFindRec openHeap openList closedList p2i maxSize
member :: (PrimMonad m, Hashable k, Eq k) => HM.MHashMap (PrimState m) k a -> k -> m Bool
member hm p = do
v' <- HM.lookup hm p
return $ isJust v'
backtraceST :: forall m p. (PrimMonad m, Eq p, Hashable p, Show p) => HM.MHashMap (PrimState m) p p -> p -> m [p]
backtraceST closedList dst = do
backtraceRec dst [dst]
where
-- we repeatedly lookup for the parent of the current node
backtraceRec :: (PrimMonad m, Eq p) => p -> [p] -> m [p]
backtraceRec current acc = do
parent' <- HM.lookup closedList current
case parent' of
Nothing -> return []
Just parent
| current == parent -> return acc
Just parent -> backtraceRec parent (parent : acc)
breadFirstFind :: forall m p. (PrimMonad m, Hashable p, Ord p, Show p) => AStarSolver m p -> p -> m [p]
breadFirstFind AStarSolver {..} src = do
let p2i = projection
maxSize <- newMutVar (0 :: Int)
let maxCount = nodesBound
openHeap <- HP.new maxCount :: m (HP.Heap (PrimState m) Min)
openList <- HM.new :: m (HM.MHashMap (PrimState m) Int (p, p))
closedList <- HM.new :: m (HM.MHashMap (PrimState m) p p)
let isrc = p2i src
HP.push (Min 0) isrc openHeap
HM.insert openList isrc (src, src)
-- _v <- readMutVar maxSize
-- traceM [qm| max size = {v} |]
breadFirstFindRec openHeap openList closedList p2i maxSize 0
where
keys :: (PrimMonad m, Hashable k, Eq k) => HM.MHashMap (PrimState m) k v -> m [k]
keys = HM.foldM (\a k _v -> return $ k : a) []
member :: (PrimMonad m, Hashable k, Eq k) => HM.MHashMap (PrimState m) k a -> k -> m Bool
member hm p = do
v' <- HM.lookup hm p
return $ isJust v'
breadFirstFindRec ::
HP.Heap (PrimState m) Min ->
HM.MHashMap (PrimState m) Int (p, p) ->
HM.MHashMap (PrimState m) p p ->
(p -> Int) ->
MutVar (PrimState m) Int ->
Int ->
m [p]
breadFirstFindRec openHeap openList closedList p2i maxSize it = do
-- do looping until heap becomes empty
top' <- HP.pop openHeap -- remove the minimum and return
case top' of
Nothing -> do
keys closedList -- gather keys
Just (Min dist0, ip0) -> do
(p0, parent0) <- fromMaybe (error [qm| {ip0} is not found in openList |]) <$> HM.lookup openList ip0
HM.insert closedList p0 parent0
neighCandidates <- neighbors p0
let isAcc p = do
mc <- member closedList p
mo <- member openList (p2i p)
return $ not mc && not mo
neighbors <- filterM isAcc neighCandidates
forM_ neighbors $ \np -> do
let inp = p2i np
let distNp = distance np p0
let gscoreNp = Min $ dist0 + distNp
pg' <- HM.lookup openList inp
case pg' of
Just (p, parent) -> do
-- the neighbour can be reached with smaller cost - change priority
-- otherwise don't touch the neighbour, it will be taken by open_list.pop()
-- openList .= Q.insert np f1 w1 openList0
modifyMutVar' maxSize (+ 1)
HP.push gscoreNp (p2i p) openHeap
HM.insert openList (p2i p) (np, parent)
Nothing -> do
-- the neighbour is new
-- openList .= Q.insert np f1 w1 openList0
modifyMutVar' maxSize (+ 1)
HP.push gscoreNp inp openHeap
HM.insert openList inp (np, p0)
breadFirstFindRec openHeap openList closedList p2i maxSize (it + 1)