never executed always true always false
1 {-# LANGUAGE FlexibleInstances #-}
2 {-# LANGUAGE MultiParamTypeClasses #-}
3 {-# LANGUAGE RecordWildCards #-}
4 {-# OPTIONS_GHC -fno-warn-orphans #-}
5 {-# OPTIONS_HADDOCK hide #-}
6 module Reanimate.Math.SSSP
7 ( -- * Single-Source-Shortest-Path
8 SSSP
9 , sssp -- :: (Fractional a, Ord a) => Ring a -> Dual -> SSSP
10 , ssspFinger
11 , dual -- :: Int -> Triangulation -> Dual
12 , Dual(..)
13 , DualTree(..)
14 -- * Misc
15 , dualToTriangulation -- :: Ring Rational -> Dual -> Triangulation
16 , visibilityArray -- :: Ring Rational -> V.Vector [Int]
17 , naive -- :: Ring Rational -> SSSP
18 , naive2 -- :: Ring Rational -> SSSP
19 , drawDual -- :: Dual -> String
20 ) where
21
22 import Control.Monad
23 import Control.Monad.ST
24 import qualified Data.FingerTree as F
25 import Data.Foldable
26 import Data.List
27 import qualified Data.Map as Map
28 import Data.Maybe
29 import Data.STRef
30 import Data.Tree
31 import qualified Data.Vector as V
32 import qualified Data.Vector.Mutable as MV
33 import Reanimate.Math.Common
34 import Reanimate.Math.Triangulate
35
36 -- import Debug.Trace
37
38 type SSSP = V.Vector Int
39
40
41 -- ssspParent :: Polygon -> SSSP -> Int -> Int
42 -- ssspParent p sTree x =
43 -- (sTree V.! ((x - polygonOffset p) `mod` n) + polygonOffset p) `mod` n
44 -- where
45 -- n = polygonSize p
46
47 visibilityArray :: Ring Rational -> V.Vector [Int]
48 visibilityArray p = arr
49 where
50 n = ringSize p
51 arr = V.fromList
52 [ visibility y
53 | y <- [0..n-1]
54 ]
55 visibility y =
56 [ i
57 | i <- [0..y-1]
58 , y `elem` arr V.! i ] ++
59 [ i
60 | i <- [y+1 .. n-1]
61 , let pI = ringAccess p i
62 isOpen = isRightTurn pYp pY pYn
63 , ringClamp p (y+1) == i || ringClamp p (y-1) == i || if isOpen
64 then isLeftTurnOrLinear pY pYn pI ||
65 isLeftTurnOrLinear pYp pY pI
66 else not $ isRightTurn pY pYn pI ||
67 isRightTurn pYp pY pI
68 , let myEdges = [(e1,e2) | (e1,e2) <- edges, e1/=y, e1/=i, e2/=y,e2/=i]
69 , all (isNothing . lineIntersect (pY,pI))
70 [ (ringAccess p e1, ringAccess p e2) | (e1,e2) <- myEdges ]]
71 where
72 pY = ringAccess p y
73 pYn = ringAccess p $ y+1
74 pYp = ringAccess p $ y-1
75 edges = zip [0..n-1] (tail [0..n-1] ++ [0])
76
77
78
79 -- Iterative Single Source Shortest Path solver. Quite slow.
80 naive :: Ring Rational -> SSSP
81 naive p =
82 V.fromList $ Map.elems $
83 Map.map snd $
84 worker initial
85 where
86 initial = Map.singleton 0 (0,0)
87 visibility = visibilityArray p
88 worker :: Map.Map Int (Rational, Int) -> Map.Map Int (Rational, Int)
89 worker m
90 | m==newM = newM
91 | otherwise = worker newM
92 where
93 ms' = [ Map.fromList
94 [ case Map.lookup v m of
95 Nothing -> (v, (distThroughI, i))
96 Just (otherDist,parent)
97 | otherDist > distThroughI -> (v, (distThroughI, i))
98 | otherwise -> (v, (otherDist, parent))
99 | v <- visibility V.! i
100 , let distThroughI = dist + approxDist (ringAccess p i) (ringAccess p v) ]
101 | (i,(dist,_)) <- Map.toList m
102 ]
103 newM = Map.unionsWith g (m:ms') :: Map.Map Int (Rational,Int)
104 g a b = if fst a < fst b then a else b
105
106 naive2 :: Ring Rational -> SSSP
107 naive2 p = runST $ do
108 parents <- MV.replicate (ringSize p) (-1)
109 costs <- MV.replicate (ringSize p) (-1)
110 MV.write parents 0 0
111 MV.write costs 0 0
112 changedRef <- newSTRef False
113 let loop i
114 | i == ringSize p = do
115 changed <- readSTRef changedRef
116 when changed $ do
117 writeSTRef changedRef False
118 loop 0
119 | otherwise = do
120 myCost <- MV.read costs i
121 unless (myCost < 0) $
122 forM_ (visibility V.! i) $ \n -> do
123 -- n is visible from i.
124 theirCost <- MV.read costs n
125 let throughCost = myCost + approxDist (ringAccess p i) (ringAccess p n)
126 when (throughCost < theirCost || theirCost < 0) $ do
127 MV.write parents n i
128 MV.write costs n throughCost
129 writeSTRef changedRef True
130 loop (i+1)
131 loop 0
132 V.unsafeFreeze parents
133 where
134 visibility = visibilityArray p
135
136 -- Dual of triangulated polygon
137 data Dual = Dual (Int,Int,Int) -- (a,b,c)
138 DualTree -- borders ca
139 DualTree -- borders bc
140 deriving (Show)
141
142 data DualTree
143 = EmptyDual
144 | NodeDual Int -- axb triangle, a and b are from parent.
145 DualTree -- borders xb
146 DualTree -- borders ax
147 deriving (Show)
148
149 drawDual :: Dual -> String
150 drawDual d = drawTree $
151 case d of
152 Dual (a,b,c) l r -> Node (show (a,b,c)) [worker c a l, worker b c r]
153 where
154 worker _a _b EmptyDual = Node "Leaf" []
155 worker a b (NodeDual x l r) =
156 Node (show (b,a,x)) [worker x b l, worker a x r]
157
158 dualToTriangulation :: Ring Rational -> Dual -> Triangulation
159 dualToTriangulation p d = edgesToTriangulation (ringSize p) $ filter goodEdge $
160 case d of
161 Dual (a,b,c) l r ->
162 (a,b):(a,c):(b,c):worker c a l ++ worker b c r
163 where
164 goodEdge (a,b)
165 = a /= ringClamp p (b+1) && a /= ringClamp p (b-1)
166 worker _a _b EmptyDual = []
167 worker a b (NodeDual x l r) =
168 (a,x) : (x, b) : worker x b l ++ worker a x r
169
170 -- Dual path:
171 -- (Int,Int,Int) + V.Vector Int + V.Vector LeftOrRight
172
173 -- simplifyDual :: DualTree -> DualTree
174 -- -- simplifyDual (NodeDual x EmptyDual EmptyDual) = NodeLeaf x
175 -- -- simplifyDual (NodeDual x l EmptyDual) = NodeDualL x l
176 -- -- simplifyDual (NodeDual x EmptyDual r) = NodeDualR x r
177 -- simplifyDual d = d
178
179 dual :: Int -> Triangulation -> Dual
180 dual root t =
181 case hasTriangle of
182 [] -> error "weird triangulation"
183 -- [] -> Dual (0,1,V.length t-1) EmptyDual (dualTree t (1, (V.length t-1)) 0)
184 (x:_) -> Dual (root,rootNext,x) (dualTree t (x,root) rootNext) (dualTree t (rootNext,x) root)
185 where
186 rootNext = idx (root+1)
187 rootPrev = idx (root-1)
188 rootNNext = idx (root+2)
189 idx i = i `mod` n
190 hasTriangle = (rootPrev : t V.! root) `intersect` (rootNNext : t V.! rootNext)
191 n = V.length t
192
193 -- a=6, b=0, e=1
194 dualTree :: Triangulation -> (Int,Int) -> Int -> DualTree
195 dualTree t (a,b) e = -- simplifyDual $
196 case hasTriangle of
197 [] -> EmptyDual
198 [(ab)] ->
199 NodeDual ab
200 (dualTree t (ab,b) a)
201 (dualTree t (a,ab) b)
202 _ -> error $ "Invalid triangulation: " ++ show (a,b,e,hasTriangle)
203 where
204 hasTriangle = (prev a : next a : t V.! a) `intersect` (prev b : next b : t V.! b)
205 \\ [e]
206 n = V.length t
207 next x = (x+1) `mod` n
208 prev x = (x-1) `mod` n
209
210
211 -- dualRoot :: Dual -> Int
212 -- dualRoot (Dual (a,_,_) _ _) = a
213
214 -- O(n*ln n), could be O(n) if I could figure out how to use fingertrees...
215 sssp :: (Fractional a, Ord a, Epsilon a) => Ring a -> Dual -> SSSP
216 sssp p d = toSSSP $
217 case d of
218 Dual (a,b,c) l r ->
219 (a, a) :
220 (b, a) :
221 (c, a) :
222 worker [c] [b] a r ++
223 loopLeft a c l
224 where
225 toSSSP edges =
226 (V.fromList . map snd . sortOn fst) edges
227 loopLeft a outer l =
228 case l of
229 EmptyDual -> []
230 NodeDual x l' r' ->
231 (x,a) :
232 worker [x] [outer] a r' ++
233 loopLeft a x l'
234 searchFn _checkStep _cusp _x [] = Nothing
235 searchFn checkStep cusp x (y:ys)
236 | not (checkStep (ringAccess p cusp) (ringAccess p y) (ringAccess p x))
237 = Just $ helper [] y ys
238 | otherwise = Nothing
239 where
240 helper acc v [] = (v, [], reverse acc)
241 helper acc v1 (v2:vs)
242 | checkStep (ringAccess p v1) (ringAccess p v2) (ringAccess p x) =
243 (v1, v2:vs, reverse acc)
244 | otherwise = helper (v1:acc) v2 vs
245 searchRight = searchFn isLeftTurn
246 searchLeft = searchFn isRightTurn
247 -- adj x = x -- ringClamp p (x-dualRoot d)
248 -- optTrace msg =
249 -- if False -- dualRoot d == 1 || dualRoot d == 0
250 -- then trace msg
251 -- else id
252 worker _ _ _ EmptyDual = []
253 worker f1 f2 cusp (NodeDual x l r) =
254 -- (optTrace ("Funnel: " ++ show
255 -- (map adj $ toList f1
256 -- ,adj cusp
257 -- ,map adj $ toList f2
258 -- ,adj x
259 -- , dualRoot d))
260 -- ) $
261 case searchLeft cusp x (toList f1) of
262 Just (v, f1Hi, f1Lo) ->
263 -- optTrace (" Visble from left: " ++ show (adj x,adj v)) $
264 (x, v::Int) :
265 worker f1Hi [x] v l ++
266 worker (f1Lo ++ [v, x]) f2 cusp r
267 Nothing ->
268 case searchRight cusp x (toList f2) of
269 Just (v, f2Hi, f2Lo) ->
270 -- optTrace (" Visble from right: " ++ show (adj x,adj v)) $
271 (x, v::Int) :
272 worker f1 (f2Lo ++ [v, x]) cusp l ++
273 worker [x] f2Hi v r
274 Nothing ->
275 -- optTrace (" Visble from cusp: " ++ show (adj x,adj cusp)) $
276 (x, cusp::Int) :
277 worker f1 [x] cusp l ++
278 worker [x] f2 cusp r
279
280 data MinMax = MinMax Int Int | MinMaxEmpty deriving (Show)
281 instance Semigroup MinMax where
282 MinMaxEmpty <> b = b
283 a <> MinMaxEmpty = a
284 MinMax a _b <> MinMax _c d
285 = MinMax a d
286 instance Monoid MinMax where
287 mempty = MinMaxEmpty
288
289 type Chain = F.FingerTree MinMax Int
290 data Funnel = Funnel
291 { funnelLeft :: Chain
292 , funnelCusp :: Int
293 , funnelRight :: Chain
294 }
295
296 instance F.Measured MinMax Int where
297 measure i = MinMax i i
298
299 splitFunnel :: (Epsilon a, Fractional a, Ord a) => Ring a -> Int -> Funnel -> (Int, Funnel, Funnel)
300 splitFunnel p x Funnel{..}
301 | isOnLeftChain =
302 case doSearch isRightTurn funnelLeft of
303 (lower, t, upper) ->
304 ( t
305 , Funnel upper t (F.singleton x)
306 , Funnel (lower F.|> t F.|> x) funnelCusp funnelRight)
307 | isOnRightChain =
308 case doSearch isLeftTurn funnelRight of
309 (lower, t, upper) ->
310 ( t
311 , Funnel funnelLeft funnelCusp (lower F.|> t F.|> x)
312 , Funnel (F.singleton x) t upper)
313 | otherwise =
314 ( funnelCusp
315 , Funnel funnelLeft funnelCusp (F.singleton x)
316 , Funnel (F.singleton x) funnelCusp funnelRight)
317 where
318 isOnLeftChain = fromMaybe False $
319 isLeftTurnOrLinear cuspElt <$> leftElt <*> pure targetElt
320 isOnRightChain = fromMaybe False $
321 isRightTurnOrLinear cuspElt <$> rightElt <*> pure targetElt
322 doSearch fn chain =
323 case F.search (searchChain fn) (chain::Chain) of
324 F.Position lower t upper -> (lower, t, upper)
325 F.OnLeft -> error "cannot happen"
326 F.OnRight -> error "cannot happen"
327 F.Nowhere -> error "cannot happen"
328 searchChain _ MinMaxEmpty _ = False
329 searchChain _ _ MinMaxEmpty = True
330 searchChain check (MinMax _ l) (MinMax r _) =
331 check (ringAccess p l) (ringAccess p r) targetElt
332 cuspElt = ringAccess p funnelCusp
333 targetElt = ringAccess p x
334 leftElt = ringAccess p <$> chainLeft funnelLeft
335 rightElt = ringAccess p <$> chainLeft funnelRight
336 chainLeft chain =
337 case F.viewl chain of
338 F.EmptyL -> Nothing
339 elt F.:< _ -> Just elt
340
341 -- O(n)
342 ssspFinger :: (Epsilon a, Fractional a, Ord a) => Ring a -> Dual -> SSSP
343 ssspFinger p d = toSSSP $
344 case d of
345 Dual (a,b,c) l r ->
346 (a, a) :
347 (b, a) :
348 (c, a) :
349 worker (Funnel (F.singleton c) a (F.singleton b)) r ++
350 loopLeft a c l
351 where
352 toSSSP edges =
353 (V.fromList . map snd . sortOn fst) edges
354 loopLeft a outer l =
355 case l of
356 EmptyDual -> []
357 NodeDual x l' r' ->
358 (x,a) :
359 worker (Funnel (F.singleton x) a (F.singleton outer)) r' ++
360 loopLeft a x l'
361 worker _ EmptyDual = []
362 worker f (NodeDual x l r) =
363 case splitFunnel p x f of
364 (v, fL, fR) ->
365 (x, v) :
366 worker fL l ++
367 worker fR r