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