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