hgeometry-0.12.0.0: src/Algorithms/Geometry/VisibilityPolygon/Lee.hs
{-# LANGUAGE TemplateHaskell #-}
--------------------------------------------------------------------------------
-- |
-- Module : Algorithms.Geometry.VisibilityPolygon.Lee
-- Copyright : (C) Frank Staals
-- License : see the LICENSE file
-- Maintainer : Frank Staals
--
-- \(O(n\log n)\) time algorithm to compute the visibility polygon of
-- a point inside a polygon (possibly containing holes) with \(n\)
-- vertices, or among a set of \(n\) disjoint segments. The alogirhtm
-- used is the the rotational sweepline algorithm by Lee, described
-- in:
--
-- D. T. Lee. Proximity and reachability in the plane. Report R-831, Dept. Elect.
-- Engrg., Univ. Illinois, Urbana, IL, 1978.
--
--------------------------------------------------------------------------------
module Algorithms.Geometry.VisibilityPolygon.Lee
( visibilityPolygon
, visibilitySweep
, VisibilityPolygon
, Definer, StarShapedPolygon
, compareAroundEndPoint
) where
import Control.Lens
import Control.Monad ((<=<))
import Data.Bifunctor (first)
import Data.Ext
import qualified Data.Foldable as F
import Data.Function (on)
import Data.Geometry.HalfLine
import Data.Geometry.Line
import Data.Geometry.LineSegment
import Data.Geometry.Point
import Data.Geometry.Polygon
import Data.Geometry.Vector
import Data.Intersection
import qualified Data.List as List
import qualified Data.List.Util as List
import Data.List.NonEmpty (NonEmpty(..))
import qualified Data.List.NonEmpty as NonEmpty
import Data.Maybe (mapMaybe, isJust)
import Data.Ord (comparing)
import Data.RealNumber.Rational
import Data.Semigroup.Foldable
import qualified Data.Set as Set
import qualified Data.Set.Util as Set
import Data.Util
import Data.Vinyl.CoRec
import Debug.Trace
type R = RealNumber 5
--------------------------------------------------------------------------------
type StarShapedPolygon p r = SimplePolygon p r
-- | Vertices of the visibility polgyon are either original vertices
-- or defined by some vertex and an edge
type Definer p e r = Either p (Point 2 r :+ p,LineSegment 2 p r :+ e)
type VisibilityPolygon p e r = StarShapedPolygon (Definer p e r) r
-- | We either insert or delete segments
data Action a = Insert a | Delete a deriving (Show,Eq,Ord)
isInsert :: Action a -> Bool
isInsert = \case
Insert _ -> True
Delete _ -> False
extract :: Action a -> a
extract = \case
Insert x -> x
Delete x -> x
-- | An event corresponds to some orientation at which the set of segments
-- intersected by the ray changes (this orientation is defined by a point)
data Event p e r = Event { _eventVtx :: Point 2 r :+ p
, _actions :: NonEmpty (Action (LineSegment 2 p r :+ e))
} deriving Show
makeLenses ''Event
-- | The status structure maintains the subset of segments currently
-- intersected by the ray that starts in the query point q, in order
-- of increasing distance along the ray.
type Status p e r = Set.Set (LineSegment 2 p r :+ e)
--------------------------------------------------------------------------------
-- | Computes the visibility polygon of a point q in a polygon with
-- \(n\) vertices.
--
-- pre: q lies strictly inside the polygon
--
-- running time: \(O(n\log n)\)
visibilityPolygon :: forall p t r. (Ord r, Fractional r)
=> Point 2 r
-> Polygon t p r
-> StarShapedPolygon (Definer p () r) r
visibilityPolygon q pg =
fromPoints . visibilitySweep v Nothing q . map ext . closedEdges $ pg
where
v = uncurry (startingDirection q) . consecutive q . polygonVertices $ pg
-- | Computes the visibility polgyon from a vertex
visibilityPolygonFromVertex :: forall p t r. (Ord r, Fractional r, Show r, Show p)
=> Polygon t p r
-> Int -- ^ from the i^th vertex on the outer boundary
-> VisibilityPolygon p () r
visibilityPolygonFromVertex pg i =
fromPoints . visibilitySweep sv (Just w) v . map ext $ segs
where
(v :+ _) = pg^.outerVertex i
(w :+ _) = pg^.outerVertex (i-1)
(u :+ _) = pg^.outerVertex (i+1)
-- rotates the polygon so that u becomes the focus, and gets all
-- other vertices. Takes the next CCW vertex around v, starting
-- form the direction indicated by v.
z = let u' :| rest = traceShowIdWith "vertices"
$ polygonVertices $ pg&outerBoundary %~ rotateRight (i+1)
in traceShowIdWith "z" $ consecutiveFrom (u .-. v) v (List.init rest)
-- the last vertex in rest is v; so kill that
sv = startingDirection v u z
segs = map (first (^._2))
. filter (not . incidentTo i)
. closedEdges $ numberVertices pg
visibilityPolygonFromVertex' q sv mt segs = sweep q statusStruct (traceShowIdWith "events" events)
where
v = undefined
-- lazily test if the segment intersects the initial ray
segs' = labelWithDistances q initialRay segs
events = computeEvents q sv (untilEnd q sv mt) segs'
-- take only until the end of the range (if defined)
initialRay = traceShowIdWith "ray" $ HalfLine q sv
statusStruct = traceShowIdWith "initialSS" $ mkInitialSS segs'
-- | Test if the line segment is incident to a point with the given
-- index.
incidentTo :: Int -> LineSegment 2 (SP Int a) r -> Bool
incidentTo i s = s^.start.extra._1 == i || s^.end.extra._1 == i
-- | computes a (partial) visibility polygon of a set of \(n\)
-- disjoint segments. The input segments are allowed to share
-- endpoints, but no intersections or no endpoints in the interior of
-- other segments. The input vector indicates the starting direction,
-- the Maybe point indicates up to which point/dicrection (CCW) of the
-- starting vector we should compute the visibility polygon.
--
-- pre : - all line segments are considered closed.
-- - no singleton linesegments exactly pointing away from q.
-- - for every orientattion the visibility is blocked somewhere, i.e.
-- no rays starting in the query point q that are disjoint from all segments.
-- - no vertices at staring direction sv
--
-- running time: \(O(n\log n)\)
visibilitySweep :: forall p r e. (Ord r, Fractional r)
=> Vector 2 r -- ^ starting direction of the sweep
-> Maybe (Point 2 r)
-- ^ -- point indicating the last point to sweep to
-> Point 2 r -- ^ the point form which we compute the visibility polgyon
-> [LineSegment 2 p r :+ e]
-> [Point 2 r :+ Definer p e r]
visibilitySweep sv mt q segs = sweep q statusStruct events
where
-- lazily test if the segment intersects the initial ray
segs' = labelWithDistances q initialRay segs
events = computeEvents q sv (untilEnd q sv mt) segs'
initialRay = HalfLine q sv
statusStruct = mkInitialSS segs'
-- | Take until the ending point if defined. We can use that the list
-- of events appears in sorted order in the cyclic orientation around
-- the query point q
untilEnd :: (Ord r, Num r)
=> Point 2 r -- ^ query point
-> Vector 2 r -- ^ starting direction
-> Maybe (Point 2 r) -- ^ possible ending point
-> [Event a e r] -> [Event a e r]
untilEnd q sv = \case
Nothing -> id
Just t -> List.takeWhile (\e -> ccwCmpAroundWith' sv (ext q) (e^.eventVtx) (ext t) == LT)
-- | Runs the actual sweep
sweep :: (Foldable t, Ord r, Fractional r)
=> Point 2 r -- ^ query point
-> Status p e r -- ^ initial status structure
-> t (Event p e r) -- ^ events to handle
-> [Point 2 r :+ Definer p e r]
sweep q statusStruct = snd . List.foldl' (handleEvent q) (statusStruct,[])
-- | Computes the events in the sweep
computeEvents :: (Ord r, Num r, Foldable t)
=> Point 2 r -- ^ query point
-> Vector 2 r -- ^ starting direction
-> ([Event p1 e1 r] -> [Event p2 e2 r]) -- ^ until where to take the vents
-> t (LineSegment 2 p1 r :+ (Maybe r, e1))
-> [Event p2 e2 r]
computeEvents q sv takeUntil =
map (combine q)
. List.groupBy' (\a b -> ccwCmpAroundWith' sv (ext q) (a^.eventVtx) (b^.eventVtx))
. takeUntil
. List.sortBy (cmp `on` (^.eventVtx))
. concatMap (mkEvent sv q)
where
cmp = ccwCmpAroundWith' sv (ext q) <> cmpByDistanceTo' (ext q)
-- | Given multiple events happening at the same orientation, combine
-- them into a single event.
combine :: (Ord r, Num r) => Point 2 r -> NonEmpty (Event p e r) -> Event p e r
combine q es = Event p acts
where
acts = foldMap1 (^.actions) es
p = F.minimumBy (cmpByDistanceTo' (ext q)) . fmap (^.eventVtx) $ es
-- | Constructs the at most two events resulting from this segement.
mkEvent :: (Ord r, Num r)
=> Vector 2 r -- ^ starting direction
-> Point 2 r -- ^ query point
-> LineSegment 2 p r :+ (Maybe r, e)
-> [Event p e r]
mkEvent sv q (s@(LineSegment' u v) :+ (d,e)) = case cmp u v of
LT -> [ Event u insert
, Event v delete
]
GT -> [ Event v insert
, Event u delete
]
EQ -> [] -- zero length segment, just skip
where
cmp = ccwCmpAroundWith' sv (ext q) <> cmpByDistanceTo' (ext q)
s' = s :+ e
insert = (if isJust d then Delete s' else Insert s') :| []
delete = (if isJust d then Insert s' else Delete s') :| []
-- | Handles an event, computes the new status structure and output polygon.
handleEvent :: (Ord r, Fractional r)
=> Point 2 r
-> (Status p e r, [Point 2 r :+ Definer p e r])
-> Event p e r
-> (Status p e r, [Point 2 r :+ Definer p e r])
handleEvent q (ss,out) (Event (p :+ z) acts) = (ss', newVtx <> out)
where
(ins,dels) = bimap (map extract) (map extract) . NonEmpty.partition isInsert $ acts
ss' = flip (foldr (insertAt q p)) ins
. flip (foldr (deleteAt q p)) dels
$ ss
newVtx = let (a :+ sa) = firstHitAt' q p ss
(b :+ sb) = firstHitAt' q p ss'
ae = valOf a sa
be = valOf b sb
in case (a /= b, a == p) of
(True, _) -> -- new window of the output polygon discovered
-- figure out who is the closest vertex, (the reflex vtx)
-- and add the appropriate two vertices
case squaredEuclideanDist q a < squaredEuclideanDist q b of
True -> [ b :+ Right (a :+ ae, sb)
, a :+ Left ae -- a must be a vertex!
]
False -> [ b :+ Left be
, a :+ Right (b :+ be, sa)
]
(False,True) -> [ p :+ Left z]
-- sweeping over a regular vertex of the visibility polygon
(False,False) -> [] -- sweeping over a vertex not in output
valOf a (LineSegment' (b :+ be) (_ :+ ce) :+ _ ) | a == b = be
| otherwise = ce
--------------------------------------------------------------------------------
-- | Given two points q and p, and a status structure retrieve the
-- first segment in the status structure intersected by the ray from q
-- through p.
--
-- pre: all segments in the status structure should intersect the ray
-- from q through p (in a point), in that order.
--
-- running time: \(O(\log n)\)
firstHitAt :: forall p r e. (Ord r, Fractional r)
=> Point 2 r -> Point 2 r
-> Status p e r
-> Maybe (Point 2 r :+ LineSegment 2 p r :+ e)
firstHitAt q p = computeIntersectionPoint <=< Set.lookupMin
where
computeIntersectionPoint s = fmap (:+ s) . asA @(Point 2 r)
$ supportingLine (s^.core) `intersect` lineThrough p q
-- | Given two points q and p, and a status structure retrieve the
-- first segment in the status structure intersected by the ray from q
-- through p.
--
-- pre: - all segments in the status structure should intersect the ray
-- from q through p (in a point), in that order.
-- - the status structure is non-empty
--
-- running time: \(O(\log n)\)
firstHitAt' :: forall p r e. (Ord r, Fractional r)
=> Point 2 r -> Point 2 r
-> Status p e r
-> Point 2 r :+ LineSegment 2 p r :+ e
firstHitAt' q p s = case firstHitAt q p s of
Just x -> x
Nothing -> error "firstHitAt: precondition failed!"
--------------------------------------------------------------------------------
-- * Status Structure Operations
-- | Insert a new segment into the status structure, depending on the
-- (distance from q to to the) intersection point with the ray from q
-- through p
--
-- pre: all segments in the status structure should intersect the ray
-- from q through p, in that order.
--
-- \(O(\log n)\)
insertAt :: (Ord r, Fractional r)
=> Point 2 r -> Point 2 r -> LineSegment 2 p r :+ e
-> Status p e r -> Status p e r
insertAt q p = Set.insertBy (compareByDistanceToAt q p <> flip (compareAroundEndPoint q))
-- if two segments have the same distance, they must share and endpoint
-- so we use the CCW ordering around this common endpoint to determine
-- the order.
-- | Delete a segment from the status structure, depending on the
-- (distance from q to to the) intersection point with the ray from q
-- through p
--
-- pre: all segments in the status structure should intersect the ray
-- from q through p, in that order.
--
-- \(O(\log n)\)
deleteAt :: (Ord r, Fractional r)
=> Point 2 r -> Point 2 r -> LineSegment 2 p r :+ e
-> Status p e r -> Status p e r
deleteAt q p = Set.deleteAllBy (compareByDistanceToAt q p <> compareAroundEndPoint q)
-- if two segments have the same distance, we use the ccw order around their common
-- (end) point.
-- FIXME: If there are somehow segmetns that would continue at p as
-- well, they are also deleted.
-- | Given a list of line segments, each labeled with the distance
-- from their intersection point with the initial ray to the query
-- point, build the initial status structure.
mkInitialSS :: forall r p e. (Ord r, Fractional r)
=> [ LineSegment 2 p r :+ (Maybe r, e)] -> Status p e r
mkInitialSS = Set.mapMonotonic (^.extra)
. foldr (Set.insertBy $ comparing (^.core)) Set.empty
. mapMaybe (\(s :+ (md,e)) -> (:+ (s :+ e)) <$> md)
-- | Given q, the initial ray, and a segment s, computes if the
-- segment intersects the initial, rightward ray starting in q, and if
-- so returns the (squared) distance from q to that point together
-- with the segment.
initialIntersection :: forall r p. (Ord r, Fractional r)
=> Point 2 r -> HalfLine 2 r -> LineSegment 2 p r
-> Maybe r
initialIntersection q ray s =
case asA @(Point 2 r) $ seg `intersect` ray of
Nothing -> Nothing
Just z -> Just $ squaredEuclideanDist q z
where
seg = first (const ()) s
-- | Labels the segments with the distance from q to their
-- intersection point with the ray.
labelWithDistances :: (Ord r, Fractional r)
=> Point 2 r -> HalfLine 2 r -> [LineSegment 2 p r :+ b]
-> [LineSegment 2 p r :+ (Maybe r, b)]
labelWithDistances q ray = map (\(s :+ e) -> s :+ (initialIntersection q ray s, e))
--------------------------------------------------------------------------------
-- * Comparators for the rotating ray
-- | Given two points q and p, and two segments a and b that are guaranteed to
-- intersect the ray from q through p once, order the segments by their
-- intersection point
compareByDistanceToAt :: forall p r e. (Ord r, Fractional r)
=> Point 2 r -> Point 2 r
-> LineSegment 2 p r :+ e
-> LineSegment 2 p r :+ e
-> Ordering
compareByDistanceToAt q p = comparing f
where
f (s :+ _) = fmap (squaredEuclideanDist q)
. asA @(Point 2 r)
$ supportingLine s `intersect` lineThrough p q
-- | Given two segments that share an endpoint, order them by their
-- order around this common endpoint. I.e. if uv and uw share endpoint
-- u we uv is considered smaller iff v is smaller than w in the
-- counterclockwise order around u (treating the direction from q to
-- the common endpoint as zero).
compareAroundEndPoint :: forall p r e. (Ord r, Fractional r)
=> Point 2 r
-> LineSegment 2 p r :+ e
-> LineSegment 2 p r :+ e
-> Ordering
compareAroundEndPoint q
(LineSegment' a b :+ _)
(LineSegment' s t :+ _)
-- traceshow ("comapreAroundEndPoint ", sa, sb) False = undefined
| a^.core == s^.core = ccwCmpAroundWith' (a^.core .-. q) a b t
| a^.core == t^.core = ccwCmpAroundWith' (a^.core .-. q) a b s
| b^.core == s^.core = ccwCmpAroundWith' (b^.core .-. q) b a t
| b^.core == t^.core = ccwCmpAroundWith' (b^.core .-. q) b a s
| otherwise = error "compareAroundEndPoint: precondition failed!"
--------------------------------------------------------------------------------
-- * Helper functions for polygon operations
-- | Given q, and two consecutive points u and v, Computes a direction
-- for the initial ray, i.e. a "generic" ray that does not go through
-- any vertices.
startingDirection :: Fractional r => Point 2 r -> Point 2 r -> Point 2 r -> Vector 2 r
startingDirection q u w = v .-. q
where
v = u .+^ ((w .-. u) ^/ 2) -- point in the middle between u and w
-- note: the segment between u and w could pass on the wrong side of q
-- (i.e. so that does not "cover" the CCW but the CW range between u and w)
-- however, in that case there is apparently nothing on the CCW side opposite
-- to v, as u and w are supposed to be the first two events. This means the
-- precondition does not hold.
-- | finds two consecutive vertices in the clockwise order around the
-- given point q. I.e. there are no other points in between the two
-- returned points.
consecutive :: (Ord r, Num r) => Point 2 r -> NonEmpty (Point 2 r :+ p)
-> (Point 2 r, Point 2 r)
consecutive q ((p :+ _):|pts) = (p,consecutiveFrom (p .-. q) q pts)
-- | pre: input list is non-empty
consecutiveFrom :: (Ord r, Num r)
=> Vector 2 r -- ^ starting vector
-> Point 2 r -- ^ query point
-> [Point 2 r :+ p] -> Point 2 r
consecutiveFrom v q = view core . List.minimumBy (ccwCmpAroundWith' v (ext q))
-- | Gets the edges of the polygon as closed line segments.
closedEdges :: Polygon t p r -> [LineSegment 2 p r]
closedEdges = map asClosed . listEdges
where
asClosed (LineSegment' u v) = ClosedLineSegment u v
--------------------------------------------------------------------------------
-- * Generic Helper functions
--------------------------------------------------------------------------------
test :: StarShapedPolygon (Definer Int () R) R
test = visibilityPolygon origin testPg
testVtx = visibilityPolygonFromVertex testPg 0
testPg :: SimplePolygon Int R
testPg = fromPoints $ zipWith (:+) [ Point2 3 1
, Point2 3 2
, Point2 4 2
, Point2 2 4
, Point2 (-1) 4
, Point2 1 2
, Point2 (-3) (-1)
, Point2 4 (-1)
] [1..]
testPg2 :: SimplePolygon Int R
testPg2 = fromPoints $ zipWith (:+) [ Point2 3 1
, Point2 3 2
, Point2 4 2
, Point2 2 4
, Point2 (-1) 4
, Point2 1 2.1
, Point2 (-3) (-1)
, Point2 4 (-1)
] [1..]
traceShowIdWith x y = traceShow (show x,y) y