{-# LANGUAGE ParallelListComp #-}
--------------------------------------------------------------------------------
-- |
-- Module      :  Algorithms.Geometry.SSSP.Naive
-- Copyright   :  (C) David Himmelstrup
-- License     :  see the LICENSE file
-- Maintainer  :  David Himmelstrup
--------------------------------------------------------------------------------
module Algorithms.Geometry.SSSP.Naive
  ( sssp
  , sssp'
  ) where

import           Algorithms.FloydWarshall  (floydWarshall, mkGraph, mkIndex)
import           Control.Lens
import           Control.Monad.ST          (runST)
import           Data.Ext                  (_core, core)
import qualified Data.Foldable             as F
import           Data.Geometry.Interval    (EndPoint (Closed, Open), end, start)
import           Data.Geometry.LineSegment (LineSegment (..), sqSegmentLength)
import           Data.Geometry.Point       (ccwCmpAroundWith')
import           Data.Geometry.Polygon     (SimplePolygon, listEdges, outerBoundaryVector)
import           Data.Intersection         (IsIntersectableWith (intersect),
                                            NoIntersection (NoIntersection))
import           Data.Vector               (Vector)
import qualified Data.Vector               as V
import qualified Data.Vector.Circular      as CV
import qualified Data.Vector.Unboxed       as VU
import           Data.Vinyl                (Rec (RNil, (:&)))
import           Data.Vinyl.CoRec          (Handler (H), match)
import           Linear.Affine             ((.-.))

type SSSP = VU.Vector Int

-- | \( O(n^3) \) Single-Source Shortest Path.
sssp :: (Real r, Fractional r) => SimplePolygon p r -> SSSP
sssp :: SimplePolygon p r -> SSSP
sssp SimplePolygon p r
p = Vector SSSP -> SSSP
forall a. Vector a -> a
V.head (Vector SSSP -> SSSP)
-> (SimplePolygon p r -> Vector SSSP) -> SimplePolygon p r -> SSSP
forall b c a. (b -> c) -> (a -> b) -> a -> c
. SimplePolygon p r -> Vector SSSP
forall r p.
(Real r, Fractional r) =>
SimplePolygon p r -> Vector SSSP
sssp' (SimplePolygon p r -> SSSP) -> SimplePolygon p r -> SSSP
forall a b. (a -> b) -> a -> b
$ SimplePolygon p r
p

-- | \( O(n^3) \) Single-Source Shortest Path from all vertices.
sssp' :: (Real r, Fractional r) => SimplePolygon p r -> Vector SSSP
sssp' :: SimplePolygon p r -> Vector SSSP
sssp' SimplePolygon p r
p = (forall s. ST s (Vector SSSP)) -> Vector SSSP
forall a. (forall s. ST s a) -> a
runST ((forall s. ST s (Vector SSSP)) -> Vector SSSP)
-> (forall s. ST s (Vector SSSP)) -> Vector SSSP
forall a b. (a -> b) -> a -> b
$ do
    -- Create an n*n matrix containing paths and distances between vertices.
    MVector s (Double, Int)
graph <- Int
-> Double -> [(Int, Int, Double)] -> ST s (MVector s (Double, Int))
forall a s.
(Unbox a, Num a) =>
Int -> a -> [(Int, Int, a)] -> ST s (MVector s (a, Int))
mkGraph Int
n Double
infinity (SimplePolygon p r -> [(Int, Int, Double)]
forall r p.
(Real r, Fractional r) =>
SimplePolygon p r -> [(Int, Int, Double)]
visibleEdges SimplePolygon p r
p)
    -- Use FloydWarshall O(n^3) to complete the matrix.
    Int -> MVector s (Double, Int) -> ST s ()
forall a s.
(Unbox a, Fractional a, Ord a) =>
Int -> MVector s (a, Int) -> ST s ()
floydWarshall Int
n MVector s (Double, Int)
graph
    -- Create a tree describing the shortest path from any node to the 0th node.
    Vector (Double, Int)
g <- MVector (PrimState (ST s)) (Double, Int)
-> ST s (Vector (Double, Int))
forall a (m :: * -> *).
(Unbox a, PrimMonad m) =>
MVector (PrimState m) a -> m (Vector a)
VU.unsafeFreeze MVector s (Double, Int)
MVector (PrimState (ST s)) (Double, Int)
graph
    Vector SSSP -> ST s (Vector SSSP)
forall (f :: * -> *) a. Applicative f => a -> f a
pure (Vector SSSP -> ST s (Vector SSSP))
-> Vector SSSP -> ST s (Vector SSSP)
forall a b. (a -> b) -> a -> b
$ Int -> (Int -> SSSP) -> Vector SSSP
forall a. Int -> (Int -> a) -> Vector a
V.generate Int
n ((Int -> SSSP) -> Vector SSSP) -> (Int -> SSSP) -> Vector SSSP
forall a b. (a -> b) -> a -> b
$ \Int
origin ->
      Int -> (Int -> Int) -> SSSP
forall a. Unbox a => Int -> (Int -> a) -> Vector a
VU.generate Int
n ((Int -> Int) -> SSSP) -> (Int -> Int) -> SSSP
forall a b. (a -> b) -> a -> b
$ \Int
i ->
        let (Double
_dist, Int
next) = Vector (Double, Int)
g Vector (Double, Int) -> Int -> (Double, Int)
forall a. Unbox a => Vector a -> Int -> a
VU.! Int -> (Int, Int) -> Int
forall a. Num a => a -> (a, a) -> a
mkIndex Int
n (Int
i, Int
origin)
        in Int
next
  where
    infinity :: Double
infinity = String -> Double
forall a. Read a => String -> a
read String
"Infinity" :: Double
    n :: Int
n = CircularVector (Point 2 r :+ p) -> Int
forall (t :: * -> *) a. Foldable t => t a -> Int
F.length (SimplePolygon p r
p SimplePolygon p r
-> Getting
     (CircularVector (Point 2 r :+ p))
     (SimplePolygon p r)
     (CircularVector (Point 2 r :+ p))
-> CircularVector (Point 2 r :+ p)
forall s a. s -> Getting a s a -> a
^. Getting
  (CircularVector (Point 2 r :+ p))
  (SimplePolygon p r)
  (CircularVector (Point 2 r :+ p))
forall (t :: PolygonType) p r.
Getter (Polygon t p r) (CircularVector (Point 2 r :+ p))
outerBoundaryVector)

-- \( O(n^3) \)
visibleEdges :: (Real r, Fractional r) => SimplePolygon p r -> [(Int, Int, Double)]
visibleEdges :: SimplePolygon p r -> [(Int, Int, Double)]
visibleEdges SimplePolygon p r
p = [[(Int, Int, Double)]] -> [(Int, Int, Double)]
forall (t :: * -> *) a. Foldable t => t [a] -> [a]
concat
  [
    [ (Int
i, Int
j, Double -> Double
forall a. Floating a => a -> a
sqrt (r -> Double
forall a b. (Real a, Fractional b) => a -> b
realToFrac (LineSegment 2 p r -> r
forall (d :: Nat) r p. (Arity d, Num r) => LineSegment d p r -> r
sqSegmentLength LineSegment 2 p r
line)))
    | Int
j <- [Int
iInt -> Int -> Int
forall a. Num a => a -> a -> a
+Int
2 .. Int
nInt -> Int -> Int
forall a. Num a => a -> a -> a
-Int
1]
    , let endPt :: Point 2 r :+ p
endPt = CircularVector (Point 2 r :+ p) -> Int -> Point 2 r :+ p
forall a. CircularVector a -> Int -> a
CV.index CircularVector (Point 2 r :+ p)
vs Int
j
    , let line :: LineSegment 2 p r
line = EndPoint (Point 2 r :+ p)
-> EndPoint (Point 2 r :+ p) -> LineSegment 2 p r
forall (d :: Nat) r p.
EndPoint (Point d r :+ p)
-> EndPoint (Point d r :+ p) -> LineSegment d p r
LineSegment ((Point 2 r :+ p) -> EndPoint (Point 2 r :+ p)
forall a. a -> EndPoint a
Closed Point 2 r :+ p
pt) ((Point 2 r :+ p) -> EndPoint (Point 2 r :+ p)
forall a. a -> EndPoint a
Open Point 2 r :+ p
endPt)
      -- Check if the line goes through the inside of the polygon.
    , Vector 2 r
-> (Point 2 r :+ p)
-> (Point 2 r :+ p)
-> (Point 2 r :+ p)
-> Ordering
forall r c a b.
(Ord r, Num r) =>
Vector 2 r
-> (Point 2 r :+ c)
-> (Point 2 r :+ a)
-> (Point 2 r :+ b)
-> Ordering
ccwCmpAroundWith' (((Point 2 r :+ p) -> Point 2 r
forall core extra. (core :+ extra) -> core
_core Point 2 r :+ p
prev) Point 2 r -> Point 2 r -> Diff (Point 2) r
forall (p :: * -> *) a. (Affine p, Num a) => p a -> p a -> Diff p a
.-. ((Point 2 r :+ p) -> Point 2 r
forall core extra. (core :+ extra) -> core
_core Point 2 r :+ p
pt)) Point 2 r :+ p
pt Point 2 r :+ p
endPt Point 2 r :+ p
next Ordering -> Ordering -> Bool
forall a. Eq a => a -> a -> Bool
== Ordering
GT
      -- Check if there are any intersections not the line end points.
    , Bool -> Bool
not (LineSegment 2 p r -> [LineSegment 2 p r] -> Bool
forall r p.
(Ord r, Fractional r) =>
LineSegment 2 p r -> [LineSegment 2 p r] -> Bool
interiorIntersection LineSegment 2 p r
line [LineSegment 2 p r]
edges)
    ]
  | Int
i <- [Int
0 .. Int
nInt -> Int -> Int
forall a. Num a => a -> a -> a
-Int
1]
  , let pt :: Point 2 r :+ p
pt = CircularVector (Point 2 r :+ p) -> Int -> Point 2 r :+ p
forall a. CircularVector a -> Int -> a
CV.index CircularVector (Point 2 r :+ p)
vs Int
i
        prev :: Point 2 r :+ p
prev = CircularVector (Point 2 r :+ p) -> Int -> Point 2 r :+ p
forall a. CircularVector a -> Int -> a
CV.index CircularVector (Point 2 r :+ p)
vs (Int
iInt -> Int -> Int
forall a. Num a => a -> a -> a
-Int
1)
        next :: Point 2 r :+ p
next = CircularVector (Point 2 r :+ p) -> Int -> Point 2 r :+ p
forall a. CircularVector a -> Int -> a
CV.index CircularVector (Point 2 r :+ p)
vs (Int
iInt -> Int -> Int
forall a. Num a => a -> a -> a
+Int
1)
  ] [(Int, Int, Double)]
-> [(Int, Int, Double)] -> [(Int, Int, Double)]
forall a. [a] -> [a] -> [a]
++
  [ (Int
i,(Int
iInt -> Int -> Int
forall a. Num a => a -> a -> a
+Int
1)Int -> Int -> Int
forall a. Integral a => a -> a -> a
`mod`Int
n,Double -> Double
forall a. Floating a => a -> a
sqrt (r -> Double
forall a b. (Real a, Fractional b) => a -> b
realToFrac (LineSegment 2 p r -> r
forall (d :: Nat) r p. (Arity d, Num r) => LineSegment d p r -> r
sqSegmentLength LineSegment 2 p r
edge)))
  | (Int
i, LineSegment 2 p r
edge) <- [Int] -> [LineSegment 2 p r] -> [(Int, LineSegment 2 p r)]
forall a b. [a] -> [b] -> [(a, b)]
zip [Int
0..] [LineSegment 2 p r]
edges
  ]
  where
    vs :: CircularVector (Point 2 r :+ p)
vs = SimplePolygon p r
pSimplePolygon p r
-> Getting
     (CircularVector (Point 2 r :+ p))
     (SimplePolygon p r)
     (CircularVector (Point 2 r :+ p))
-> CircularVector (Point 2 r :+ p)
forall s a. s -> Getting a s a -> a
^.Getting
  (CircularVector (Point 2 r :+ p))
  (SimplePolygon p r)
  (CircularVector (Point 2 r :+ p))
forall (t :: PolygonType) p r.
Getter (Polygon t p r) (CircularVector (Point 2 r :+ p))
outerBoundaryVector
    n :: Int
n = CircularVector (Point 2 r :+ p) -> Int
forall (t :: * -> *) a. Foldable t => t a -> Int
F.length CircularVector (Point 2 r :+ p)
vs
    edges :: [LineSegment 2 p r]
edges = SimplePolygon p r -> [LineSegment 2 p r]
forall (t :: PolygonType) p r. Polygon t p r -> [LineSegment 2 p r]
listEdges SimplePolygon p r
p

interiorIntersection :: (Ord r, Fractional r) => LineSegment 2 p r -> [LineSegment 2 p r] -> Bool
interiorIntersection :: LineSegment 2 p r -> [LineSegment 2 p r] -> Bool
interiorIntersection LineSegment 2 p r
_ [] = Bool
False
interiorIntersection LineSegment 2 p r
l (LineSegment 2 p r
x:[LineSegment 2 p r]
xs) =
  CoRec Identity '[NoIntersection, Point 2 r, LineSegment 2 p r]
-> Handlers '[NoIntersection, Point 2 r, LineSegment 2 p r] Bool
-> Bool
forall (ts :: [*]) b. CoRec Identity ts -> Handlers ts b -> b
match (LineSegment 2 p r
l LineSegment 2 p r
-> LineSegment 2 p r
-> Intersection (LineSegment 2 p r) (LineSegment 2 p r)
forall g h. IsIntersectableWith g h => g -> h -> Intersection g h
`intersect` LineSegment 2 p r
x) (
       (NoIntersection -> Bool) -> Handler Bool NoIntersection
forall b a. (a -> b) -> Handler b a
H (\NoIntersection
NoIntersection -> Bool
False)
    Handler Bool NoIntersection
-> Rec (Handler Bool) '[Point 2 r, LineSegment 2 p r]
-> Handlers '[NoIntersection, Point 2 r, LineSegment 2 p r] Bool
forall u (a :: u -> *) (r :: u) (rs :: [u]).
a r -> Rec a rs -> Rec a (r : rs)
:& (Point 2 r -> Bool) -> Handler Bool (Point 2 r)
forall b a. (a -> b) -> Handler b a
H (\Point 2 r
pt -> Point 2 r
pt Point 2 r -> Point 2 r -> Bool
forall a. Eq a => a -> a -> Bool
/= LineSegment 2 p r
lLineSegment 2 p r
-> Getting (Point 2 r) (LineSegment 2 p r) (Point 2 r) -> Point 2 r
forall s a. s -> Getting a s a -> a
^.((Point 2 r :+ p) -> Const (Point 2 r) (Point 2 r :+ p))
-> LineSegment 2 p r -> Const (Point 2 r) (LineSegment 2 p r)
forall t. HasStart t => Lens' t (StartCore t :+ StartExtra t)
start(((Point 2 r :+ p) -> Const (Point 2 r) (Point 2 r :+ p))
 -> LineSegment 2 p r -> Const (Point 2 r) (LineSegment 2 p r))
-> ((Point 2 r -> Const (Point 2 r) (Point 2 r))
    -> (Point 2 r :+ p) -> Const (Point 2 r) (Point 2 r :+ p))
-> Getting (Point 2 r) (LineSegment 2 p r) (Point 2 r)
forall b c a. (b -> c) -> (a -> b) -> a -> c
.(Point 2 r -> Const (Point 2 r) (Point 2 r))
-> (Point 2 r :+ p) -> Const (Point 2 r) (Point 2 r :+ p)
forall core extra core'.
Lens (core :+ extra) (core' :+ extra) core core'
core Bool -> Bool -> Bool
&& Point 2 r
pt Point 2 r -> Point 2 r -> Bool
forall a. Eq a => a -> a -> Bool
/= LineSegment 2 p r
lLineSegment 2 p r
-> Getting (Point 2 r) (LineSegment 2 p r) (Point 2 r) -> Point 2 r
forall s a. s -> Getting a s a -> a
^.((Point 2 r :+ p) -> Const (Point 2 r) (Point 2 r :+ p))
-> LineSegment 2 p r -> Const (Point 2 r) (LineSegment 2 p r)
forall t. HasEnd t => Lens' t (EndCore t :+ EndExtra t)
end(((Point 2 r :+ p) -> Const (Point 2 r) (Point 2 r :+ p))
 -> LineSegment 2 p r -> Const (Point 2 r) (LineSegment 2 p r))
-> ((Point 2 r -> Const (Point 2 r) (Point 2 r))
    -> (Point 2 r :+ p) -> Const (Point 2 r) (Point 2 r :+ p))
-> Getting (Point 2 r) (LineSegment 2 p r) (Point 2 r)
forall b c a. (b -> c) -> (a -> b) -> a -> c
.(Point 2 r -> Const (Point 2 r) (Point 2 r))
-> (Point 2 r :+ p) -> Const (Point 2 r) (Point 2 r :+ p)
forall core extra core'.
Lens (core :+ extra) (core' :+ extra) core core'
core)
    Handler Bool (Point 2 r)
-> Rec (Handler Bool) '[LineSegment 2 p r]
-> Rec (Handler Bool) '[Point 2 r, LineSegment 2 p r]
forall u (a :: u -> *) (r :: u) (rs :: [u]).
a r -> Rec a rs -> Rec a (r : rs)
:& (LineSegment 2 p r -> Bool) -> Handler Bool (LineSegment 2 p r)
forall b a. (a -> b) -> Handler b a
H (\LineSegment 2 p r
line -> LineSegment 2 p r -> r
forall (d :: Nat) r p. (Arity d, Num r) => LineSegment d p r -> r
sqSegmentLength LineSegment 2 p r
line r -> r -> Bool
forall a. Eq a => a -> a -> Bool
/= r
0)
    Handler Bool (LineSegment 2 p r)
-> Rec (Handler Bool) '[]
-> Rec (Handler Bool) '[LineSegment 2 p r]
forall u (a :: u -> *) (r :: u) (rs :: [u]).
a r -> Rec a rs -> Rec a (r : rs)
:& Rec (Handler Bool) '[]
forall u (a :: u -> *). Rec a '[]
RNil)
  Bool -> Bool -> Bool
|| LineSegment 2 p r -> [LineSegment 2 p r] -> Bool
forall r p.
(Ord r, Fractional r) =>
LineSegment 2 p r -> [LineSegment 2 p r] -> Bool
interiorIntersection LineSegment 2 p r
l [LineSegment 2 p r]
xs