module Algorithms.Geometry.RayShooting.Naive
  ( firstHit
  , firstHit'


  , firstHitSegments
  , intersectionDistance
  , labelWithDistances
  ) where

import           Control.Lens
import           Data.Bifunctor
import           Data.Ext
import           Data.Geometry.HalfLine
import           Data.Geometry.LineSegment
import           Data.Geometry.Point
import           Data.Geometry.Polygon
import           Data.Intersection
import qualified Data.List as List
import           Data.Maybe
import           Data.Ord (comparing)
import           Data.Vinyl.CoRec
import           Data.Vinyl

--------------------------------------------------------------------------------

-- |
--
-- pre: halfline should start in the interior
firstHit     :: (Fractional r, Ord r)
             => HalfLine 2 r
             -> Polygon t p r
             -> LineSegment 2 p r
firstHit :: HalfLine 2 r -> Polygon t p r -> LineSegment 2 p r
firstHit HalfLine 2 r
ray = LineSegment 2 p r -> Maybe (LineSegment 2 p r) -> LineSegment 2 p r
forall a. a -> Maybe a -> a
fromMaybe LineSegment 2 p r
forall a. a
err (Maybe (LineSegment 2 p r) -> LineSegment 2 p r)
-> (Polygon t p r -> Maybe (LineSegment 2 p r))
-> Polygon t p r
-> LineSegment 2 p r
forall b c a. (b -> c) -> (a -> b) -> a -> c
. HalfLine 2 r -> Polygon t p r -> Maybe (LineSegment 2 p r)
forall r (t :: PolygonType) p.
(Fractional r, Ord r) =>
HalfLine 2 r -> Polygon t p r -> Maybe (LineSegment 2 p r)
firstHit' HalfLine 2 r
ray
  where
    err :: a
err = [Char] -> a
forall a. HasCallStack => [Char] -> a
error [Char]
"Algorithms.Geometry.RayShooting.Naive: no intersections; ray must have started outside the polygon"

-- | Compute the first edge hit by the ray, if it exists
firstHit'        :: (Fractional r, Ord r)
                 => HalfLine 2 r
                 -> Polygon t p r
                 -> Maybe (LineSegment 2 p r)
firstHit' :: HalfLine 2 r -> Polygon t p r -> Maybe (LineSegment 2 p r)
firstHit' HalfLine 2 r
ray Polygon t p r
pg = ((LineSegment 2 p r :+ ()) -> LineSegment 2 p r)
-> Maybe (LineSegment 2 p r :+ ()) -> Maybe (LineSegment 2 p r)
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
fmap ((LineSegment 2 p r :+ ())
-> Getting
     (LineSegment 2 p r) (LineSegment 2 p r :+ ()) (LineSegment 2 p r)
-> LineSegment 2 p r
forall s a. s -> Getting a s a -> a
^.Getting
  (LineSegment 2 p r) (LineSegment 2 p r :+ ()) (LineSegment 2 p r)
forall core extra core'.
Lens (core :+ extra) (core' :+ extra) core core'
core) (Maybe (LineSegment 2 p r :+ ()) -> Maybe (LineSegment 2 p r))
-> ([LineSegment 2 p r] -> Maybe (LineSegment 2 p r :+ ()))
-> [LineSegment 2 p r]
-> Maybe (LineSegment 2 p r)
forall b c a. (b -> c) -> (a -> b) -> a -> c
. HalfLine 2 r
-> [LineSegment 2 p r :+ ()] -> Maybe (LineSegment 2 p r :+ ())
forall r p e.
(Ord r, Fractional r) =>
HalfLine 2 r
-> [LineSegment 2 p r :+ e] -> Maybe (LineSegment 2 p r :+ e)
firstHitSegments HalfLine 2 r
ray ([LineSegment 2 p r :+ ()] -> Maybe (LineSegment 2 p r :+ ()))
-> ([LineSegment 2 p r] -> [LineSegment 2 p r :+ ()])
-> [LineSegment 2 p r]
-> Maybe (LineSegment 2 p r :+ ())
forall b c a. (b -> c) -> (a -> b) -> a -> c
. (LineSegment 2 p r -> LineSegment 2 p r :+ ())
-> [LineSegment 2 p r] -> [LineSegment 2 p r :+ ()]
forall a b. (a -> b) -> [a] -> [b]
map LineSegment 2 p r -> LineSegment 2 p r :+ ()
forall a. a -> a :+ ()
ext ([LineSegment 2 p r] -> Maybe (LineSegment 2 p r))
-> [LineSegment 2 p r] -> Maybe (LineSegment 2 p r)
forall a b. (a -> b) -> a -> b
$ Polygon t p r -> [LineSegment 2 p r]
forall (t :: PolygonType) p r. Polygon t p r -> [LineSegment 2 p r]
listEdges Polygon t p r
pg


-- | Compute the first segment hit by the ray, if it exists
firstHitSegments     :: (Ord r, Fractional r)
                     => HalfLine 2 r
                     -> [LineSegment 2 p r :+ e]
                     -> Maybe (LineSegment 2 p r :+ e)
firstHitSegments :: HalfLine 2 r
-> [LineSegment 2 p r :+ e] -> Maybe (LineSegment 2 p r :+ e)
firstHitSegments HalfLine 2 r
ray = ((r :+ (LineSegment 2 p r :+ e)) -> LineSegment 2 p r :+ e)
-> Maybe (r :+ (LineSegment 2 p r :+ e))
-> Maybe (LineSegment 2 p r :+ e)
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
fmap ((r :+ (LineSegment 2 p r :+ e))
-> Getting
     (LineSegment 2 p r :+ e)
     (r :+ (LineSegment 2 p r :+ e))
     (LineSegment 2 p r :+ e)
-> LineSegment 2 p r :+ e
forall s a. s -> Getting a s a -> a
^.Getting
  (LineSegment 2 p r :+ e)
  (r :+ (LineSegment 2 p r :+ e))
  (LineSegment 2 p r :+ e)
forall core extra extra'.
Lens (core :+ extra) (core :+ extra') extra extra'
extra) (Maybe (r :+ (LineSegment 2 p r :+ e))
 -> Maybe (LineSegment 2 p r :+ e))
-> ([LineSegment 2 p r :+ e]
    -> Maybe (r :+ (LineSegment 2 p r :+ e)))
-> [LineSegment 2 p r :+ e]
-> Maybe (LineSegment 2 p r :+ e)
forall b c a. (b -> c) -> (a -> b) -> a -> c
. ((r :+ (LineSegment 2 p r :+ e)) -> r)
-> [r :+ (LineSegment 2 p r :+ e)]
-> Maybe (r :+ (LineSegment 2 p r :+ e))
forall b a. Ord b => (a -> b) -> [a] -> Maybe a
minimumOn ((r :+ (LineSegment 2 p r :+ e))
-> Getting r (r :+ (LineSegment 2 p r :+ e)) r -> r
forall s a. s -> Getting a s a -> a
^.Getting r (r :+ (LineSegment 2 p r :+ e)) r
forall core extra core'.
Lens (core :+ extra) (core' :+ extra) core core'
core)
                     ([r :+ (LineSegment 2 p r :+ e)]
 -> Maybe (r :+ (LineSegment 2 p r :+ e)))
-> ([LineSegment 2 p r :+ e] -> [r :+ (LineSegment 2 p r :+ e)])
-> [LineSegment 2 p r :+ e]
-> Maybe (r :+ (LineSegment 2 p r :+ e))
forall b c a. (b -> c) -> (a -> b) -> a -> c
. ((LineSegment 2 p r :+ (Maybe r, e))
 -> Maybe (r :+ (LineSegment 2 p r :+ e)))
-> [LineSegment 2 p r :+ (Maybe r, e)]
-> [r :+ (LineSegment 2 p r :+ e)]
forall a b. (a -> Maybe b) -> [a] -> [b]
mapMaybe (\(LineSegment 2 p r
s :+ (Maybe r
md, e
e)) -> (r -> (LineSegment 2 p r :+ e) -> r :+ (LineSegment 2 p r :+ e)
forall core extra. core -> extra -> core :+ extra
:+ (LineSegment 2 p r
s LineSegment 2 p r -> e -> LineSegment 2 p r :+ e
forall core extra. core -> extra -> core :+ extra
:+ e
e)) (r -> r :+ (LineSegment 2 p r :+ e))
-> Maybe r -> Maybe (r :+ (LineSegment 2 p r :+ e))
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> Maybe r
md)
                     ([LineSegment 2 p r :+ (Maybe r, e)]
 -> [r :+ (LineSegment 2 p r :+ e)])
-> ([LineSegment 2 p r :+ e]
    -> [LineSegment 2 p r :+ (Maybe r, e)])
-> [LineSegment 2 p r :+ e]
-> [r :+ (LineSegment 2 p r :+ e)]
forall b c a. (b -> c) -> (a -> b) -> a -> c
. Point 2 r
-> HalfLine 2 r
-> [LineSegment 2 p r :+ e]
-> [LineSegment 2 p r :+ (Maybe r, e)]
forall r p b.
(Ord r, Fractional r) =>
Point 2 r
-> HalfLine 2 r
-> [LineSegment 2 p r :+ b]
-> [LineSegment 2 p r :+ (Maybe r, b)]
labelWithDistances (HalfLine 2 r
rayHalfLine 2 r
-> Getting (Point 2 r) (HalfLine 2 r) (Point 2 r) -> Point 2 r
forall s a. s -> Getting a s a -> a
^.Getting (Point 2 r) (HalfLine 2 r) (Point 2 r)
forall (d :: Nat) r. Lens' (HalfLine d r) (Point d r)
startPoint) HalfLine 2 r
ray

minimumOn   :: Ord b => (a -> b) -> [a] -> Maybe a
minimumOn :: (a -> b) -> [a] -> Maybe a
minimumOn a -> b
f = \case
  [] -> Maybe a
forall a. Maybe a
Nothing
  [a]
xs -> a -> Maybe a
forall a. a -> Maybe a
Just (a -> Maybe a) -> ([a] -> a) -> [a] -> Maybe a
forall b c a. (b -> c) -> (a -> b) -> a -> c
. (a -> a -> Ordering) -> [a] -> a
forall (t :: * -> *) a.
Foldable t =>
(a -> a -> Ordering) -> t a -> a
List.minimumBy ((a -> b) -> a -> a -> Ordering
forall a b. Ord a => (b -> a) -> b -> b -> Ordering
comparing a -> b
f) ([a] -> Maybe a) -> [a] -> Maybe a
forall a b. (a -> b) -> a -> b
$ [a]
xs


-- | Given q, a 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.
intersectionDistance         :: forall r p. (Ord r, Fractional r)
                             => Point 2 r -> HalfLine 2 r -> LineSegment 2 p r
                             -> Maybe r
intersectionDistance :: Point 2 r -> HalfLine 2 r -> LineSegment 2 p r -> Maybe r
intersectionDistance Point 2 r
q HalfLine 2 r
ray LineSegment 2 p r
s = CoRec Identity '[NoIntersection, Point 2 r, LineSegment 2 () r]
-> Handlers
     '[NoIntersection, Point 2 r, LineSegment 2 () r] (Maybe r)
-> Maybe r
forall (ts :: [*]) b. CoRec Identity ts -> Handlers ts b -> b
match (LineSegment 2 () r
seg LineSegment 2 () r
-> HalfLine 2 r -> Intersection (LineSegment 2 () r) (HalfLine 2 r)
forall g h. IsIntersectableWith g h => g -> h -> Intersection g h
`intersect` HalfLine 2 r
ray) (Handlers
   '[NoIntersection, Point 2 r, LineSegment 2 () r] (Maybe r)
 -> Maybe r)
-> Handlers
     '[NoIntersection, Point 2 r, LineSegment 2 () r] (Maybe r)
-> Maybe r
forall a b. (a -> b) -> a -> b
$
       (NoIntersection -> Maybe r) -> Handler (Maybe r) NoIntersection
forall b a. (a -> b) -> Handler b a
H (\NoIntersection
NoIntersection                   -> Maybe r
forall a. Maybe a
Nothing)
    Handler (Maybe r) NoIntersection
-> Rec (Handler (Maybe r)) '[Point 2 r, LineSegment 2 () r]
-> Handlers
     '[NoIntersection, Point 2 r, LineSegment 2 () r] (Maybe r)
forall u (a :: u -> *) (r :: u) (rs :: [u]).
a r -> Rec a rs -> Rec a (r : rs)
:& (Point 2 r -> Maybe r) -> Handler (Maybe r) (Point 2 r)
forall b a. (a -> b) -> Handler b a
H (\Point 2 r
p                                -> r -> Maybe r
forall a. a -> Maybe a
Just (r -> Maybe r) -> r -> Maybe r
forall a b. (a -> b) -> a -> b
$ Point 2 r -> r
d Point 2 r
p)
    Handler (Maybe r) (Point 2 r)
-> Rec (Handler (Maybe r)) '[LineSegment 2 () r]
-> Rec (Handler (Maybe 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 -> Maybe r)
-> Handler (Maybe r) (LineSegment 2 () r)
forall b a. (a -> b) -> Handler b a
H (\(LineSegment' (Point 2 r
a :+ ()
_) (Point 2 r
b :+ ()
_)) -> r -> Maybe r
forall a. a -> Maybe a
Just (r -> Maybe r) -> r -> Maybe r
forall a b. (a -> b) -> a -> b
$ Point 2 r -> r
d Point 2 r
a r -> r -> r
forall a. Ord a => a -> a -> a
`min` Point 2 r -> r
d Point 2 r
b)
    Handler (Maybe r) (LineSegment 2 () r)
-> Rec (Handler (Maybe r)) '[]
-> Rec (Handler (Maybe r)) '[LineSegment 2 () r]
forall u (a :: u -> *) (r :: u) (rs :: [u]).
a r -> Rec a rs -> Rec a (r : rs)
:& Rec (Handler (Maybe r)) '[]
forall u (a :: u -> *). Rec a '[]
RNil
    -- TODO: there is some slight subtility if the segment is open.
  where
    d :: Point 2 r -> r
d = Point 2 r -> Point 2 r -> r
forall r (d :: Nat).
(Num r, Arity d) =>
Point d r -> Point d r -> r
squaredEuclideanDist Point 2 r
q
    seg :: LineSegment 2 () r
seg = (p -> ()) -> LineSegment 2 p r -> LineSegment 2 () r
forall (p :: * -> * -> *) a b c.
Bifunctor p =>
(a -> b) -> p a c -> p b c
first (() -> p -> ()
forall a b. a -> b -> a
const ()) LineSegment 2 p r
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 :: Point 2 r
-> HalfLine 2 r
-> [LineSegment 2 p r :+ b]
-> [LineSegment 2 p r :+ (Maybe r, b)]
labelWithDistances Point 2 r
q HalfLine 2 r
ray = ((LineSegment 2 p r :+ b) -> LineSegment 2 p r :+ (Maybe r, b))
-> [LineSegment 2 p r :+ b] -> [LineSegment 2 p r :+ (Maybe r, b)]
forall a b. (a -> b) -> [a] -> [b]
map (\(LineSegment 2 p r
s :+ b
e) -> LineSegment 2 p r
s LineSegment 2 p r
-> (Maybe r, b) -> LineSegment 2 p r :+ (Maybe r, b)
forall core extra. core -> extra -> core :+ extra
:+ (Point 2 r -> HalfLine 2 r -> LineSegment 2 p r -> Maybe r
forall r p.
(Ord r, Fractional r) =>
Point 2 r -> HalfLine 2 r -> LineSegment 2 p r -> Maybe r
intersectionDistance Point 2 r
q HalfLine 2 r
ray LineSegment 2 p r
s, b
e))



--------------------------------------------------------------------------------