{-# LANGUAGE RecordWildCards #-}
--------------------------------------------------------------------------------
-- |
-- Module      :  Algorithms.Geometry.SSSP
-- Copyright   :  (C) David Himmelstrup
-- License     :  see the LICENSE file
-- Maintainer  :  David Himmelstrup
--------------------------------------------------------------------------------
module Algorithms.Geometry.SSSP
  ( SSSP
  , triangulate
  , sssp
  , visibilityDual
  , visibilityFinger
  , visibilitySensitive
  ) where

import Algorithms.Geometry.PolygonTriangulation.Triangulate (triangulate')
import Algorithms.Geometry.PolygonTriangulation.Types       (PolygonEdgeType)

import           Algorithms.Graph.DFS            (adjacencyLists, dfs', dfsSensitive)
import           Control.Lens                    ((^.))
import           Data.Bitraversable
import           Data.Either
import           Data.Ext                        (ext, extra, type (:+) (..))
import qualified Data.FingerTree                 as F
import           Data.Geometry.Line              (lineThrough)
import           Data.Geometry.LineSegment       (LineSegment (ClosedLineSegment, LineSegment))
import           Data.Geometry.PlanarSubdivision (PolygonFaceData (..))
import           Data.Geometry.Point             (Point, ccw, pattern CCW, pattern CW)
import           Data.Geometry.Polygon
import           Data.Intersection
import           Data.List                       (sortOn, (\\))
import           Data.Maybe                      (fromMaybe)
import           Data.PlanarGraph                (PlanarGraph)
import qualified Data.PlanarGraph                as Graph
import           Data.PlaneGraph                 (FaceId (..), PlaneGraph, VertexData (..),
                                                  VertexId, VertexId', dual, graph, incidentEdges,
                                                  leftFace, vertices)
import qualified Data.PlaneGraph                 as PlaneGraph
import           Data.Proxy
import           Data.Tree                       (Tree (Node))
import qualified Data.Vector                     as V
import qualified Data.Vector.Circular            as CV
import qualified Data.Vector.Circular.Util       as CV
import           Data.Vector.Unboxed             (Vector)
import qualified Data.Vector.Unboxed             as VU
import           Data.Vinyl
import           Data.Vinyl.CoRec

{-
type AbsOffset = Int

data TriangulatedPolygon t p r = TriangulatedPolygon
  { triangulatedMap   :: Map AbsOffset (VertexId () Primal)
  , triangulatedGraph :: PlaneGraph () AbsOffset PolygonEdgeType PolygonFaceData r
  , triangulatedPolygon :: Polygon t p r
  }
-}



-- | Single-source shortest paths tree. Both keys and values are vertex offset ints.
--
--   @parentOf(i) = sssp[i]@
type SSSP = Vector Int

-- FIXME: The code for generating the dual cannot deal with offsets so
--        we're running 'unsafeFromPoints . toPoints' to reset the polygon.
--        Super silly. Please fix.
-- | \( O(n \log n) \)
triangulate :: (Ord r, Fractional r) => SimplePolygon p r -> PlaneGraph s Int PolygonEdgeType PolygonFaceData r
triangulate :: SimplePolygon p r
-> PlaneGraph s Int PolygonEdgeType PolygonFaceData r
triangulate SimplePolygon p r
p =
  let poly' :: Polygon 'Simple Int r
poly' = (Int, Polygon 'Simple Int r) -> Polygon 'Simple Int r
forall a b. (a, b) -> b
snd ((Int, Polygon 'Simple Int r) -> Polygon 'Simple Int r)
-> (Int, Polygon 'Simple Int r) -> Polygon 'Simple Int r
forall a b. (a -> b) -> a -> b
$ (Int -> p -> (Int, Int))
-> (Int -> r -> (Int, r))
-> Int
-> SimplePolygon p r
-> (Int, Polygon 'Simple Int r)
forall (t :: * -> * -> *) a b c d e.
Bitraversable t =>
(a -> b -> (a, c))
-> (a -> d -> (a, e)) -> a -> t b d -> (a, t c e)
bimapAccumL (\Int
a p
_ -> (Int
aInt -> Int -> Int
forall a. Num a => a -> a -> a
+Int
1,Int
a)) (,) Int
0 (SimplePolygon p r -> (Int, Polygon 'Simple Int r))
-> SimplePolygon p r -> (Int, Polygon 'Simple Int r)
forall a b. (a -> b) -> a -> b
$ [Point 2 r :+ p] -> SimplePolygon p r
forall r p. [Point 2 r :+ p] -> SimplePolygon p r
unsafeFromPoints ([Point 2 r :+ p] -> SimplePolygon p r)
-> [Point 2 r :+ p] -> SimplePolygon p r
forall a b. (a -> b) -> a -> b
$ SimplePolygon p r -> [Point 2 r :+ p]
forall (t :: PolygonType) p r. Polygon t p r -> [Point 2 r :+ p]
toPoints SimplePolygon p r
p
  in Proxy s
-> Polygon 'Simple Int r
-> PlaneGraph s Int PolygonEdgeType PolygonFaceData r
forall k r (proxy :: k -> *) (s :: k) (t :: PolygonType) p.
(Ord r, Fractional r) =>
proxy s
-> Polygon t p r
-> PlaneGraph s p PolygonEdgeType PolygonFaceData r
triangulate' Proxy s
forall k (t :: k). Proxy t
Proxy Polygon 'Simple Int r
poly'

-- | \( O(n) \) Single-Source shortest path.
sssp :: (Ord r, Fractional r)
  => PlaneGraph s Int PolygonEdgeType PolygonFaceData r
  -> SSSP
sssp :: PlaneGraph s Int PolygonEdgeType PolygonFaceData r -> SSSP
sssp PlaneGraph s Int PolygonEdgeType PolygonFaceData r
trig =
    Dual r -> SSSP
forall r. (Fractional r, Ord r) => Dual r -> SSSP
ssspFinger Dual r
d
  where
    Just VertexId' s
v0 = (VertexId' s, VertexData r Int) -> VertexId' s
forall a b. (a, b) -> a
fst ((VertexId' s, VertexData r Int) -> VertexId' s)
-> Maybe (VertexId' s, VertexData r Int) -> Maybe (VertexId' s)
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> ((VertexId' s, VertexData r Int) -> Bool)
-> Vector (VertexId' s, VertexData r Int)
-> Maybe (VertexId' s, VertexData r Int)
forall a. (a -> Bool) -> Vector a -> Maybe a
V.find (\(VertexId' s
_vid, VertexData Point 2 r
_ Int
idx) -> Int
idx Int -> Int -> Bool
forall a. Eq a => a -> a -> Bool
== Int
0) (PlaneGraph s Int PolygonEdgeType PolygonFaceData r
-> Vector (VertexId' s, VertexData r Int)
forall k (s :: k) v e f r.
PlaneGraph s v e f r -> Vector (VertexId' s, VertexData r v)
vertices PlaneGraph s Int PolygonEdgeType PolygonFaceData r
trig)
    v0i :: Vector (Dart s)
v0i = VertexId' s
-> PlaneGraph s Int PolygonEdgeType PolygonFaceData r
-> Vector (Dart s)
forall k (s :: k) v e f r.
VertexId' s -> PlaneGraph s v e f r -> Vector (Dart s)
incidentEdges VertexId' s
v0 PlaneGraph s Int PolygonEdgeType PolygonFaceData r
trig
    Just (FaceId VertexId s (DualOf 'Primal)
firstFace) = (FaceId s 'Primal -> Bool)
-> Vector (FaceId s 'Primal) -> Maybe (FaceId s 'Primal)
forall a. (a -> Bool) -> Vector a -> Maybe a
V.find (FaceId s 'Primal -> FaceId s 'Primal -> Bool
forall a. Eq a => a -> a -> Bool
/= VertexId s (DualOf 'Primal) -> FaceId s 'Primal
forall k (s :: k) (w :: World). VertexId s (DualOf w) -> FaceId s w
FaceId VertexId s (DualOf 'Primal)
outer) (Vector (FaceId s 'Primal) -> Maybe (FaceId s 'Primal))
-> Vector (FaceId s 'Primal) -> Maybe (FaceId s 'Primal)
forall a b. (a -> b) -> a -> b
$ (Dart s -> FaceId s 'Primal)
-> Vector (Dart s) -> Vector (FaceId s 'Primal)
forall a b. (a -> b) -> Vector a -> Vector b
V.map (Dart s
-> PlaneGraph s Int PolygonEdgeType PolygonFaceData r
-> FaceId s 'Primal
forall k (s :: k) v e f r.
Dart s -> PlaneGraph s v e f r -> FaceId' s
`leftFace` PlaneGraph s Int PolygonEdgeType PolygonFaceData r
trig) Vector (Dart s)
v0i
    FaceId VertexId s (DualOf 'Primal)
outer = PlaneGraph s Int PolygonEdgeType PolygonFaceData r
-> FaceId s 'Primal
forall k r (s :: k) v e f.
(Ord r, Fractional r) =>
PlaneGraph s v e f r -> FaceId' s
PlaneGraph.outerFaceId PlaneGraph s Int PolygonEdgeType PolygonFaceData r
trig
    dualGraph :: PlanarGraph
  s 'Dual PolygonFaceData PolygonEdgeType (VertexData r Int)
dualGraph = PlaneGraph s Int PolygonEdgeType PolygonFaceData r
trigPlaneGraph s Int PolygonEdgeType PolygonFaceData r
-> Getting
     (PlanarGraph
        s 'Dual PolygonFaceData PolygonEdgeType (VertexData r Int))
     (PlaneGraph s Int PolygonEdgeType PolygonFaceData r)
     (PlanarGraph
        s 'Dual PolygonFaceData PolygonEdgeType (VertexData r Int))
-> PlanarGraph
     s 'Dual PolygonFaceData PolygonEdgeType (VertexData r Int)
forall s a. s -> Getting a s a -> a
^.(PlanarGraph
   s 'Primal (VertexData r Int) PolygonEdgeType PolygonFaceData
 -> Const
      (PlanarGraph
         s 'Dual PolygonFaceData PolygonEdgeType (VertexData r Int))
      (PlanarGraph
         s 'Primal (VertexData r Int) PolygonEdgeType PolygonFaceData))
-> PlaneGraph s Int PolygonEdgeType PolygonFaceData r
-> Const
     (PlanarGraph
        s 'Dual PolygonFaceData PolygonEdgeType (VertexData r Int))
     (PlaneGraph s Int PolygonEdgeType PolygonFaceData r)
forall k1 (s1 :: k1) v1 e1 f1 r1 k2 (s2 :: k2) v2 e2 f2 r2.
Iso
  (PlaneGraph s1 v1 e1 f1 r1)
  (PlaneGraph s2 v2 e2 f2 r2)
  (PlanarGraph s1 'Primal (VertexData r1 v1) e1 f1)
  (PlanarGraph s2 'Primal (VertexData r2 v2) e2 f2)
graph((PlanarGraph
    s 'Primal (VertexData r Int) PolygonEdgeType PolygonFaceData
  -> Const
       (PlanarGraph
          s 'Dual PolygonFaceData PolygonEdgeType (VertexData r Int))
       (PlanarGraph
          s 'Primal (VertexData r Int) PolygonEdgeType PolygonFaceData))
 -> PlaneGraph s Int PolygonEdgeType PolygonFaceData r
 -> Const
      (PlanarGraph
         s 'Dual PolygonFaceData PolygonEdgeType (VertexData r Int))
      (PlaneGraph s Int PolygonEdgeType PolygonFaceData r))
-> ((PlanarGraph
       s 'Dual PolygonFaceData PolygonEdgeType (VertexData r Int)
     -> Const
          (PlanarGraph
             s 'Dual PolygonFaceData PolygonEdgeType (VertexData r Int))
          (PlanarGraph
             s 'Dual PolygonFaceData PolygonEdgeType (VertexData r Int)))
    -> PlanarGraph
         s 'Primal (VertexData r Int) PolygonEdgeType PolygonFaceData
    -> Const
         (PlanarGraph
            s 'Dual PolygonFaceData PolygonEdgeType (VertexData r Int))
         (PlanarGraph
            s 'Primal (VertexData r Int) PolygonEdgeType PolygonFaceData))
-> Getting
     (PlanarGraph
        s 'Dual PolygonFaceData PolygonEdgeType (VertexData r Int))
     (PlaneGraph s Int PolygonEdgeType PolygonFaceData r)
     (PlanarGraph
        s 'Dual PolygonFaceData PolygonEdgeType (VertexData r Int))
forall b c a. (b -> c) -> (a -> b) -> a -> c
.(PlanarGraph
   s 'Dual PolygonFaceData PolygonEdgeType (VertexData r Int)
 -> Const
      (PlanarGraph
         s 'Dual PolygonFaceData PolygonEdgeType (VertexData r Int))
      (PlanarGraph
         s 'Dual PolygonFaceData PolygonEdgeType (VertexData r Int)))
-> PlanarGraph
     s 'Primal (VertexData r Int) PolygonEdgeType PolygonFaceData
-> Const
     (PlanarGraph
        s 'Dual PolygonFaceData PolygonEdgeType (VertexData r Int))
     (PlanarGraph
        s 'Primal (VertexData r Int) PolygonEdgeType PolygonFaceData)
forall k (s :: k) (w :: World) v e f.
Getter (PlanarGraph s w v e f) (PlanarGraph s (DualOf w) f e v)
dual
    dualTree' :: Tree (VertexId s 'Dual)
dualTree' = AdjacencyLists s 'Dual
-> VertexId s 'Dual -> Tree (VertexId s 'Dual)
forall k (s :: k) (w :: World).
AdjacencyLists s w -> VertexId s w -> Tree (VertexId s w)
dfs' (([VertexId s 'Dual] -> [VertexId s 'Dual])
-> AdjacencyLists s 'Dual -> AdjacencyLists s 'Dual
forall a b. (a -> b) -> Vector a -> Vector b
V.map ((VertexId s 'Dual -> Bool)
-> [VertexId s 'Dual] -> [VertexId s 'Dual]
forall a. (a -> Bool) -> [a] -> [a]
filter (VertexId s 'Dual -> VertexId s 'Dual -> Bool
forall a. Eq a => a -> a -> Bool
/= VertexId s 'Dual
VertexId s (DualOf 'Primal)
outer)) (AdjacencyLists s 'Dual -> AdjacencyLists s 'Dual)
-> AdjacencyLists s 'Dual -> AdjacencyLists s 'Dual
forall a b. (a -> b) -> a -> b
$ PlanarGraph
  s 'Dual PolygonFaceData PolygonEdgeType (VertexData r Int)
-> AdjacencyLists s 'Dual
forall k (s :: k) (w :: World) v e f.
PlanarGraph s w v e f -> AdjacencyLists s w
adjacencyLists PlanarGraph
  s 'Dual PolygonFaceData PolygonEdgeType (VertexData r Int)
dualGraph) VertexId s 'Dual
VertexId s (DualOf 'Primal)
firstFace
    dualVS :: Tree (Vector (VertexId' s))
dualVS = (VertexId s 'Dual -> Vector (VertexId' s))
-> Tree (VertexId s 'Dual) -> Tree (Vector (VertexId' s))
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
fmap (\VertexId s 'Dual
v -> Vector (VertexId' s) -> Vector (VertexId' s)
toCCW (Vector (VertexId' s) -> Vector (VertexId' s))
-> Vector (VertexId' s) -> Vector (VertexId' s)
forall a b. (a -> b) -> a -> b
$ FaceId s 'Primal
-> PlaneGraph s Int PolygonEdgeType PolygonFaceData r
-> Vector (VertexId' s)
forall k (s :: k) v e f r.
FaceId' s -> PlaneGraph s v e f r -> Vector (VertexId' s)
PlaneGraph.boundaryVertices (VertexId s (DualOf 'Primal) -> FaceId s 'Primal
forall k (s :: k) (w :: World). VertexId s (DualOf w) -> FaceId s w
FaceId VertexId s 'Dual
VertexId s (DualOf 'Primal)
v) PlaneGraph s Int PolygonEdgeType PolygonFaceData r
trig) Tree (VertexId s 'Dual)
dualTree'
    trigTree :: Tree (Index r, Index r, Index r)
trigTree = PlaneGraph s Int PolygonEdgeType PolygonFaceData r
-> Tree (Vector (VertexId' s)) -> Tree (Index r, Index r, Index r)
forall k (s :: k) r.
PlaneGraph s Int PolygonEdgeType PolygonFaceData r
-> Tree (Vector (VertexId' s)) -> Tree (Index r, Index r, Index r)
toTrigTree PlaneGraph s Int PolygonEdgeType PolygonFaceData r
trig Tree (Vector (VertexId' s))
dualVS
    d :: Dual r
d = Tree (Index r, Index r, Index r) -> Dual r
forall r. Tree (Index r, Index r, Index r) -> Dual r
mkDual Tree (Index r, Index r, Index r)
trigTree

    toCCW :: Vector (VertexId' s) -> Vector (VertexId' s)
toCCW Vector (VertexId' s)
v =
      let cv :: CircularVector (VertexId' s)
cv = CircularVector (VertexId' s) -> CircularVector (VertexId' s)
forall a. CircularVector a -> CircularVector a
CV.reverse (CircularVector (VertexId' s) -> CircularVector (VertexId' s))
-> CircularVector (VertexId' s) -> CircularVector (VertexId' s)
forall a b. (a -> b) -> a -> b
$ Vector (VertexId' s) -> CircularVector (VertexId' s)
forall a. Vector a -> CircularVector a
CV.unsafeFromVector Vector (VertexId' s)
v
      in CircularVector (VertexId' s) -> Vector (VertexId' s)
forall a. CircularVector a -> Vector a
CV.toVector (CircularVector (VertexId' s) -> Vector (VertexId' s))
-> CircularVector (VertexId' s) -> Vector (VertexId' s)
forall a b. (a -> b) -> a -> b
$ CircularVector (VertexId' s)
-> Maybe (CircularVector (VertexId' s))
-> CircularVector (VertexId' s)
forall a. a -> Maybe a -> a
fromMaybe CircularVector (VertexId' s)
cv (Maybe (CircularVector (VertexId' s))
 -> CircularVector (VertexId' s))
-> Maybe (CircularVector (VertexId' s))
-> CircularVector (VertexId' s)
forall a b. (a -> b) -> a -> b
$ (VertexId' s -> Bool)
-> CircularVector (VertexId' s)
-> Maybe (CircularVector (VertexId' s))
forall a.
(a -> Bool) -> CircularVector a -> Maybe (CircularVector a)
CV.findRotateTo (VertexId' s -> VertexId' s -> Bool
forall a. Eq a => a -> a -> Bool
== VertexId' s
v0) CircularVector (VertexId' s)
cv

{-
1. Find the starting face.
-}
visibilitySensitive :: forall s r. (Ord r, Fractional r, Show r)
  => PlaneGraph s Int PolygonEdgeType PolygonFaceData r
  -> SimplePolygon () r
visibilitySensitive :: PlaneGraph s Int PolygonEdgeType PolygonFaceData r
-> SimplePolygon () r
visibilitySensitive = [Point 2 r :+ ()] -> SimplePolygon () r
forall p r. (Eq r, Num r) => [Point 2 r :+ p] -> SimplePolygon p r
fromPoints ([Point 2 r :+ ()] -> SimplePolygon () r)
-> (PlaneGraph s Int PolygonEdgeType PolygonFaceData r
    -> [Point 2 r :+ ()])
-> PlaneGraph s Int PolygonEdgeType PolygonFaceData r
-> SimplePolygon () r
forall b c a. (b -> c) -> (a -> b) -> a -> c
. (Point 2 r -> Point 2 r :+ ()) -> [Point 2 r] -> [Point 2 r :+ ()]
forall a b. (a -> b) -> [a] -> [b]
map Point 2 r -> Point 2 r :+ ()
forall a. a -> a :+ ()
ext ([Point 2 r] -> [Point 2 r :+ ()])
-> (PlaneGraph s Int PolygonEdgeType PolygonFaceData r
    -> [Point 2 r])
-> PlaneGraph s Int PolygonEdgeType PolygonFaceData r
-> [Point 2 r :+ ()]
forall b c a. (b -> c) -> (a -> b) -> a -> c
. [Either (Int, Int, Int) (Point 2 r)] -> [Point 2 r]
forall a b. [Either a b] -> [b]
rights ([Either (Int, Int, Int) (Point 2 r)] -> [Point 2 r])
-> (PlaneGraph s Int PolygonEdgeType PolygonFaceData r
    -> [Either (Int, Int, Int) (Point 2 r)])
-> PlaneGraph s Int PolygonEdgeType PolygonFaceData r
-> [Point 2 r]
forall b c a. (b -> c) -> (a -> b) -> a -> c
. Dual r -> [Either (Int, Int, Int) (Point 2 r)]
forall r.
(Fractional r, Ord r, Show r) =>
Dual r -> [Either (Int, Int, Int) (Point 2 r)]
visibilityFinger (Dual r -> [Either (Int, Int, Int) (Point 2 r)])
-> (PlaneGraph s Int PolygonEdgeType PolygonFaceData r -> Dual r)
-> PlaneGraph s Int PolygonEdgeType PolygonFaceData r
-> [Either (Int, Int, Int) (Point 2 r)]
forall b c a. (b -> c) -> (a -> b) -> a -> c
. PlaneGraph s Int PolygonEdgeType PolygonFaceData r -> Dual r
forall k (s :: k) r.
(Ord r, Fractional r) =>
PlaneGraph s Int PolygonEdgeType PolygonFaceData r -> Dual r
visibilityDual


visibilityDual :: forall s r. (Ord r, Fractional r)
  => PlaneGraph s Int PolygonEdgeType PolygonFaceData r
  -> Dual r
visibilityDual :: PlaneGraph s Int PolygonEdgeType PolygonFaceData r -> Dual r
visibilityDual PlaneGraph s Int PolygonEdgeType PolygonFaceData r
trig = Dual r
d
  where
    Just VertexId' s
v0 = (VertexId' s, VertexData r Int) -> VertexId' s
forall a b. (a, b) -> a
fst ((VertexId' s, VertexData r Int) -> VertexId' s)
-> Maybe (VertexId' s, VertexData r Int) -> Maybe (VertexId' s)
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> ((VertexId' s, VertexData r Int) -> Bool)
-> Vector (VertexId' s, VertexData r Int)
-> Maybe (VertexId' s, VertexData r Int)
forall a. (a -> Bool) -> Vector a -> Maybe a
V.find (\(VertexId' s
_vid, VertexData Point 2 r
_ Int
idx) -> Int
idx Int -> Int -> Bool
forall a. Eq a => a -> a -> Bool
== Int
0) (PlaneGraph s Int PolygonEdgeType PolygonFaceData r
-> Vector (VertexId' s, VertexData r Int)
forall k (s :: k) v e f r.
PlaneGraph s v e f r -> Vector (VertexId' s, VertexData r v)
vertices PlaneGraph s Int PolygonEdgeType PolygonFaceData r
trig)
    v0i :: Vector (Dart s)
v0i = VertexId' s
-> PlaneGraph s Int PolygonEdgeType PolygonFaceData r
-> Vector (Dart s)
forall k (s :: k) v e f r.
VertexId' s -> PlaneGraph s v e f r -> Vector (Dart s)
incidentEdges VertexId' s
v0 PlaneGraph s Int PolygonEdgeType PolygonFaceData r
trig

    outer :: VertexId s Graph.Dual
    FaceId VertexId s (DualOf 'Primal)
outer = PlaneGraph s Int PolygonEdgeType PolygonFaceData r
-> FaceId s 'Primal
forall k r (s :: k) v e f.
(Ord r, Fractional r) =>
PlaneGraph s v e f r -> FaceId' s
PlaneGraph.outerFaceId PlaneGraph s Int PolygonEdgeType PolygonFaceData r
trig

    firstFace :: VertexId s Graph.Dual
    Just (FaceId VertexId s (DualOf 'Primal)
firstFace) = (FaceId s 'Primal -> Bool)
-> Vector (FaceId s 'Primal) -> Maybe (FaceId s 'Primal)
forall a. (a -> Bool) -> Vector a -> Maybe a
V.find (FaceId s 'Primal -> FaceId s 'Primal -> Bool
forall a. Eq a => a -> a -> Bool
/= VertexId s (DualOf 'Primal) -> FaceId s 'Primal
forall k (s :: k) (w :: World). VertexId s (DualOf w) -> FaceId s w
FaceId VertexId s 'Dual
VertexId s (DualOf 'Primal)
outer) (Vector (FaceId s 'Primal) -> Maybe (FaceId s 'Primal))
-> Vector (FaceId s 'Primal) -> Maybe (FaceId s 'Primal)
forall a b. (a -> b) -> a -> b
$ (Dart s -> FaceId s 'Primal)
-> Vector (Dart s) -> Vector (FaceId s 'Primal)
forall a b. (a -> b) -> Vector a -> Vector b
V.map (Dart s
-> PlaneGraph s Int PolygonEdgeType PolygonFaceData r
-> FaceId s 'Primal
forall k (s :: k) v e f r.
Dart s -> PlaneGraph s v e f r -> FaceId' s
`leftFace` PlaneGraph s Int PolygonEdgeType PolygonFaceData r
trig) Vector (Dart s)
v0i

    dualGraph :: PlanarGraph s Graph.Dual PolygonFaceData PolygonEdgeType (VertexData r Int)
    dualGraph :: PlanarGraph
  s 'Dual PolygonFaceData PolygonEdgeType (VertexData r Int)
dualGraph = PlaneGraph s Int PolygonEdgeType PolygonFaceData r
trigPlaneGraph s Int PolygonEdgeType PolygonFaceData r
-> Getting
     (PlanarGraph
        s 'Dual PolygonFaceData PolygonEdgeType (VertexData r Int))
     (PlaneGraph s Int PolygonEdgeType PolygonFaceData r)
     (PlanarGraph
        s 'Dual PolygonFaceData PolygonEdgeType (VertexData r Int))
-> PlanarGraph
     s 'Dual PolygonFaceData PolygonEdgeType (VertexData r Int)
forall s a. s -> Getting a s a -> a
^.(PlanarGraph
   s 'Primal (VertexData r Int) PolygonEdgeType PolygonFaceData
 -> Const
      (PlanarGraph
         s 'Dual PolygonFaceData PolygonEdgeType (VertexData r Int))
      (PlanarGraph
         s 'Primal (VertexData r Int) PolygonEdgeType PolygonFaceData))
-> PlaneGraph s Int PolygonEdgeType PolygonFaceData r
-> Const
     (PlanarGraph
        s 'Dual PolygonFaceData PolygonEdgeType (VertexData r Int))
     (PlaneGraph s Int PolygonEdgeType PolygonFaceData r)
forall k1 (s1 :: k1) v1 e1 f1 r1 k2 (s2 :: k2) v2 e2 f2 r2.
Iso
  (PlaneGraph s1 v1 e1 f1 r1)
  (PlaneGraph s2 v2 e2 f2 r2)
  (PlanarGraph s1 'Primal (VertexData r1 v1) e1 f1)
  (PlanarGraph s2 'Primal (VertexData r2 v2) e2 f2)
graph((PlanarGraph
    s 'Primal (VertexData r Int) PolygonEdgeType PolygonFaceData
  -> Const
       (PlanarGraph
          s 'Dual PolygonFaceData PolygonEdgeType (VertexData r Int))
       (PlanarGraph
          s 'Primal (VertexData r Int) PolygonEdgeType PolygonFaceData))
 -> PlaneGraph s Int PolygonEdgeType PolygonFaceData r
 -> Const
      (PlanarGraph
         s 'Dual PolygonFaceData PolygonEdgeType (VertexData r Int))
      (PlaneGraph s Int PolygonEdgeType PolygonFaceData r))
-> ((PlanarGraph
       s 'Dual PolygonFaceData PolygonEdgeType (VertexData r Int)
     -> Const
          (PlanarGraph
             s 'Dual PolygonFaceData PolygonEdgeType (VertexData r Int))
          (PlanarGraph
             s 'Dual PolygonFaceData PolygonEdgeType (VertexData r Int)))
    -> PlanarGraph
         s 'Primal (VertexData r Int) PolygonEdgeType PolygonFaceData
    -> Const
         (PlanarGraph
            s 'Dual PolygonFaceData PolygonEdgeType (VertexData r Int))
         (PlanarGraph
            s 'Primal (VertexData r Int) PolygonEdgeType PolygonFaceData))
-> Getting
     (PlanarGraph
        s 'Dual PolygonFaceData PolygonEdgeType (VertexData r Int))
     (PlaneGraph s Int PolygonEdgeType PolygonFaceData r)
     (PlanarGraph
        s 'Dual PolygonFaceData PolygonEdgeType (VertexData r Int))
forall b c a. (b -> c) -> (a -> b) -> a -> c
.(PlanarGraph
   s 'Dual PolygonFaceData PolygonEdgeType (VertexData r Int)
 -> Const
      (PlanarGraph
         s 'Dual PolygonFaceData PolygonEdgeType (VertexData r Int))
      (PlanarGraph
         s 'Dual PolygonFaceData PolygonEdgeType (VertexData r Int)))
-> PlanarGraph
     s 'Primal (VertexData r Int) PolygonEdgeType PolygonFaceData
-> Const
     (PlanarGraph
        s 'Dual PolygonFaceData PolygonEdgeType (VertexData r Int))
     (PlanarGraph
        s 'Primal (VertexData r Int) PolygonEdgeType PolygonFaceData)
forall k (s :: k) (w :: World) v e f.
Getter (PlanarGraph s w v e f) (PlanarGraph s (DualOf w) f e v)
dual

    dualTree' :: Tree (VertexId s Graph.Dual)
    dualTree' :: Tree (VertexId s 'Dual)
dualTree' = (VertexId s 'Dual -> [VertexId s 'Dual])
-> VertexId s 'Dual -> Tree (VertexId s 'Dual)
forall k (s :: k) (w :: World).
(VertexId s w -> [VertexId s w])
-> VertexId s w -> Tree (VertexId s w)
dfsSensitive VertexId s 'Dual -> [VertexId s 'Dual]
neigh VertexId s 'Dual
firstFace

    neigh :: VertexId s Graph.Dual -> [VertexId s Graph.Dual]
    neigh :: VertexId s 'Dual -> [VertexId s 'Dual]
neigh VertexId s 'Dual
v = Vector (VertexId s 'Dual) -> [VertexId s 'Dual]
forall a. Vector a -> [a]
V.toList (Vector (VertexId s 'Dual) -> [VertexId s 'Dual])
-> Vector (VertexId s 'Dual) -> [VertexId s 'Dual]
forall a b. (a -> b) -> a -> b
$ (VertexId s 'Dual -> Bool)
-> Vector (VertexId s 'Dual) -> Vector (VertexId s 'Dual)
forall a. (a -> Bool) -> Vector a -> Vector a
V.filter (VertexId s 'Dual -> VertexId s 'Dual -> Bool
forall a. Eq a => a -> a -> Bool
/=VertexId s 'Dual
outer) (Vector (VertexId s 'Dual) -> Vector (VertexId s 'Dual))
-> Vector (VertexId s 'Dual) -> Vector (VertexId s 'Dual)
forall a b. (a -> b) -> a -> b
$ VertexId s 'Dual
-> PlanarGraph
     s 'Dual PolygonFaceData PolygonEdgeType (VertexData r Int)
-> Vector (VertexId s 'Dual)
forall k (s :: k) (w :: World) v e f.
VertexId s w -> PlanarGraph s w v e f -> Vector (VertexId s w)
Graph.neighboursOf VertexId s 'Dual
v PlanarGraph
  s 'Dual PolygonFaceData PolygonEdgeType (VertexData r Int)
dualGraph

    dualVS :: Tree (V.Vector (VertexId' s))
    dualVS :: Tree (Vector (VertexId' s))
dualVS = (VertexId s 'Dual -> Vector (VertexId' s))
-> Tree (VertexId s 'Dual) -> Tree (Vector (VertexId' s))
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
fmap (\VertexId s 'Dual
v -> Vector (VertexId' s) -> Vector (VertexId' s)
toCCW (Vector (VertexId' s) -> Vector (VertexId' s))
-> Vector (VertexId' s) -> Vector (VertexId' s)
forall a b. (a -> b) -> a -> b
$ FaceId s 'Primal
-> PlaneGraph s Int PolygonEdgeType PolygonFaceData r
-> Vector (VertexId' s)
forall k (s :: k) v e f r.
FaceId' s -> PlaneGraph s v e f r -> Vector (VertexId' s)
PlaneGraph.boundaryVertices (VertexId s (DualOf 'Primal) -> FaceId s 'Primal
forall k (s :: k) (w :: World). VertexId s (DualOf w) -> FaceId s w
FaceId VertexId s 'Dual
VertexId s (DualOf 'Primal)
v) PlaneGraph s Int PolygonEdgeType PolygonFaceData r
trig) Tree (VertexId s 'Dual)
dualTree'

    trigTree :: Tree (Index r, Index r, Index r)
    trigTree :: Tree (Index r, Index r, Index r)
trigTree = PlaneGraph s Int PolygonEdgeType PolygonFaceData r
-> Tree (Vector (VertexId' s)) -> Tree (Index r, Index r, Index r)
forall k (s :: k) r.
PlaneGraph s Int PolygonEdgeType PolygonFaceData r
-> Tree (Vector (VertexId' s)) -> Tree (Index r, Index r, Index r)
toTrigTree PlaneGraph s Int PolygonEdgeType PolygonFaceData r
trig Tree (Vector (VertexId' s))
dualVS

    d :: Dual r
    d :: Dual r
d = Tree (Index r, Index r, Index r) -> Dual r
forall r. Tree (Index r, Index r, Index r) -> Dual r
mkDual Tree (Index r, Index r, Index r)
trigTree

    toCCW :: Vector (VertexId' s) -> Vector (VertexId' s)
toCCW Vector (VertexId' s)
v =
      let cv :: CircularVector (VertexId' s)
cv = CircularVector (VertexId' s) -> CircularVector (VertexId' s)
forall a. CircularVector a -> CircularVector a
CV.reverse (CircularVector (VertexId' s) -> CircularVector (VertexId' s))
-> CircularVector (VertexId' s) -> CircularVector (VertexId' s)
forall a b. (a -> b) -> a -> b
$ Vector (VertexId' s) -> CircularVector (VertexId' s)
forall a. Vector a -> CircularVector a
CV.unsafeFromVector Vector (VertexId' s)
v
      in CircularVector (VertexId' s) -> Vector (VertexId' s)
forall a. CircularVector a -> Vector a
CV.toVector (CircularVector (VertexId' s) -> Vector (VertexId' s))
-> CircularVector (VertexId' s) -> Vector (VertexId' s)
forall a b. (a -> b) -> a -> b
$ CircularVector (VertexId' s)
-> Maybe (CircularVector (VertexId' s))
-> CircularVector (VertexId' s)
forall a. a -> Maybe a -> a
fromMaybe CircularVector (VertexId' s)
cv (Maybe (CircularVector (VertexId' s))
 -> CircularVector (VertexId' s))
-> Maybe (CircularVector (VertexId' s))
-> CircularVector (VertexId' s)
forall a b. (a -> b) -> a -> b
$ (VertexId' s -> Bool)
-> CircularVector (VertexId' s)
-> Maybe (CircularVector (VertexId' s))
forall a.
(a -> Bool) -> CircularVector a -> Maybe (CircularVector a)
CV.findRotateTo (VertexId' s -> VertexId' s -> Bool
forall a. Eq a => a -> a -> Bool
== VertexId' s
v0) CircularVector (VertexId' s)
cv



visibilityFinger :: forall r. (Fractional r, Ord r, Show r) => Dual r -> [Either (Int, Int, Int) (Point 2 r)]
visibilityFinger :: Dual r -> [Either (Int, Int, Int) (Point 2 r)]
visibilityFinger Dual r
d =
    case Dual r
d of
      Dual (Index r
a,Index r
b,Index r
c) DualTree r
ab DualTree r
bc DualTree r
ca ->
        (Int, Int, Int) -> Either (Int, Int, Int) (Point 2 r)
forall a b. a -> Either a b
Left (Index r -> Int
forall r. Index r -> Int
indexExtra Index r
a, Index r -> Int
forall r. Index r -> Int
indexExtra Index r
b, Index r -> Int
forall r. Index r -> Int
indexExtra Index r
c) Either (Int, Int, Int) (Point 2 r)
-> [Either (Int, Int, Int) (Point 2 r)]
-> [Either (Int, Int, Int) (Point 2 r)]
forall a. a -> [a] -> [a]
:
        Funnel r -> DualTree r -> [Either (Int, Int, Int) (Point 2 r)]
forall r.
(Ord r, Fractional r) =>
Funnel r -> DualTree r -> [Either (Int, Int, Int) (Point 2 r)]
worker (Chain r -> Index r -> Chain r -> Funnel r
forall r. Chain r -> Index r -> Chain r -> Funnel r
Funnel (Index r -> Chain r
forall v a. Measured v a => a -> FingerTree v a
F.singleton Index r
b) Index r
a Chain r
forall v a. Measured v a => FingerTree v a
F.empty) DualTree r
ab [Either (Int, Int, Int) (Point 2 r)]
-> [Either (Int, Int, Int) (Point 2 r)]
-> [Either (Int, Int, Int) (Point 2 r)]
forall a. [a] -> [a] -> [a]
++
        Funnel r -> DualTree r -> [Either (Int, Int, Int) (Point 2 r)]
forall r.
(Ord r, Fractional r) =>
Funnel r -> DualTree r -> [Either (Int, Int, Int) (Point 2 r)]
worker (Chain r -> Index r -> Chain r -> Funnel r
forall r. Chain r -> Index r -> Chain r -> Funnel r
Funnel (Index r -> Chain r
forall v a. Measured v a => a -> FingerTree v a
F.singleton Index r
c) Index r
a (Index r -> Chain r
forall v a. Measured v a => a -> FingerTree v a
F.singleton Index r
b)) DualTree r
bc [Either (Int, Int, Int) (Point 2 r)]
-> [Either (Int, Int, Int) (Point 2 r)]
-> [Either (Int, Int, Int) (Point 2 r)]
forall a. [a] -> [a] -> [a]
++
        Funnel r -> DualTree r -> [Either (Int, Int, Int) (Point 2 r)]
forall r.
(Ord r, Fractional r) =>
Funnel r -> DualTree r -> [Either (Int, Int, Int) (Point 2 r)]
worker (Chain r -> Index r -> Chain r -> Funnel r
forall r. Chain r -> Index r -> Chain r -> Funnel r
Funnel Chain r
forall v a. Measured v a => FingerTree v a
F.empty Index r
a (Index r -> Chain r
forall v a. Measured v a => a -> FingerTree v a
F.singleton Index r
c)) DualTree r
ca
  where
    -- Final edge is the leftmost of each funnel.
    -- The most visible are the rightmost of each funnel.
    -- Cut line segment.
    worker :: Funnel r -> DualTree r -> [Either (Int, Int, Int) (Point 2 r)]
worker Funnel r
f DualTree r
EmptyDual =
      let edgeA :: Point 2 r
edgeA = Index r -> Point 2 r
forall r. Index r -> Point 2 r
ringAccess (Index r -> Point 2 r) -> Index r -> Point 2 r
forall a b. (a -> b) -> a -> b
$ Funnel r -> Index r
forall r. Funnel r -> Index r
funnelRightTop Funnel r
f
          edgeB :: Point 2 r
edgeB = Index r -> Point 2 r
forall r. Index r -> Point 2 r
ringAccess (Index r -> Point 2 r) -> Index r -> Point 2 r
forall a b. (a -> b) -> a -> b
$ Funnel r -> Index r
forall r. Funnel r -> Index r
funnelLeftTop Funnel r
f
          edge :: LineSegment 2 () r
edge = (Point 2 r :+ ()) -> (Point 2 r :+ ()) -> LineSegment 2 () r
forall (d :: Nat) r p.
(Point d r :+ p) -> (Point d r :+ p) -> LineSegment d p r
ClosedLineSegment (Point 2 r -> Point 2 r :+ ()
forall a. a -> a :+ ()
ext Point 2 r
edgeA) (Point 2 r -> Point 2 r :+ ()
forall a. a -> a :+ ()
ext Point 2 r
edgeB)
          coneA :: Point 2 r
coneA = Index r -> Point 2 r
forall r. Index r -> Point 2 r
ringAccess (Index r -> Point 2 r) -> Index r -> Point 2 r
forall a b. (a -> b) -> a -> b
$ Funnel r -> Index r
forall r. Funnel r -> Index r
funnelRightBottom Funnel r
f
          coneB :: Point 2 r
coneB = Index r -> Point 2 r
forall r. Index r -> Point 2 r
ringAccess (Index r -> Point 2 r) -> Index r -> Point 2 r
forall a b. (a -> b) -> a -> b
$ Funnel r -> Index r
forall r. Funnel r -> Index r
funnelLeftBottom Funnel r
f
          lineA :: Line 2 r
lineA = Point 2 r -> Point 2 r -> Line 2 r
forall r (d :: Nat).
(Num r, Arity d) =>
Point d r -> Point d r -> Line d r
lineThrough (Index r -> Point 2 r
forall r. Index r -> Point 2 r
ringAccess (Index r -> Point 2 r) -> Index r -> Point 2 r
forall a b. (a -> b) -> a -> b
$ Funnel r -> Index r
forall r. Funnel r -> Index r
funnelCusp Funnel r
f) Point 2 r
coneA
          lineB :: Line 2 r
lineB = Point 2 r -> Point 2 r -> Line 2 r
forall r (d :: Nat).
(Num r, Arity d) =>
Point d r -> Point d r -> Line d r
lineThrough (Index r -> Point 2 r
forall r. Index r -> Point 2 r
ringAccess (Index r -> Point 2 r) -> Index r -> Point 2 r
forall a b. (a -> b) -> a -> b
$ Funnel r -> Index r
forall r. Funnel r -> Index r
funnelCusp Funnel r
f) Point 2 r
coneB
          -- findIntersection :: Line 2 r -> Point 2 r
          findIntersection :: Line 2 r -> Either (Int, Int, Int) (Point 2 r)
findIntersection Line 2 r
line =
            CoRec Identity '[NoIntersection, Point 2 r, LineSegment 2 () r]
-> Handlers
     '[NoIntersection, Point 2 r, LineSegment 2 () r]
     (Either (Int, Int, Int) (Point 2 r))
-> Either (Int, Int, Int) (Point 2 r)
forall (ts :: [*]) b. CoRec Identity ts -> Handlers ts b -> b
match (LineSegment 2 () r
edge LineSegment 2 () r
-> Line 2 r -> Intersection (LineSegment 2 () r) (Line 2 r)
forall g h. IsIntersectableWith g h => g -> h -> Intersection g h
`intersect` Line 2 r
line) (Handlers
   '[NoIntersection, Point 2 r, LineSegment 2 () r]
   (Either (Int, Int, Int) (Point 2 r))
 -> Either (Int, Int, Int) (Point 2 r))
-> Handlers
     '[NoIntersection, Point 2 r, LineSegment 2 () r]
     (Either (Int, Int, Int) (Point 2 r))
-> Either (Int, Int, Int) (Point 2 r)
forall a b. (a -> b) -> a -> b
$
               (NoIntersection -> Either (Int, Int, Int) (Point 2 r))
-> Handler (Either (Int, Int, Int) (Point 2 r)) NoIntersection
forall b a. (a -> b) -> Handler b a
H (\NoIntersection
NoIntersection -> [Char] -> Either (Int, Int, Int) (Point 2 r)
forall a. HasCallStack => [Char] -> a
error [Char]
"no intersection")
            Handler (Either (Int, Int, Int) (Point 2 r)) NoIntersection
-> Rec
     (Handler (Either (Int, Int, Int) (Point 2 r)))
     '[Point 2 r, LineSegment 2 () r]
-> Handlers
     '[NoIntersection, Point 2 r, LineSegment 2 () r]
     (Either (Int, Int, Int) (Point 2 r))
forall u (a :: u -> *) (r :: u) (rs :: [u]).
a r -> Rec a rs -> Rec a (r : rs)
:& (Point 2 r -> Either (Int, Int, Int) (Point 2 r))
-> Handler (Either (Int, Int, Int) (Point 2 r)) (Point 2 r)
forall b a. (a -> b) -> Handler b a
H (\Point 2 r
pt -> Point 2 r -> Either (Int, Int, Int) (Point 2 r)
forall a b. b -> Either a b
Right Point 2 r
pt)
            Handler (Either (Int, Int, Int) (Point 2 r)) (Point 2 r)
-> Rec
     (Handler (Either (Int, Int, Int) (Point 2 r)))
     '[LineSegment 2 () r]
-> Rec
     (Handler (Either (Int, Int, Int) (Point 2 r)))
     '[Point 2 r, LineSegment 2 () r]
forall u (a :: u -> *) (r :: u) (rs :: [u]).
a r -> Rec a rs -> Rec a (r : rs)
:& (LineSegment 2 () r -> Either (Int, Int, Int) (Point 2 r))
-> Handler
     (Either (Int, Int, Int) (Point 2 r)) (LineSegment 2 () r)
forall b a. (a -> b) -> Handler b a
H (\LineSegment{} -> [Char] -> Either (Int, Int, Int) (Point 2 r)
forall a. HasCallStack => [Char] -> a
error [Char]
"line intersection")
            Handler (Either (Int, Int, Int) (Point 2 r)) (LineSegment 2 () r)
-> Rec (Handler (Either (Int, Int, Int) (Point 2 r))) '[]
-> Rec
     (Handler (Either (Int, Int, Int) (Point 2 r)))
     '[LineSegment 2 () r]
forall u (a :: u -> *) (r :: u) (rs :: [u]).
a r -> Rec a rs -> Rec a (r : rs)
:& Rec (Handler (Either (Int, Int, Int) (Point 2 r))) '[]
forall u (a :: u -> *). Rec a '[]
RNil
      in [if Point 2 r
edgeA Point 2 r -> Point 2 r -> Bool
forall a. Eq a => a -> a -> Bool
== Point 2 r
coneA then Point 2 r -> Either (Int, Int, Int) (Point 2 r)
forall a b. b -> Either a b
Right Point 2 r
coneA else Line 2 r -> Either (Int, Int, Int) (Point 2 r)
findIntersection Line 2 r
lineA] [Either (Int, Int, Int) (Point 2 r)]
-> [Either (Int, Int, Int) (Point 2 r)]
-> [Either (Int, Int, Int) (Point 2 r)]
forall a. [a] -> [a] -> [a]
++
         if Point 2 r
edgeB Point 2 r -> Point 2 r -> Bool
forall a. Eq a => a -> a -> Bool
== Point 2 r
coneB then [] else [Line 2 r -> Either (Int, Int, Int) (Point 2 r)
findIntersection Line 2 r
lineB]
    worker Funnel r
f (NodeDual Index r
x DualTree r
l DualTree r
r) =
      (Int, Int, Int) -> Either (Int, Int, Int) (Point 2 r)
forall a b. a -> Either a b
Left (Index r -> Int
forall r. Index r -> Int
indexExtra (Index r -> Int) -> Index r -> Int
forall a b. (a -> b) -> a -> b
$ Index r -> Maybe (Index r) -> Index r
forall a. a -> Maybe a -> a
fromMaybe (Funnel r -> Index r
forall r. Funnel r -> Index r
funnelCusp Funnel r
f) (Maybe (Index r) -> Index r) -> Maybe (Index r) -> Index r
forall a b. (a -> b) -> a -> b
$ Chain r -> Maybe (Index r)
forall r. Chain r -> Maybe (Index r)
chainTop (Funnel r -> Chain r
forall r. Funnel r -> Chain r
funnelRight Funnel r
f)
           ,Index r -> Int
forall r. Index r -> Int
indexExtra Index r
x
           ,Index r -> Int
forall r. Index r -> Int
indexExtra (Index r -> Int) -> Index r -> Int
forall a b. (a -> b) -> a -> b
$ Index r -> Maybe (Index r) -> Index r
forall a. a -> Maybe a -> a
fromMaybe (Funnel r -> Index r
forall r. Funnel r -> Index r
funnelCusp Funnel r
f) (Maybe (Index r) -> Index r) -> Maybe (Index r) -> Index r
forall a b. (a -> b) -> a -> b
$ Chain r -> Maybe (Index r)
forall r. Chain r -> Maybe (Index r)
chainTop (Funnel r -> Chain r
forall r. Funnel r -> Chain r
funnelLeft Funnel r
f)) Either (Int, Int, Int) (Point 2 r)
-> [Either (Int, Int, Int) (Point 2 r)]
-> [Either (Int, Int, Int) (Point 2 r)]
forall a. a -> [a] -> [a]
:
      case Index r
-> Funnel r -> (Index r, Funnel r, Funnel r, SplitDirection)
forall r.
(Fractional r, Ord r) =>
Index r
-> Funnel r -> (Index r, Funnel r, Funnel r, SplitDirection)
splitFunnel Index r
x Funnel r
f of
        (Index r
_v, Funnel r
fL, Funnel r
fR, SplitDirection
dir) -> case SplitDirection
dir of
          -- 'x' is to the left of the visibility cone. Everything further to the left cannot
          -- be visible to just go right.
          SplitDirection
SplitLeft  -> Funnel r -> DualTree r -> [Either (Int, Int, Int) (Point 2 r)]
worker Funnel r
fR DualTree r
r -- assert cusp of fR == cusp of f
          -- 'x' is visible from our cusp. Add it to the output and go both to the left and right.
          SplitDirection
NoSplit    -> Funnel r -> DualTree r -> [Either (Int, Int, Int) (Point 2 r)]
worker Funnel r
fR DualTree r
r [Either (Int, Int, Int) (Point 2 r)]
-> [Either (Int, Int, Int) (Point 2 r)]
-> [Either (Int, Int, Int) (Point 2 r)]
forall a. [a] -> [a] -> [a]
++ [Point 2 r -> Either (Int, Int, Int) (Point 2 r)
forall a b. b -> Either a b
Right (Index r -> Point 2 r
forall r. Index r -> Point 2 r
ringAccess Index r
x)] [Either (Int, Int, Int) (Point 2 r)]
-> [Either (Int, Int, Int) (Point 2 r)]
-> [Either (Int, Int, Int) (Point 2 r)]
forall a. [a] -> [a] -> [a]
++ Funnel r -> DualTree r -> [Either (Int, Int, Int) (Point 2 r)]
worker Funnel r
fL DualTree r
l
          -- 'x' is to the right of the visibility cone. Everything further to the right cannot
          -- be visible to just go left.
          SplitDirection
SplitRight -> Funnel r -> DualTree r -> [Either (Int, Int, Int) (Point 2 r)]
worker Funnel r
fL DualTree r
l -- assert cusp of fL == cusp of f


--------------------------------------------------------------------------------
-- SSSP (with fingertree) implementation





data MinMax r = MinMax (Index r) (Index r) | MinMaxEmpty deriving (Int -> MinMax r -> ShowS
[MinMax r] -> ShowS
MinMax r -> [Char]
(Int -> MinMax r -> ShowS)
-> (MinMax r -> [Char]) -> ([MinMax r] -> ShowS) -> Show (MinMax r)
forall r. Int -> MinMax r -> ShowS
forall r. [MinMax r] -> ShowS
forall r. MinMax r -> [Char]
forall a.
(Int -> a -> ShowS) -> (a -> [Char]) -> ([a] -> ShowS) -> Show a
showList :: [MinMax r] -> ShowS
$cshowList :: forall r. [MinMax r] -> ShowS
show :: MinMax r -> [Char]
$cshow :: forall r. MinMax r -> [Char]
showsPrec :: Int -> MinMax r -> ShowS
$cshowsPrec :: forall r. Int -> MinMax r -> ShowS
Show)
instance Semigroup (MinMax r) where
  MinMax r
MinMaxEmpty <> :: MinMax r -> MinMax r -> MinMax r
<> MinMax r
b = MinMax r
b
  MinMax r
a <> MinMax r
MinMaxEmpty = MinMax r
a
  MinMax Index r
a Index r
_b <> MinMax Index r
_c Index r
d
    = Index r -> Index r -> MinMax r
forall r. Index r -> Index r -> MinMax r
MinMax Index r
a Index r
d
instance Monoid (MinMax r) where
  mempty :: MinMax r
mempty = MinMax r
forall r. MinMax r
MinMaxEmpty

-- Including the 'Point 2 r' here means we don't have to look it up.
-- This mattered since lookups used to be O(log n) rather than O(1).
newtype Index r = Index (Point 2 r :+ Int) -- deriving (Show)

instance Show (Index r) where
  show :: Index r -> [Char]
show = Int -> [Char]
forall a. Show a => a -> [Char]
show (Int -> [Char]) -> (Index r -> Int) -> Index r -> [Char]
forall b c a. (b -> c) -> (a -> b) -> a -> c
. Index r -> Int
forall r. Index r -> Int
indexExtra

indexExtra :: Index r -> Int
indexExtra :: Index r -> Int
indexExtra (Index Point 2 r :+ Int
p) = Point 2 r :+ Int
p(Point 2 r :+ Int) -> Getting Int (Point 2 r :+ Int) Int -> Int
forall s a. s -> Getting a s a -> a
^.Getting Int (Point 2 r :+ Int) Int
forall core extra extra'.
Lens (core :+ extra) (core :+ extra') extra extra'
extra

instance Eq (Index r) where
  Index (Point 2 r
_ :+ Int
a) == :: Index r -> Index r -> Bool
== Index (Point 2 r
_ :+ Int
b) = Int
a Int -> Int -> Bool
forall a. Eq a => a -> a -> Bool
== Int
b

type Chain r = F.FingerTree (MinMax r) (Index r)
data Funnel r = Funnel
  { Funnel r -> Chain r
funnelLeft  :: Chain r -- Left-most element is furthest away from cusp.
  , Funnel r -> Index r
funnelCusp  :: Index r
  , Funnel r -> Chain r
funnelRight :: Chain r -- Left-most element is furthest away from cusp.
  } deriving (Int -> Funnel r -> ShowS
[Funnel r] -> ShowS
Funnel r -> [Char]
(Int -> Funnel r -> ShowS)
-> (Funnel r -> [Char]) -> ([Funnel r] -> ShowS) -> Show (Funnel r)
forall r. Int -> Funnel r -> ShowS
forall r. [Funnel r] -> ShowS
forall r. Funnel r -> [Char]
forall a.
(Int -> a -> ShowS) -> (a -> [Char]) -> ([a] -> ShowS) -> Show a
showList :: [Funnel r] -> ShowS
$cshowList :: forall r. [Funnel r] -> ShowS
show :: Funnel r -> [Char]
$cshow :: forall r. Funnel r -> [Char]
showsPrec :: Int -> Funnel r -> ShowS
$cshowsPrec :: forall r. Int -> Funnel r -> ShowS
Show)

-- Left side of the funnel, furthest away from the cusp.
funnelLeftTop :: Funnel r -> Index r
funnelLeftTop :: Funnel r -> Index r
funnelLeftTop Funnel r
f = Index r -> Maybe (Index r) -> Index r
forall a. a -> Maybe a -> a
fromMaybe (Funnel r -> Index r
forall r. Funnel r -> Index r
funnelCusp Funnel r
f) (Maybe (Index r) -> Index r) -> Maybe (Index r) -> Index r
forall a b. (a -> b) -> a -> b
$ Chain r -> Maybe (Index r)
forall r. Chain r -> Maybe (Index r)
chainTop (Funnel r -> Chain r
forall r. Funnel r -> Chain r
funnelLeft Funnel r
f)

-- Left side of the funnel, closest to the cusp.
funnelLeftBottom :: Funnel r -> Index r
funnelLeftBottom :: Funnel r -> Index r
funnelLeftBottom Funnel r
f = Index r -> Maybe (Index r) -> Index r
forall a. a -> Maybe a -> a
fromMaybe (Funnel r -> Index r
forall r. Funnel r -> Index r
funnelCusp Funnel r
f) (Maybe (Index r) -> Index r) -> Maybe (Index r) -> Index r
forall a b. (a -> b) -> a -> b
$ Chain r -> Maybe (Index r)
forall r. Chain r -> Maybe (Index r)
chainBottom (Funnel r -> Chain r
forall r. Funnel r -> Chain r
funnelLeft Funnel r
f)

-- Right side of the funnel, furthest away from the cusp.
funnelRightTop :: Funnel r -> Index r
funnelRightTop :: Funnel r -> Index r
funnelRightTop Funnel r
f = Index r -> Maybe (Index r) -> Index r
forall a. a -> Maybe a -> a
fromMaybe (Funnel r -> Index r
forall r. Funnel r -> Index r
funnelCusp Funnel r
f) (Maybe (Index r) -> Index r) -> Maybe (Index r) -> Index r
forall a b. (a -> b) -> a -> b
$ Chain r -> Maybe (Index r)
forall r. Chain r -> Maybe (Index r)
chainTop (Funnel r -> Chain r
forall r. Funnel r -> Chain r
funnelRight Funnel r
f)

-- Right side of the funnel, closest to the cusp.
funnelRightBottom :: Funnel r -> Index r
funnelRightBottom :: Funnel r -> Index r
funnelRightBottom Funnel r
f = Index r -> Maybe (Index r) -> Index r
forall a. a -> Maybe a -> a
fromMaybe (Funnel r -> Index r
forall r. Funnel r -> Index r
funnelCusp Funnel r
f) (Maybe (Index r) -> Index r) -> Maybe (Index r) -> Index r
forall a b. (a -> b) -> a -> b
$ Chain r -> Maybe (Index r)
forall r. Chain r -> Maybe (Index r)
chainBottom (Funnel r -> Chain r
forall r. Funnel r -> Chain r
funnelRight Funnel r
f)

-- Element closest to the cusp.
chainBottom :: Chain r -> Maybe (Index r)
chainBottom :: Chain r -> Maybe (Index r)
chainBottom Chain r
chain = case Chain r -> ViewL (FingerTree (MinMax r)) (Index r)
forall v a.
Measured v a =>
FingerTree v a -> ViewL (FingerTree v) a
F.viewl Chain r
chain of
  ViewL (FingerTree (MinMax r)) (Index r)
F.EmptyL   -> Maybe (Index r)
forall a. Maybe a
Nothing
  Index r
elt F.:< Chain r
_ -> Index r -> Maybe (Index r)
forall a. a -> Maybe a
Just Index r
elt

-- Element furthest away from the cusp.
chainTop :: Chain r -> Maybe (Index r)
chainTop :: Chain r -> Maybe (Index r)
chainTop Chain r
chain = case Chain r -> ViewR (FingerTree (MinMax r)) (Index r)
forall v a.
Measured v a =>
FingerTree v a -> ViewR (FingerTree v) a
F.viewr Chain r
chain of
  ViewR (FingerTree (MinMax r)) (Index r)
F.EmptyR   -> Maybe (Index r)
forall a. Maybe a
Nothing
  Chain r
_ F.:> Index r
elt -> Index r -> Maybe (Index r)
forall a. a -> Maybe a
Just Index r
elt

instance F.Measured (MinMax r) (Index r) where
  measure :: Index r -> MinMax r
measure Index r
i = Index r -> Index r -> MinMax r
forall r. Index r -> Index r -> MinMax r
MinMax Index r
i Index r
i

data SplitDirection = SplitLeft | NoSplit | SplitRight
  deriving (Int -> SplitDirection -> ShowS
[SplitDirection] -> ShowS
SplitDirection -> [Char]
(Int -> SplitDirection -> ShowS)
-> (SplitDirection -> [Char])
-> ([SplitDirection] -> ShowS)
-> Show SplitDirection
forall a.
(Int -> a -> ShowS) -> (a -> [Char]) -> ([a] -> ShowS) -> Show a
showList :: [SplitDirection] -> ShowS
$cshowList :: [SplitDirection] -> ShowS
show :: SplitDirection -> [Char]
$cshow :: SplitDirection -> [Char]
showsPrec :: Int -> SplitDirection -> ShowS
$cshowsPrec :: Int -> SplitDirection -> ShowS
Show)

-- Split a funnel w.r.t. a point 'x'. There are three cases:
--   1. 'x' is visible from the cusp.
--   2. the path to 'x' hits the left side of the funnel.
--   3. the path to 'x' hits the right side of the funnel.
--
-- ********************************************************
-- Drawing guide:
--                       \     /
-- left side of funnel -> \   / <- right side of funnel
--                         \ /
--                          * <- cusp
-- ********************************************************
--
-- Case 1:
--      x
--   \     /
--    \   /
--     \ /
--      *
--
-- Case 2:
--
-- x
--   \     /
--    \   /
--     \ /
--      *
--
-- Case 3:
--
--           x
--   \     /
--    \   /
--     \ /
--      *
--
-- If 'x' is visible from the cusp, then the shortest path is a straight line and we're done.
-- If 'x' is not visible from the cusp, then we find the first point up the funnel where
-- 'x' becomes visible. We'll use a fingertree to find the point in O(log(min(n,m))). Because
-- of math, this adds up to O(n) for the entire SSSP tree.
--
-- Once we've found the first point that can see 'x', we split the funnel in two: One funnel
-- that will be used for points to the left of 'x' and one funnel for points to the right of
-- 'x'. Oh, "left" and "right" here are used to indicate branches in the dual tree.
splitFunnel :: (Fractional r, Ord r) => Index r -> Funnel r -> (Index r, Funnel r, Funnel r, SplitDirection)
splitFunnel :: Index r
-> Funnel r -> (Index r, Funnel r, Funnel r, SplitDirection)
splitFunnel Index r
x Funnel{Chain r
Index r
funnelRight :: Chain r
funnelCusp :: Index r
funnelLeft :: Chain r
funnelLeft :: forall r. Funnel r -> Chain r
funnelRight :: forall r. Funnel r -> Chain r
funnelCusp :: forall r. Funnel r -> Index r
..}
    | Bool
isOnLeftChain =
      case (Point 2 r -> Point 2 r -> Point 2 r -> Bool)
-> Chain r -> (Chain r, Index r, Chain r)
doSearch Point 2 r -> Point 2 r -> Point 2 r -> Bool
forall r.
(Ord r, Num r) =>
Point 2 r -> Point 2 r -> Point 2 r -> Bool
isRightTurn Chain r
funnelLeft of
        (Chain r
lower, Index r
t, Chain r
upper) ->
          ( Index r
t
          , Chain r -> Index r -> Chain r -> Funnel r
forall r. Chain r -> Index r -> Chain r -> Funnel r
Funnel Chain r
upper Index r
t (Index r -> Chain r
forall v a. Measured v a => a -> FingerTree v a
F.singleton Index r
x)
          , Chain r -> Index r -> Chain r -> Funnel r
forall r. Chain r -> Index r -> Chain r -> Funnel r
Funnel (Chain r
lower Chain r -> Index r -> Chain r
forall v a. Measured v a => FingerTree v a -> a -> FingerTree v a
F.|> Index r
t Chain r -> Index r -> Chain r
forall v a. Measured v a => FingerTree v a -> a -> FingerTree v a
F.|> Index r
x) Index r
funnelCusp Chain r
funnelRight
          , SplitDirection
SplitLeft)
    | Bool
isOnRightChain =
      case (Point 2 r -> Point 2 r -> Point 2 r -> Bool)
-> Chain r -> (Chain r, Index r, Chain r)
doSearch Point 2 r -> Point 2 r -> Point 2 r -> Bool
forall r.
(Ord r, Num r) =>
Point 2 r -> Point 2 r -> Point 2 r -> Bool
isLeftTurn Chain r
funnelRight of
        (Chain r
lower, Index r
t, Chain r
upper) ->
          ( Index r
t
          , Chain r -> Index r -> Chain r -> Funnel r
forall r. Chain r -> Index r -> Chain r -> Funnel r
Funnel Chain r
funnelLeft Index r
funnelCusp (Chain r
lower Chain r -> Index r -> Chain r
forall v a. Measured v a => FingerTree v a -> a -> FingerTree v a
F.|> Index r
t Chain r -> Index r -> Chain r
forall v a. Measured v a => FingerTree v a -> a -> FingerTree v a
F.|> Index r
x)
          , Chain r -> Index r -> Chain r -> Funnel r
forall r. Chain r -> Index r -> Chain r -> Funnel r
Funnel (Index r -> Chain r
forall v a. Measured v a => a -> FingerTree v a
F.singleton Index r
x) Index r
t Chain r
upper
          , SplitDirection
SplitRight)
    | Bool
otherwise =
      ( Index r
funnelCusp
      , Chain r -> Index r -> Chain r -> Funnel r
forall r. Chain r -> Index r -> Chain r -> Funnel r
Funnel Chain r
funnelLeft Index r
funnelCusp (Index r -> Chain r
forall v a. Measured v a => a -> FingerTree v a
F.singleton Index r
x)
      , Chain r -> Index r -> Chain r -> Funnel r
forall r. Chain r -> Index r -> Chain r -> Funnel r
Funnel (Index r -> Chain r
forall v a. Measured v a => a -> FingerTree v a
F.singleton Index r
x) Index r
funnelCusp Chain r
funnelRight
      , SplitDirection
NoSplit)
  where
    isOnLeftChain :: Bool
isOnLeftChain  = Bool -> Maybe Bool -> Bool
forall a. a -> Maybe a -> a
fromMaybe Bool
False (Maybe Bool -> Bool) -> Maybe Bool -> Bool
forall a b. (a -> b) -> a -> b
$
      Point 2 r -> Point 2 r -> Point 2 r -> Bool
forall r.
(Ord r, Num r) =>
Point 2 r -> Point 2 r -> Point 2 r -> Bool
isLeftTurnOrLinear Point 2 r
cuspElt (Point 2 r -> Point 2 r -> Bool)
-> Maybe (Point 2 r) -> Maybe (Point 2 r -> Bool)
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> Maybe (Point 2 r)
leftElt Maybe (Point 2 r -> Bool) -> Maybe (Point 2 r) -> Maybe Bool
forall (f :: * -> *) a b. Applicative f => f (a -> b) -> f a -> f b
<*> Point 2 r -> Maybe (Point 2 r)
forall (f :: * -> *) a. Applicative f => a -> f a
pure Point 2 r
targetElt
    isOnRightChain :: Bool
isOnRightChain = Bool -> Maybe Bool -> Bool
forall a. a -> Maybe a -> a
fromMaybe Bool
False (Maybe Bool -> Bool) -> Maybe Bool -> Bool
forall a b. (a -> b) -> a -> b
$
      Point 2 r -> Point 2 r -> Point 2 r -> Bool
forall r.
(Ord r, Num r) =>
Point 2 r -> Point 2 r -> Point 2 r -> Bool
isRightTurnOrLinear Point 2 r
cuspElt (Point 2 r -> Point 2 r -> Bool)
-> Maybe (Point 2 r) -> Maybe (Point 2 r -> Bool)
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> Maybe (Point 2 r)
rightElt Maybe (Point 2 r -> Bool) -> Maybe (Point 2 r) -> Maybe Bool
forall (f :: * -> *) a b. Applicative f => f (a -> b) -> f a -> f b
<*> Point 2 r -> Maybe (Point 2 r)
forall (f :: * -> *) a. Applicative f => a -> f a
pure Point 2 r
targetElt
    doSearch :: (Point 2 r -> Point 2 r -> Point 2 r -> Bool)
-> Chain r -> (Chain r, Index r, Chain r)
doSearch Point 2 r -> Point 2 r -> Point 2 r -> Bool
fn Chain r
chain =
      case (MinMax r -> MinMax r -> Bool)
-> Chain r -> SearchResult (MinMax r) (Index r)
forall v a.
Measured v a =>
(v -> v -> Bool) -> FingerTree v a -> SearchResult v a
F.search ((Point 2 r -> Point 2 r -> Point 2 r -> Bool)
-> MinMax r -> MinMax r -> Bool
searchChain Point 2 r -> Point 2 r -> Point 2 r -> Bool
fn) Chain r
chain of
        F.Position Chain r
lower Index r
t Chain r
upper -> (Chain r
lower, Index r
t, Chain r
upper)
        SearchResult (MinMax r) (Index r)
F.OnLeft                 -> [Char] -> (Chain r, Index r, Chain r)
forall a. HasCallStack => [Char] -> a
error [Char]
"cannot happen"
        SearchResult (MinMax r) (Index r)
F.OnRight                -> [Char] -> (Chain r, Index r, Chain r)
forall a. HasCallStack => [Char] -> a
error [Char]
"cannot happen"
        SearchResult (MinMax r) (Index r)
F.Nowhere                -> [Char] -> (Chain r, Index r, Chain r)
forall a. HasCallStack => [Char] -> a
error [Char]
"cannot happen"
    searchChain :: (Point 2 r -> Point 2 r -> Point 2 r -> Bool)
-> MinMax r -> MinMax r -> Bool
searchChain Point 2 r -> Point 2 r -> Point 2 r -> Bool
_ MinMax r
MinMaxEmpty MinMax r
_             = Bool
False
    searchChain Point 2 r -> Point 2 r -> Point 2 r -> Bool
_ MinMax r
_ MinMax r
MinMaxEmpty             = Bool
True
    searchChain Point 2 r -> Point 2 r -> Point 2 r -> Bool
check (MinMax Index r
_ Index r
l) (MinMax Index r
r Index r
_) =
      Point 2 r -> Point 2 r -> Point 2 r -> Bool
check (Index r -> Point 2 r
forall r. Index r -> Point 2 r
ringAccess Index r
l) (Index r -> Point 2 r
forall r. Index r -> Point 2 r
ringAccess Index r
r) Point 2 r
targetElt
    cuspElt :: Point 2 r
cuspElt   = Index r -> Point 2 r
forall r. Index r -> Point 2 r
ringAccess Index r
funnelCusp
    targetElt :: Point 2 r
targetElt = Index r -> Point 2 r
forall r. Index r -> Point 2 r
ringAccess Index r
x
    leftElt :: Maybe (Point 2 r)
leftElt   = Index r -> Point 2 r
forall r. Index r -> Point 2 r
ringAccess (Index r -> Point 2 r) -> Maybe (Index r) -> Maybe (Point 2 r)
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> Chain r -> Maybe (Index r)
forall r. Chain r -> Maybe (Index r)
chainBottom Chain r
funnelLeft
    rightElt :: Maybe (Point 2 r)
rightElt  = Index r -> Point 2 r
forall r. Index r -> Point 2 r
ringAccess (Index r -> Point 2 r) -> Maybe (Index r) -> Maybe (Point 2 r)
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> Chain r -> Maybe (Index r)
forall r. Chain r -> Maybe (Index r)
chainBottom Chain r
funnelRight

-- FIXME: Turning a list of pairs into a vector is incredibly inefficient.
--        Would be much faster to write directly into a mutable vector and
--        then freeze it at the end.
-- \( O(n) \)
ssspFinger :: (Fractional r, Ord r) => Dual r -> SSSP
ssspFinger :: Dual r -> SSSP
ssspFinger Dual r
d = [(Index r, Index r)] -> SSSP
forall r. [(Index r, Index r)] -> SSSP
toSSSP ([(Index r, Index r)] -> SSSP) -> [(Index r, Index r)] -> SSSP
forall a b. (a -> b) -> a -> b
$
    case Dual r
d of
      Dual (Index r
a,Index r
b,Index r
c) DualTree r
ab DualTree r
bc DualTree r
ca ->
        (Index r
a, Index r
a) (Index r, Index r) -> [(Index r, Index r)] -> [(Index r, Index r)]
forall a. a -> [a] -> [a]
:
        (Index r
b, Index r
a) (Index r, Index r) -> [(Index r, Index r)] -> [(Index r, Index r)]
forall a. a -> [a] -> [a]
:
        (Index r
c, Index r
a) (Index r, Index r) -> [(Index r, Index r)] -> [(Index r, Index r)]
forall a. a -> [a] -> [a]
:
        Index r -> Index r -> DualTree r -> [(Index r, Index r)]
forall r.
(Fractional r, Ord r) =>
Index r -> Index r -> DualTree r -> [(Index r, Index r)]
loopLeft Index r
a Index r
c DualTree r
ca [(Index r, Index r)]
-> [(Index r, Index r)] -> [(Index r, Index r)]
forall a. [a] -> [a] -> [a]
++
        Funnel r -> DualTree r -> [(Index r, Index r)]
forall r.
(Fractional r, Ord r) =>
Funnel r -> DualTree r -> [(Index r, Index r)]
worker (Chain r -> Index r -> Chain r -> Funnel r
forall r. Chain r -> Index r -> Chain r -> Funnel r
Funnel (Index r -> Chain r
forall v a. Measured v a => a -> FingerTree v a
F.singleton Index r
c) Index r
a (Index r -> Chain r
forall v a. Measured v a => a -> FingerTree v a
F.singleton Index r
b)) DualTree r
bc [(Index r, Index r)]
-> [(Index r, Index r)] -> [(Index r, Index r)]
forall a. [a] -> [a] -> [a]
++
        Index r -> Index r -> DualTree r -> [(Index r, Index r)]
forall r.
(Fractional r, Ord r) =>
Index r -> Index r -> DualTree r -> [(Index r, Index r)]
loopRight Index r
a Index r
b DualTree r
ab
  where
    toSSSP :: [(Index r,Index r)] -> SSSP
    toSSSP :: [(Index r, Index r)] -> SSSP
toSSSP [(Index r, Index r)]
lst =
      [Int] -> SSSP
forall a. Unbox a => [a] -> Vector a
VU.fromList ([Int] -> SSSP) -> ([(Int, Int)] -> [Int]) -> [(Int, Int)] -> SSSP
forall b c a. (b -> c) -> (a -> b) -> a -> c
. ((Int, Int) -> Int) -> [(Int, Int)] -> [Int]
forall a b. (a -> b) -> [a] -> [b]
map (Int, Int) -> Int
forall a b. (a, b) -> b
snd ([(Int, Int)] -> [Int])
-> ([(Int, Int)] -> [(Int, Int)]) -> [(Int, Int)] -> [Int]
forall b c a. (b -> c) -> (a -> b) -> a -> c
. ((Int, Int) -> Int) -> [(Int, Int)] -> [(Int, Int)]
forall b a. Ord b => (a -> b) -> [a] -> [a]
sortOn (Int, Int) -> Int
forall a b. (a, b) -> a
fst ([(Int, Int)] -> SSSP) -> [(Int, Int)] -> SSSP
forall a b. (a -> b) -> a -> b
$
      [ (Int
a,Int
b) | (Index (Point 2 r
_ :+ Int
a), Index (Point 2 r
_ :+ Int
b)) <- [(Index r, Index r)]
lst ]
    loopLeft :: Index r -> Index r -> DualTree r -> [(Index r, Index r)]
loopLeft Index r
a Index r
outer DualTree r
l =
      case DualTree r
l of
        DualTree r
EmptyDual -> []
        NodeDual Index r
x DualTree r
l' DualTree r
r' ->
          (Index r
x,Index r
a) (Index r, Index r) -> [(Index r, Index r)] -> [(Index r, Index r)]
forall a. a -> [a] -> [a]
:
          Funnel r -> DualTree r -> [(Index r, Index r)]
forall r.
(Fractional r, Ord r) =>
Funnel r -> DualTree r -> [(Index r, Index r)]
worker (Chain r -> Index r -> Chain r -> Funnel r
forall r. Chain r -> Index r -> Chain r -> Funnel r
Funnel (Index r -> Chain r
forall v a. Measured v a => a -> FingerTree v a
F.singleton Index r
x) Index r
a (Index r -> Chain r
forall v a. Measured v a => a -> FingerTree v a
F.singleton Index r
outer)) DualTree r
r' [(Index r, Index r)]
-> [(Index r, Index r)] -> [(Index r, Index r)]
forall a. [a] -> [a] -> [a]
++
          Index r -> Index r -> DualTree r -> [(Index r, Index r)]
loopLeft Index r
a Index r
x DualTree r
l'
    loopRight :: Index r -> Index r -> DualTree r -> [(Index r, Index r)]
loopRight Index r
a Index r
outer DualTree r
r =
      case DualTree r
r of
        DualTree r
EmptyDual -> []
        NodeDual Index r
x DualTree r
l' DualTree r
r' ->
          (Index r
x, Index r
a) (Index r, Index r) -> [(Index r, Index r)] -> [(Index r, Index r)]
forall a. a -> [a] -> [a]
:
          Funnel r -> DualTree r -> [(Index r, Index r)]
forall r.
(Fractional r, Ord r) =>
Funnel r -> DualTree r -> [(Index r, Index r)]
worker (Chain r -> Index r -> Chain r -> Funnel r
forall r. Chain r -> Index r -> Chain r -> Funnel r
Funnel (Index r -> Chain r
forall v a. Measured v a => a -> FingerTree v a
F.singleton Index r
outer) Index r
a (Index r -> Chain r
forall v a. Measured v a => a -> FingerTree v a
F.singleton Index r
x)) DualTree r
l' [(Index r, Index r)]
-> [(Index r, Index r)] -> [(Index r, Index r)]
forall a. [a] -> [a] -> [a]
++
          Index r -> Index r -> DualTree r -> [(Index r, Index r)]
loopRight Index r
a Index r
x DualTree r
r'
    worker :: Funnel r -> DualTree r -> [(Index r, Index r)]
worker Funnel r
_ DualTree r
EmptyDual = []
    worker Funnel r
f (NodeDual Index r
x DualTree r
l DualTree r
r) =
      case Index r
-> Funnel r -> (Index r, Funnel r, Funnel r, SplitDirection)
forall r.
(Fractional r, Ord r) =>
Index r
-> Funnel r -> (Index r, Funnel r, Funnel r, SplitDirection)
splitFunnel Index r
x Funnel r
f of
        (Index r
v, Funnel r
fL, Funnel r
fR, SplitDirection
_) ->
          (Index r
x, Index r
v) (Index r, Index r) -> [(Index r, Index r)] -> [(Index r, Index r)]
forall a. a -> [a] -> [a]
:
          Funnel r -> DualTree r -> [(Index r, Index r)]
worker Funnel r
fL DualTree r
l [(Index r, Index r)]
-> [(Index r, Index r)] -> [(Index r, Index r)]
forall a. [a] -> [a] -> [a]
++
          Funnel r -> DualTree r -> [(Index r, Index r)]
worker Funnel r
fR DualTree r
r


--------------------------------------------------------------------------------
-- Duals



data Dual r = Dual (Index r, Index r, Index r) -- (a,b,c)
                   (DualTree r) -- borders ab
                   (DualTree r) -- borders bc
                   (DualTree r) -- borders ca
  deriving (Int -> Dual r -> ShowS
[Dual r] -> ShowS
Dual r -> [Char]
(Int -> Dual r -> ShowS)
-> (Dual r -> [Char]) -> ([Dual r] -> ShowS) -> Show (Dual r)
forall r. Int -> Dual r -> ShowS
forall r. [Dual r] -> ShowS
forall r. Dual r -> [Char]
forall a.
(Int -> a -> ShowS) -> (a -> [Char]) -> ([a] -> ShowS) -> Show a
showList :: [Dual r] -> ShowS
$cshowList :: forall r. [Dual r] -> ShowS
show :: Dual r -> [Char]
$cshow :: forall r. Dual r -> [Char]
showsPrec :: Int -> Dual r -> ShowS
$cshowsPrec :: forall r. Int -> Dual r -> ShowS
Show)

data DualTree r
  = EmptyDual
  | NodeDual (Index r) -- axb triangle, a and b are from parent.
      (DualTree r) -- borders xb
      (DualTree r) -- borders ax
  deriving (Int -> DualTree r -> ShowS
[DualTree r] -> ShowS
DualTree r -> [Char]
(Int -> DualTree r -> ShowS)
-> (DualTree r -> [Char])
-> ([DualTree r] -> ShowS)
-> Show (DualTree r)
forall r. Int -> DualTree r -> ShowS
forall r. [DualTree r] -> ShowS
forall r. DualTree r -> [Char]
forall a.
(Int -> a -> ShowS) -> (a -> [Char]) -> ([a] -> ShowS) -> Show a
showList :: [DualTree r] -> ShowS
$cshowList :: forall r. [DualTree r] -> ShowS
show :: DualTree r -> [Char]
$cshow :: forall r. DualTree r -> [Char]
showsPrec :: Int -> DualTree r -> ShowS
$cshowsPrec :: forall r. Int -> DualTree r -> ShowS
Show)

toTrigTree :: PlaneGraph s Int PolygonEdgeType PolygonFaceData r
           -> Tree (V.Vector (VertexId' s))
           -> Tree (Index r,Index r,Index r)
toTrigTree :: PlaneGraph s Int PolygonEdgeType PolygonFaceData r
-> Tree (Vector (VertexId' s)) -> Tree (Index r, Index r, Index r)
toTrigTree PlaneGraph s Int PolygonEdgeType PolygonFaceData r
trig = (Vector (Index r) -> (Index r, Index r, Index r))
-> Tree (Vector (Index r)) -> Tree (Index r, Index r, Index r)
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
fmap Vector (Index r) -> (Index r, Index r, Index r)
forall c. Vector c -> (c, c, c)
toTrig (Tree (Vector (Index r)) -> Tree (Index r, Index r, Index r))
-> (Tree (Vector (VertexId' s)) -> Tree (Vector (Index r)))
-> Tree (Vector (VertexId' s))
-> Tree (Index r, Index r, Index r)
forall b c a. (b -> c) -> (a -> b) -> a -> c
. (Vector (VertexId' s) -> Vector (Index r))
-> Tree (Vector (VertexId' s)) -> Tree (Vector (Index r))
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
fmap ((VertexId' s -> Index r)
-> Vector (VertexId' s) -> Vector (Index r)
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
fmap VertexId' s -> Index r
toDat)
  where
    toTrig :: Vector c -> (c, c, c)
toTrig Vector c
v = case Vector c -> [c]
forall a. Vector a -> [a]
V.toList Vector c
v of
      [c
a,c
b,c
c] -> (c
a,c
b,c
c)
      [c]
_       -> [Char] -> (c, c, c)
forall a. HasCallStack => [Char] -> a
error [Char]
"Algorithms.Geometry.SSSP: Invalid triangulation."
    toDat :: VertexId' s -> Index r
toDat VertexId' s
v = (Point 2 r :+ Int) -> Index r
forall r. (Point 2 r :+ Int) -> Index r
Index ((Point 2 r :+ Int) -> Index r) -> (Point 2 r :+ Int) -> Index r
forall a b. (a -> b) -> a -> b
$ VertexData r Int -> Point 2 r :+ Int
forall r v. VertexData r v -> Point 2 r :+ v
PlaneGraph.vtxDataToExt (PlaneGraph s Int PolygonEdgeType PolygonFaceData r
trig PlaneGraph s Int PolygonEdgeType PolygonFaceData r
-> Getting
     (VertexData r Int)
     (PlaneGraph s Int PolygonEdgeType PolygonFaceData r)
     (VertexData r Int)
-> VertexData r Int
forall s a. s -> Getting a s a -> a
^. VertexId' s
-> Lens'
     (PlaneGraph s Int PolygonEdgeType PolygonFaceData r)
     (VertexData r Int)
forall k (s :: k) v e f r.
VertexId' s -> Lens' (PlaneGraph s v e f r) (VertexData r v)
PlaneGraph.vertexDataOf VertexId' s
v)

-- pp :: Show a => Tree a -> IO ()
-- pp = putStrLn . drawTree . fmap show

mkDual :: Tree (Index r,Index r,Index r) -> Dual r
mkDual :: Tree (Index r, Index r, Index r) -> Dual r
mkDual (Node (Index r
a,Index r
b,Index r
c) Forest (Index r, Index r, Index r)
forest) =
    (Index r, Index r, Index r)
-> DualTree r -> DualTree r -> DualTree r -> Dual r
forall r.
(Index r, Index r, Index r)
-> DualTree r -> DualTree r -> DualTree r -> Dual r
Dual (Index r
a, Index r
b, Index r
c)
      (Index r
-> Index r -> Forest (Index r, Index r, Index r) -> DualTree r
forall r.
Index r
-> Index r -> [Tree (Index r, Index r, Index r)] -> DualTree r
dualTree Index r
a Index r
b Forest (Index r, Index r, Index r)
forest)
      (Index r
-> Index r -> Forest (Index r, Index r, Index r) -> DualTree r
forall r.
Index r
-> Index r -> [Tree (Index r, Index r, Index r)] -> DualTree r
dualTree Index r
b Index r
c Forest (Index r, Index r, Index r)
forest)
      (Index r
-> Index r -> Forest (Index r, Index r, Index r) -> DualTree r
forall r.
Index r
-> Index r -> [Tree (Index r, Index r, Index r)] -> DualTree r
dualTree Index r
c Index r
a Forest (Index r, Index r, Index r)
forest)

dualTree :: Index r -> Index r -> [Tree (Index r,Index r,Index r)] -> DualTree r
dualTree :: Index r
-> Index r -> [Tree (Index r, Index r, Index r)] -> DualTree r
dualTree Index r
p1 Index r
p2 (Node (Index r
a,Index r
b,Index r
c) [Tree (Index r, Index r, Index r)]
sub:[Tree (Index r, Index r, Index r)]
xs) =
  case [Index r
a,Index r
b,Index r
c] [Index r] -> [Index r] -> [Index r]
forall a. Eq a => [a] -> [a] -> [a]
\\ [Index r
p1,Index r
p2] of
    [Index r
x] -> Index r -> DualTree r -> DualTree r -> DualTree r
forall r. Index r -> DualTree r -> DualTree r -> DualTree r
NodeDual Index r
x (Index r
-> Index r -> [Tree (Index r, Index r, Index r)] -> DualTree r
forall r.
Index r
-> Index r -> [Tree (Index r, Index r, Index r)] -> DualTree r
dualTree Index r
x Index r
p2 [Tree (Index r, Index r, Index r)]
sub) (Index r
-> Index r -> [Tree (Index r, Index r, Index r)] -> DualTree r
forall r.
Index r
-> Index r -> [Tree (Index r, Index r, Index r)] -> DualTree r
dualTree Index r
p1 Index r
x [Tree (Index r, Index r, Index r)]
sub)
    [Index r]
_   -> Index r
-> Index r -> [Tree (Index r, Index r, Index r)] -> DualTree r
forall r.
Index r
-> Index r -> [Tree (Index r, Index r, Index r)] -> DualTree r
dualTree Index r
p1 Index r
p2 [Tree (Index r, Index r, Index r)]
xs
dualTree Index r
_p1 Index r
_p2 [] = DualTree r
forall r. DualTree r
EmptyDual





--------------------------------------------------------------------------------
-- Helpers

ringAccess :: Index r -> Point 2 r
ringAccess :: Index r -> Point 2 r
ringAccess (Index (Point 2 r
pt :+ Int
_idx)) = Point 2 r
pt

isRightTurnOrLinear :: (Ord r, Num r) => Point 2 r -> Point 2 r -> Point 2 r -> Bool
isRightTurnOrLinear :: Point 2 r -> Point 2 r -> Point 2 r -> Bool
isRightTurnOrLinear Point 2 r
p1 Point 2 r
p2 Point 2 r
p3 = Bool -> Bool
not (Bool -> Bool) -> Bool -> Bool
forall a b. (a -> b) -> a -> b
$ Point 2 r -> Point 2 r -> Point 2 r -> Bool
forall r.
(Ord r, Num r) =>
Point 2 r -> Point 2 r -> Point 2 r -> Bool
isLeftTurn Point 2 r
p1 Point 2 r
p2 Point 2 r
p3

isLeftTurnOrLinear :: (Ord r, Num r) => Point 2 r -> Point 2 r -> Point 2 r -> Bool
isLeftTurnOrLinear :: Point 2 r -> Point 2 r -> Point 2 r -> Bool
isLeftTurnOrLinear Point 2 r
p1 Point 2 r
p2 Point 2 r
p3 = Bool -> Bool
not (Bool -> Bool) -> Bool -> Bool
forall a b. (a -> b) -> a -> b
$ Point 2 r -> Point 2 r -> Point 2 r -> Bool
forall r.
(Ord r, Num r) =>
Point 2 r -> Point 2 r -> Point 2 r -> Bool
isRightTurn Point 2 r
p1 Point 2 r
p2 Point 2 r
p3

isLeftTurn :: (Ord r, Num r) => Point 2 r -> Point 2 r -> Point 2 r -> Bool
isLeftTurn :: Point 2 r -> Point 2 r -> Point 2 r -> Bool
isLeftTurn Point 2 r
p1 Point 2 r
p2 Point 2 r
p3 =
  Point 2 r -> Point 2 r -> Point 2 r -> CCW
forall r.
(Ord r, Num r) =>
Point 2 r -> Point 2 r -> Point 2 r -> CCW
ccw Point 2 r
p1 Point 2 r
p2 Point 2 r
p3 CCW -> CCW -> Bool
forall a. Eq a => a -> a -> Bool
== CCW
CCW

isRightTurn :: (Ord r, Num r) => Point 2 r -> Point 2 r -> Point 2 r -> Bool
isRightTurn :: Point 2 r -> Point 2 r -> Point 2 r -> Bool
isRightTurn Point 2 r
p1 Point 2 r
p2 Point 2 r
p3 =
  Point 2 r -> Point 2 r -> Point 2 r -> CCW
forall r.
(Ord r, Num r) =>
Point 2 r -> Point 2 r -> Point 2 r -> CCW
ccw Point 2 r
p1 Point 2 r
p2 Point 2 r
p3 CCW -> CCW -> Bool
forall a. Eq a => a -> a -> Bool
== CCW
CW