loop/src/Geometry/Intersect.hs

{-# LANGUAGE BangPatterns #-}

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

import Data.Monoid
import Data.List (sortOn)
import Control.Applicative
import Control.Lens
import Control.Monad
import Data.Maybe
import Geometry.Data
import Geometry.LHS
import Geometry.Vector
import Geometry.Vector3D
import Linear

-- | If two lines intersect, return 'Just' that point.
intersectLineLine :: Point2 -> Point2 -> Point2 -> Point2 -> Maybe Point2
{-# INLINE intersectLineLine #-}
--intersectLineLine a@(V2 x1 y1) b@(V2 x2 y2) c@(V2 x3 y3) d@(V2 x4 y4)
intersectLineLine a b c d = do
    let den = detV (a-b) (c-d)
    guard $ den /= 0
    let t = detV (a-c) (c-d)
    return $ a + (t/den) *^ (b-a)

intersectLinePlaneAlong :: Point3 -> Point3 -> Point3 -> Point3 -> Maybe Float
intersectLinePlaneAlong x y p n = do
    let den = dot (y - x) n
    guard $ den /= 0
    return $ dot (p - x) n / den

-- this needs to be checked
intersectSegPlane :: Point3 -> Point3 -> Point3 -> Point3 -> Maybe Point3
intersectSegPlane x y p n = do
    d <- intersectLinePlaneAlong x y p n
    guard $ d >= 0 && d < 1
    return $ x + d *^ (y - x)

intersectSegSurface ::
    Point3 ->
    Point3 ->
    Point3 ->
    Point3 ->
    [(Point3, Point3)] ->
    Maybe Point3
intersectSegSurface sp ep p n ss = do
    xp <- intersectSegPlane sp ep p n
    let f (a, b) = isNHS a b xp
    guard $ all f ss
    return xp

isNHS :: Point3 -> Point3 -> Point3 -> Bool
isNHS p n x = 0 > dot (p - x) n

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.
Will intersect with the first endpoint of the segment, but NOT the second.
This is to allow sensible intersections with polygons described as lists of points.
It will also intersect with the point of the ray.
-}
intersectSegRay :: Point2 -> Point2 -> Point2 -> Point2 -> Maybe Point2
{-# INLINE intersectSegRay #-}
intersectSegRay (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)

{- | Intersect a segment with a line.
 the line intersects with the first endpoint of the segment
 but NOT the second
-}
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)

-- | 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.
This should intersect on endpoints.
-}
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 = compareLHS a b c /= compareLHS a b d
--        (not (isRHS a b c) && not (isLHS a b d))
--            || (not (isLHS a b c) && not (isRHS a b d))
--        (isRHS a b c && isLHS a b d)
--            || (isLHS a b c && 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

-- orders intersecting points according to the line
intersectLinePoly :: Point2 -> Point2 -> [Point2] -> [Point2]
intersectLinePoly a b (p:ps) = sortOn (dotV (b - a)) 
    . catMaybes
    $ zipWith (\x y -> intersectSegLine x y a b) (p:ps) (ps ++[p])
intersectLinePoly _ _ [] = error "intersectLinePoly empty polygon"

intersectRayPoly :: Point2 -> Point2 -> [Point2] -> Maybe Point2
intersectRayPoly a b (p:ps) = getFirst . mconcat $ zipWith f (p:ps) (ps++[p])
  where
    f x y = First $ intersectSegRay x y a b
intersectRayPoly _ _ _ = error "intersectRayPoly: polygon too small"


{- | 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 -> (Maybe Point2,Maybe Point2)
intersectCircSeg c r s e = intersectCircLineAlong c r s e
    & g
    & each %~ f
  where
    f (Just x) | x >= 0 && x < 1 = Just $ s + x *^ (e - s)
    f _ = Nothing
    g Nothing = (Nothing,Nothing)
    g (Just (x,y)) = (Just x,Just y)
        
--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 :: Int) - x ^ (2 :: Int)
--    z = sqrt y
--    v = z *.* normalizeV (b -.- a)

intersectCircLineAlong :: Point2 -> Float -> Point2 -> Point2 -> Maybe (Float, Float)
intersectCircLineAlong p r x y = do
    let d = y - x
        f = p - x
        dsc = (dot d f / dot d d) ** 2 - (dot f f - r ** 2) / dot d d
    guard $ dsc > 0
    let a = dot d f / dot d d
    return (a - sqrt dsc, a + sqrt dsc)

intersectCircLine :: Point2 -> Float -> Point2 -> Point2 -> Maybe (Point2, Point2)
intersectCircLine c r x y =
    intersectCircLineAlong c r x y
        & _Just . _1 %~ f
        & _Just . _2 %~ f
  where
    f :: Float -> Point2
    f a = x + (a *.* (y - x))

intersectCylSeg :: Point3 -> Float -> Float -> Point3 -> Point3 ->
    (Maybe (Point3, Point3), Maybe (Point3, Point3))
intersectCylSeg p r h s e = fromMaybe (Nothing, Nothing) $ do
    (a, b) <- intersectCircLineAlong (p ^. _xy) r (s ^. _xy) (e ^. _xy)
    return (f min a, f max b)
  where
    mtopx = intersectLinePlaneAlong s e (V3 0 0 (h + p ^. _z)) (V3 0 0 1)
    mbotx = intersectLinePlaneAlong s e (V3 0 0 (p^._z)) (V3 0 0 1)
    v = e - s
    f mm a = do
        let x = s + a *.*.* v
        if x ^. _z < (h + p ^. _z) && x ^. _z > (p ^. _z)
            then do
                guard $ a >= 0 && a < 1
                return (x, v & _xy %~ reflectInNormal (x ^. _xy - p ^. _xy))
            else do
                topx <- mtopx
                botx <- mbotx
                let d = mm topx botx
                guard $ d > 0 && d < 1
                let int = s + d *.*.* v
                guard $ dist (int ^. _xy) (p ^. _xy) <= r
                return (int, reflectInNormal (V3 0 0 1) v)

intersectCircSegTest :: Point2 -> Float -> Point2 -> Point2 -> Bool
intersectCircSegTest c r x y =
    intersectSegSegTest (c + z) (c - z) x y
        || dist c x <= r
        || dist c y <= r
  where
    z = r *.* vNormal (normalizeV x - y)

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