loop/src/Geometry/Intersect.hs

{-# LANGUAGE BangPatterns #-}

--{-# LANGUAGE TupleSections #-}
{- Testing for and finding intersection points.  -}
module Geometry.Intersect where

import Control.Applicative
import Data.List
import Data.Maybe
import Geometry.Data
import Geometry.LHS
import Geometry.Vector

-- | If two lines intersect, return 'Just' that point.
intersectLineLine' :: Point2 -> Point2 -> Point2 -> Point2 -> Maybe Point2
{-# INLINE intersectLineLine' #-}
intersectLineLine' (V2 x1 y1) (V2 x2 y2) (V2 x3 y3) (V2 x4 y4)
    | den == 0 = Nothing
    | otherwise = Just $ V2 (x1 + (x2 - x1) * t' / den) (y1 + (y2 - y1) * t' / den)
  where
    den = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4)
    t' = (x1 - x3) * (y3 - y4) - (y1 - y3) * (x3 - x4)

intersectSegSegErrorTest :: Point2 -> Point2 -> Point2 -> Point2 -> Maybe Point2
{-# INLINE intersectSegSegErrorTest #-}
intersectSegSegErrorTest a b c d = case intersectSegSeg a b c d of
    Nothing
        | intersectSegSegFullTest a b c d ->
            error $ "intersectSegSeg did not intersect" ++ show a ++ show b ++ show c ++ show d
    Just x
        | not $ intersectSegSegFullTest a b c d ->
            error $
                "intersectSegSeg did intersect" ++ show a ++ show b ++ show c ++ show d
                    ++ " at "
                    ++ show x
    m -> m

-- | If two segments intersect, return 'Just' that point.
intersectSegSeg :: Point2 -> Point2 -> Point2 -> Point2 -> Maybe Point2
{-# INLINE intersectSegSeg #-}
intersectSegSeg (V2 x1 y1) (V2 x2 y2) (V2 x3 y3) (V2 x4 y4)
    | V2 x1 y1 == V2 x2 y2 || V2 x3 y3 == V2 x4 y4 = Nothing
    | den == 0 = Nothing
    | den > 0 && (t' < 0 || u' < 0 || t' > den || u' > den) =
        Nothing
    | den < 0 && (t' > 0 || u' > 0 || t' < den || u' < den) =
        Nothing
    | otherwise = Just $ V2 (x1 + (x2 - x1) * t' / den) (y1 + (y2 - y1) * t' / den)
  where
    den = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4)
    t' = (x1 - x3) * (y3 - y4) - (y1 - y3) * (x3 - x4)
    u' = (y1 - y2) * (x1 - x3) - (x1 - x2) * (y1 - y3)

{- | Intended to intersect a segment with a half-line-segment, ie a segment
 extending infinitely in one direction.
-}
intersectSegLineFrom :: Point2 -> Point2 -> Point2 -> Point2 -> Maybe Point2
{-# INLINE intersectSegLineFrom #-}
intersectSegLineFrom (V2 x1 y1) (V2 x2 y2) (V2 x3 y3) (V2 x4 y4)
    | den == 0 = Nothing
    | den > 0 && (t' < 0 || u' < 0 || t' > den) =
        Nothing
    | den < 0 && (t' > 0 || u' > 0 || t' < den) =
        Nothing
    | otherwise = Just $ V2 (x1 + (x2 - x1) * t' / den) (y1 + (y2 - y1) * t' / den)
  where
    den = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4)
    t' = (x1 - x3) * (y3 - y4) - (y1 - y3) * (x3 - x4)
    u' = (y1 - y2) * (x1 - x3) - (x1 - x2) * (y1 - y3)

-- | Similar to 'intersectSegLineFrom'', but this version is probably not correct...
intersectSegLineext :: Point2 -> Point2 -> Point2 -> Point2 -> Maybe Point2
{-# INLINE intersectSegLineext #-}
intersectSegLineext (V2 x1 y1) (V2 x2 y2) (V2 x3 y3) (V2 x4 y4)
    | den == 0 = Nothing
    | den > 0 && (t' < 0 || u' < den || t' > den) =
        Nothing
    | den < 0 && (t' > 0 || u' > - den || t' < den) =
        Nothing
    | otherwise = Just $ V2 (x1 + (x2 - x1) * t' / den) (y1 + (y2 - y1) * t' / den)
  where
    den = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4)
    t' = (x1 - x3) * (y3 - y4) - (y1 - y3) * (x3 - x4)
    u' = (y1 - y2) * (x1 - x3) - (x1 - x2) * (y1 - y3)

-- | Intersect a segment with a line.
intersectSegLine :: Point2 -> Point2 -> Point2 -> Point2 -> Maybe Point2
{-# INLINE intersectSegLine #-}
intersectSegLine (V2 x1 y1) (V2 x2 y2) (V2 x3 y3) (V2 x4 y4)
    | den == 0 = Nothing
    | den > 0 && (t' < 0 || t' > den) =
        Nothing
    | den < 0 && (t' > 0 || t' < den) =
        Nothing
    | otherwise = Just $ V2 (x1 + (x2 - x1) * t' / den) (y1 + (y2 - y1) * t' / den)
  where
    den = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4)
    t' = (x1 - x3) * (y3 - y4) - (y1 - y3) * (x3 - x4)

--u'  = (y1-y2)*(x1-x3) - (x1-x2)*(y1-y3)

-- | A test that should align with Just values from intersectSegSeg.
intersectSegSegFullTest ::
    Point2 ->
    Point2 ->
    Point2 ->
    Point2 ->
    Bool
{-# INLINE intersectSegSegFullTest #-}
intersectSegSegFullTest x y z w =
    f x y z w && f z w x y && x /= y && z /= w
        && normalizeV (x -.- y) /= normalizeV (z -.- w)
        && normalizeV (y -.- x) /= normalizeV (z -.- w)
  where
    f a b c d =
        (not (isRHS a b c) && not (isLHS a b d))
            || (not (isLHS a b c) && not (isRHS a b d))

{- | It is not always necessary to find a point of intersection, sometimes a
 test may suffice.
-}
intersectSegSegTest ::
    Point2 ->
    Point2 ->
    Point2 ->
    Point2 ->
    Bool
{-# INLINE intersectSegSegTest #-}
intersectSegSegTest x y z w =
    f x y z w && f z w x y && x /= y && z /= w
  where
    f a b c d =
        (not (isRHS a b c) && not (isLHS a b d))
            || (not (isLHS a b c) && not (isRHS a b d))

intersectSegSegPreTest ::
    Point2 ->
    Point2 ->
    Point2 ->
    Point2 ->
    Maybe Point2
{-# INLINE intersectSegSegPreTest #-}
intersectSegSegPreTest a b c d
    | intersectSegSegTest a b c d = myIntersectSegSeg a b c d
    | otherwise = Nothing

{- | Due to floating point issues, 'intersectSegSeg'' is not always
 accurate---'myIntersectSegSeg'
 fixes at least some of
 the problem cases by transforming the points into rationals and then doing the
 intersection.
 This version is, probably, slower---both testing and benchmarking should be
 done.
-}
myIntersectSegSeg ::
    Point2 ->
    Point2 ->
    Point2 ->
    Point2 ->
    Maybe Point2
{-# INLINE myIntersectSegSeg #-}
myIntersectSegSeg a@(V2 ax ay) b@(V2 bx by) c@(V2 cx cy) d@(V2 dx dy) = case ratIntersectLineLine a b c d of
    Nothing -> Nothing
    Just (V2 x y) ->
        if inbetween x && inbetween' y
            then Just (V2 x y)
            else Nothing
  where
    inbetween x =
        ((ax <= x && x <= bx) || (bx <= x && x <= ax))
            && ((cx <= x && x <= dx) || (dx <= x && x <= cx))
    inbetween' y =
        ((ay <= y && y <= by) || (by <= y && y <= ay))
            && ((cy <= y && y <= dy) || (dy <= y && y <= cy))

-- | Polymorphic intersection of fractional line points.
myIntersectLineLine :: (Eq a, Fractional a) => V2 a -> V2 a -> V2 a -> V2 a -> Maybe (V2 a)
{-# INLINE myIntersectLineLine #-}
myIntersectLineLine a@(V2 ax _) b c@(V2 cx _) d
    | isNothing (linGrad a b) = V2 ax <$> axisInt (c *-* V2 ax 0) (d *-* V2 ax 0)
    | isNothing (linGrad c d) = V2 cx <$> axisInt (a *-* V2 cx 0) (b *-* V2 cx 0)
    | otherwise =
        case linGrad a b ^-^ linGrad c d of
            Just 0 -> Nothing
            _ -> liftA2 V2 newx ((linGrad a b ^*^ newx) ^+^ axisInt a b)
  where
    (^-^) = liftA2 (-)
    (^+^) = liftA2 (+)
    (^/^) = liftA2 (/)
    (^*^) = liftA2 (*)
    newx = (axisInt c d ^-^ axisInt a b) ^/^ (linGrad a b ^-^ linGrad c d)
    (*-*) (V2 ax' ay) (V2 bx by) = V2 (ax' - bx) (ay - by)

-- | Transforms floating points to rationals then performs line intersection.
ratIntersectLineLine :: Point2 -> Point2 -> Point2 -> Point2 -> Maybe Point2
{-# INLINE ratIntersectLineLine #-}
ratIntersectLineLine a b c d =
    toNumPoint2
        <$> myIntersectLineLine (toRatPoint2 a) (toRatPoint2 b) (toRatPoint2 c) (toRatPoint2 d)
  where
    toRatPoint2 (V2 x y) = V2 (toRational x) (toRational y)
    toNumPoint2 (V2 x y) = V2 (fromRational x) (fromRational y)

{- | Round the floats within a 'Point2' to the nearest integer.
__Examples__
Rounding jumps after intervals of .5:

>>> roundPoint (0.5,0.5001)
(0.0,1.0)

but is symmetric around 0:

>>> roundPoint2 (0.5,-0.5)
(0.0,0.0)
-}
roundPoint2 :: Point2 -> Point2
roundPoint2 (V2 x y) = V2 (fromIntegral (round x :: Int)) (fromIntegral (round y :: Int))

-- | Given two points, finds the linear gradient if it is non-infinite.
linGrad :: (Eq a, Fractional a) => V2 a -> V2 a -> Maybe a
{-# INLINE linGrad #-}
linGrad (V2 x y) (V2 a b)
    | x - a == 0 = Nothing
    | otherwise = Just $ (y - b) / (x - a)

-- | Given two points, finds the intersection with the y axis if it exists.
axisInt :: (Eq a, Fractional a) => V2 a -> V2 a -> Maybe a
{-# INLINE axisInt #-}
axisInt p (V2 a b) = (\lg -> b - (a * lg)) <$> linGrad p (V2 a b)

-- | Placeholder, undefined.
intersectSegsSeg :: [Point2] -> Point2 -> Point2 -> Maybe Point2
intersectSegsSeg = undefined

-- | Placeholder: should intersect a segment with a bezier curve.
intersectSegBezquad :: Point2 -> Point2 -> Point2 -> Point2 -> Point2 -> [Point2]
intersectSegBezquad = undefined

{- | finds one (if any) of the points of intersection between a segment and a
 polygon.
 Can almost certainly be optimised.
-}
intersectSegPolyFirst :: Point2 -> Point2 -> [Point2] -> Maybe Point2
intersectSegPolyFirst a b xs = foldr (<|>) Nothing $ zipWith lineColl xs (tail xs ++ [head xs])
  where
    lineColl = intersectSegSeg a b

{- | Given a line and a point return the point on the line closest to the
 point.
-}
closestPointOnLine ::
    -- | First line point.
    Point2 ->
    -- | Second line point.
    Point2 ->
    -- | Point not on line.
    Point2 ->
    Point2
{-# INLINE closestPointOnLine #-}
closestPointOnLine !a !b !p = a +.+ u *.* (b -.- a)
  where
    u = closestPointOnLineParam a b p

{- | Given a line and a point return a value corresponding to how far along the
 line the point is.
-}
closestPointOnLineParam ::
    -- | First line point.
    Point2 ->
    -- | Second line point.
    Point2 ->
    -- | Point not on line.
    Point2 ->
    Float
{-# INLINE closestPointOnLineParam #-}
closestPointOnLineParam !a !b !p =
    (p -.- a) `dotV` (b -.- a) / (b -.- a) `dotV` (b -.- a)

{- | Given a segment and external point, find the closest point on the segment.
 clamps to the end of the segment
-}
closestPointOnSeg :: Point2 -> Point2 -> Point2 -> Point2
closestPointOnSeg segP1 segP2 p
    | closestPointOnLineParam segP1 segP2 p <= 0 = segP1
    | closestPointOnLineParam segP1 segP2 p >= 1 = segP2
    | otherwise = closestPointOnLine segP1 segP2 p

{- | Given a segment and external point, find the closest point on the segment.
 does not return closest points beyond the segment
-}
orthogonalPointOnSeg :: Point2 -> Point2 -> Point2 -> Maybe Point2
orthogonalPointOnSeg a b p
    | param < 0 || param > 1 = Nothing
    | otherwise = Just $ a +.+ param *.* normalizeV (b -.- a)
  where
    param = closestPointOnLineParam a b p

inSegArea :: Point2 -> Point2 -> Point2 -> Bool
inSegArea a b c = param >= 0 && param <= dotV (b -.- a) (b -.- a)
  where
    param = dotV (b -.- a) (c -.- a)

intersectCircSeg :: Point2 -> Float -> Point2 -> Point2 -> [Point2]
intersectCircSeg c r a b
    | y < 0 = []
    | otherwise = nub $ filter (inSegArea a b) [d -.- v, d +.+ v]
  where
    d = closestPointOnLine a b c
    x = dist d c
    y = r ** 2 - x ** 2
    z = sqrt y
    v = z *.* normalizeV (b -.- a)

intersectCircSegFirst :: Point2 -> Float -> Point2 -> Point2 -> Maybe Point2
intersectCircSegFirst c r a = listToMaybe . intersectCircSeg c r a