loop/src/Geometry.hs

{-# LANGUAGE BangPatterns #-}

{- |
Module      : Geometry
Description : Geometry helpers

This module provides geometry functions that manipulate pairs of floats.
Conventions:
Seg refers to a segment between two points and does not extend beyond either of these points.
Line is also defined by two points but does extend beyond the two points.
-}
module Geometry (
    module Geometry,
    module Geometry.Data,
    module Geometry.Intersect,
    module Geometry.Bezier,
    module Geometry.Vector,
    module Geometry.Vector3D,
    module Geometry.LHS,
    module Geometry.Polygon,
    module Geometry.Triangulate,
    loopPairs,
) where

import qualified Data.Set as S
import Geometry.Bezier
import Geometry.Data
import Geometry.Intersect
import Geometry.LHS
import Geometry.Polygon
import Geometry.Triangulate
import Geometry.Vector
import Geometry.Vector3D
import ListHelp

{- | Return a point a distance away from a first point towards a second point.
 Does not go past the second point.
 No check is made for a negative distance, so can go past the first point.
-}
alongSegBy :: Float -> Point2 -> Point2 -> Point2
alongSegBy !x !a !b = a +.+ y *.* normalizeV (b -.- a)
  where
    y = min x $ dist a b

-- | Debug version of 'pointInPolygon'.
errorPointInPolygon :: Int -> Point2 -> [Point2] -> Bool
errorPointInPolygon !i !p xs
    | length xs == 1 = error "one point polygon"
    | length xs == 2 = error "two point polygon"
    | nub xs == xs = pointInPoly p xs
    | otherwise = error $ "errorPointInPolygon " ++ show i

-- | Debug version of 'normalizeV'.
errorNormalizeV :: Int -> Point2 -> Point2
errorNormalizeV !i (V2 0 0) = error $ "problem with function: errorNormalizeV " ++ show i
errorNormalizeV _ !p = normalizeV p

-- | Debug version of 'angleVV'.
errorAngleVV :: Int -> Point2 -> Point2 -> Float
errorAngleVV !i (V2 0 0) _ = error $ "problem with function: errorAngleVV " ++ show i
errorAngleVV !i _ (V2 0 0) = error $ "problem with function: errorAngleVV " ++ show i
errorAngleVV _ !p !p' = angleVV p p'

-- | Debug version of 'isLHS'.
errorIsLHS :: Int -> Point2 -> Point2 -> Point2 -> Bool
errorIsLHS !i !x !y
    | x == y = error $ "problem with function: errorIsLHS " ++ show i
    | otherwise = isLHS x y

-- | Debug version of 'closestPointOnLine'
errorClosestPointOnLine :: Int -> Point2 -> Point2 -> Point2 -> Point2
errorClosestPointOnLine !i !x !y
    | x == y = error $ "problem with function: errorClosestPointOnLine " ++ show i
    | otherwise = closestPointOnLine x y

-- | Debug version of 'closestPointOnLineParam'
errorClosestPointOnLineParam :: Int -> Point2 -> Point2 -> Point2 -> Float
errorClosestPointOnLineParam _ !x !y !z
    | x == y = dist x z
    | otherwise = closestPointOnLineParam x y z

-- | Return midpoint between two points.
midPoint :: Point2 -> Point2 -> Point2
midPoint !a !b = 0.5 *.* (a +.+ b)

{- | Test whether a circle is on a segment by intersecting a new normal segment through the
 center of the circle with the segment itself.
 Returns False if the circle center is beyond the endpoints of the
 segment.
-}
circOnSegNoEndpoints :: Point2 -> Point2 -> Point2 -> Float -> Bool
{-# INLINE circOnSegNoEndpoints #-}
circOnSegNoEndpoints !p1 !p2 !c !rad = intersectSegSegTest p1 p2 (c -.- n) (c +.+ n)
  where
    n = rad *.* vNormal (normalizeV $ p1 -.- p2)

{- | Test whether a circle is on a segment by intersecting a normal and testing
 the distance to the endpoints of the segment.
 Perhaps a better order of arguments.
-}
circOnSeg :: Point2 -> Float -> Point2 -> Point2 -> Bool
{-# INLINE circOnSeg #-}
circOnSeg !c !rad !p1 !p2 =
    magV (p1 -.- c) <= rad
        || magV (p2 -.- c) <= rad
        || intersectSegSegTest p1 p2 (c -.- n) (c +.+ n)
  where
    n = rad *.* vNormal (normalizeV $ p1 -.- p2)

{- | Test whether a segment intersects a circle by intersecting a normal and testing
 the distance to the endpoints of the segment.
-}
segOnCirc :: Point2 -> Point2 -> Point2 -> Float -> Bool
{-# INLINE segOnCirc #-}
segOnCirc !p1 !p2 !c !rad =
    magV (p1 -.- c) <= rad
        || magV (p2 -.- c) <= rad
        || intersectSegSegTest p1 p2 (c -.- n) (c +.+ n)
  where
    n = rad *.* vNormal (normalizeV $ p1 -.- p2)

cylinderOnSeg :: Point3 -> Point3 -> Point3 -> Float -> Bool
{-# INLINE cylinderOnSeg #-}
cylinderOnSeg = undefined -- TODO

-- | Find the difference between two Nums.
difference :: (Ord a, Num a) => a -> a -> a
{-# INLINE difference #-}
difference x y
    | x > y = x - y
    | otherwise = y - x

{- | Given vector line direction and a vector movement,
 reflects the movement according to the line.
-}
reflectIn :: Point2 -> Point2 -> Point2
reflectIn line vec = rotateV (2 * angleBetween line vec) vec


-- takes an angle of entry (measured from x axis) and two wall points and gives a
-- reflected angle (from x axis)
reflectAngle :: Float -> Point2 -> Point2 -> Float
reflectAngle a x y = 2 * argV (x - y) - a

{- | Find the representation (by applying +-pi) of an angle
 - that is nearest to another given angle
-}
nearestAngleRep :: Float -> Float -> Float
nearestAngleRep a b
    | b >= a && b - a <= pi = b
    | b >= a = nearestAngleRep a (b - 2 * pi)
    | a - b <= pi = b
    | otherwise = nearestAngleRep a (b + 2 * pi)

{- | Find angle between two points.
 Not normalised, ranges from -2*pi to 2*pi.
-}
angleBetween :: Point2 -> Point2 -> Float
angleBetween v1 v2 = argV v1 - argV v2

-- | Return a list containing two copies of a pair.
doublePair :: (a, a) -> [(a, a)]
doublePair (x, y) = [(x, y), (y, x)]

-- this shouldn't be here
doublePairSet :: Ord a => (a, a) -> S.Set (a, a)
doublePairSet (x, y) = S.fromList [(x, y), (y, x)]

doubleV2 :: V2 a -> [V2 a]
doubleV2 (V2 x y) = [V2 x y, V2 y x]

polyToTris' :: [s] -> [s]
{-# INLINE polyToTris' #-}
polyToTris' [] = []
polyToTris' (a : as) = prependTwo a as

prependTwo :: a -> [a] -> [a]
prependTwo sep (x : y : xs) = sep : x : y : prependTwo sep (y : xs)
prependTwo _ _ = []

-- | Return n equidistant points on a circle with a radius of 600.
nRays :: Int -> [Point2]
nRays n = nRaysRad n 600

-- | Return n equidistant points on a circle with a radius of x.
nRaysRad :: Int -> Float -> [Point2]
nRaysRad n x = take n $ iterate (rotateV (2 * pi / fromIntegral n)) (V2 x 0)

{- | Test whether an angle is to the left of another angle, according to the
 smallest change in rotation between them.
 This appears to sometimes fail if the angles are not normalized.
-}
isLeftOfA :: Float -> Float -> Bool
isLeftOfA angle1 angle2 = normalizeAngle (angle1 - angle2) < pi

{- | Test whether a vector is to the left of another, according to the smallest
 change of rotation between them.
-}
isLeftOf :: Point2 -> Point2 -> Bool
isLeftOf x y = isLeftOfA (argV x) (argV y)

{- | Find the difference between two angles.
 TODO write tests
-}
diffAngles :: Float -> Float -> Float
diffAngles x y = nearZeroAngle $ x - y

-- | Return Just a point if it is inside a circle, Nothing otherwise.
pointInCircle :: Point2 -> Float -> Point2 -> Maybe Point2
pointInCircle p r c
    | p == c = Just p
    | magV (p -.- c) < r = Just p
    | otherwise = Nothing

{- | Finds the height of a triangle using herons formula.
 The base is the line between the first two points.
-}
heron :: Point2 -> Point2 -> Point2 -> Float
heron x y z
    | x == y = 0
    | otherwise =
        let a = magV $ x -.- y
            b = magV $ y -.- z
            c = magV $ z -.- x
            s = (a + b + c) / 2
            area = sqrt (s * (s - a) * (s - b) * (s - c))
         in 2 * area / a

-- | Multiplies reflection in normal by factor. With 0 same as reflectAngle
reflectInParam :: Float -> Point2 -> Point2 -> Point2
reflectInParam x line vec =
    let angle = 2 * angleBetween line vec
        rAng = rotateV angle vec
        p = x *.* errorClosestPointOnLine 3 (V2 0 0) (vNormal line) rAng
     in rAng -.- p

isOnSeg :: Point2 -> Point2 -> Point2 -> Bool
isOnSeg l1 l2 p =
    errorClosestPointOnLineParam 10 l1 (l1 +.+ vNormal (l2 -.- l1)) p == 0
        && errorClosestPointOnLineParam 11 l1 l2 p <= 1
        && errorClosestPointOnLineParam 12 l1 l2 p >= 0

{- | Divide a segment into a list of points with a maximal distance between
 them.
 the take 5000 here is a hack, otherwise divideLine seems to sometimes
 generate an infinite list, and I don't know why
-}
divideLine :: Float -> Point2 -> Point2 -> [Point2]
--divideLine x a b = map (\i -> a +.+ (i / (fromIntegral numPoints)) *.* (b -.- a))
divideLine x a b =
    take 5000 $
        map
            (\i -> a +.+ (fromIntegral i / fromIntegral numPoints *.* (b -.- a)))
            ns
  where
    d = dist a b
    numPoints = max 1 $ ceiling $ d / x
    ns = [0 :: Int .. numPoints]

-- | As 'divideLine', but must return an odd number of points.
divideLineOddNumPoints :: Float -> Point2 -> Point2 -> [Point2]
--divideLine x a b = map (\i -> a +.+ (i / (fromIntegral numPoints)) *.* (b -.- a))
divideLineOddNumPoints x a b =
    take 5000 $
        map
            (\i -> a +.+ (fromIntegral i / fromIntegral numPoints *.* (b -.- a)))
            ns
  where
    d = dist a b
    numPoints' = max 1 $ ceiling $ d / x
    numPoints
        | even numPoints' = numPoints'
        | otherwise = numPoints' + 1
    ns = [0 .. numPoints] :: [Int]

divideLineExact :: Float -> Point2 -> Point2 -> [Point2]
divideLineExact x a b = map ((a +.+) . (*.* v)) [0, x .. d]
  where
    d = dist a b
    v = normalizeV $ b -.- a

{- | Given two pairs of Ints, returns a list of pairs of Ints that form
 a digital line between them.
-}
digitalLine :: (Int, Int) -> (Int, Int) -> [(Int, Int)]
--{-# INLINE digitalLine #-}
digitalLine (x1, y1) (x2, y2)
    | abs (x1 - x2) > abs (y1 - y2) =
        [ (x, ((y1 - y2) * x + x1 * y2 - x2 * y1) `rdiv` (x1 - x2))
        | x <- intervalList x1 x2
        ]
    | otherwise =
        [ (((x1 - x2) * y + y1 * x2 - y2 * x1) `rdiv` (y1 - y2), y)
        | y <- intervalList y1 y2
        ]
  where
    rdiv a b = round $ fromIntegral a / (fromIntegral b :: Float)

{- | Given two pairs of 'Int's, create a list of pairs of 'Int's that form a
 rectangle between them.
-}
digitalRect :: (Int, Int) -> (Int, Int) -> [(Int, Int)]
{-# INLINE digitalRect #-}
digitalRect (a, b) (c, d) = [(s, t) | s <- [minx .. maxx], t <- [miny .. maxy]]
  where
    maxx = max a c
    minx = min a c
    maxy = max b d
    miny = min b d

-- | Given two Ints, creates the list of Ints between these.
intervalList :: Int -> Int -> [Int]
{-# INLINE intervalList #-}
intervalList x y
    | y > x = [x .. y]
    | otherwise = reverse [y .. x]

{- | Create points on the circumference of a circle with maximal distance
 between them.
-}
divideCircle :: Float -> Point2 -> Float -> [Point2]
divideCircle x cen rad = map (cen +.+) $ nRaysRad n rad
  where
    n = ceiling $ rad * 2 * pi / x

arcStepwise ::
    -- | Maximum distance between points
    Float ->
    -- | Angle to travel
    Float ->
    -- | Center
    Point2 ->
    -- | Start vector from center
    Point2 ->
    [Point2]
arcStepwise ssize a c v
    | a < 0 = reverse $ arcStepwisePositive ssize (negate a) c (rotateV a v)
    | otherwise = arcStepwisePositive ssize a c v

arcStepwisePositive ::
    -- | Maximum distance between points
    Float ->
    -- | Angle to travel, assumed to be positive
    Float ->
    -- | Center
    Point2 ->
    -- | Start vector from center
    Point2 ->
    [Point2]
arcStepwisePositive ssize a cen v = (cen +.+) . (`rotateV` v) <$> rots
  where
    rots :: [Float]
    rots = map ((a *) . (/ fromIntegral n) . fromIntegral) [0 .. n]
    n :: Int
    n = ceiling (a * magV v / ssize)

{- | Given a list of points, returns pairs of points linking the points into a
 loop.
-}
chainPairs :: [Point2] -> [(Point2, Point2)]
chainPairs [] = error "tried to make chain with empty list of points"
chainPairs [_] = error "tried to make chain with singleton list of points"
chainPairs xs = zip xs $ tail xs

{- | Given a list of points, returns pairs of points linking the points into a
 loop.
loopPairs :: [a] -> [(a,a)]
loopPairs [] = error "tried to make loop with empty list of elements"
loopPairs [_] = error "tried to make loop with singleton list of elements"
loopPairs (x:xs) = zip (x:xs) (xs ++ [x])
 | Test whether a point is in a cone.
 Note the pair is ordered.
 Doesn't work for obtuse angles.
-}
pointIsInCone ::
    -- | Cone point.
    Point2 ->
    -- | Points delimiting the left and right boundaries of the cone.
    (Point2, Point2) ->
    -- | Point to test.
    Point2 ->
    Bool
pointIsInCone c (rightp, leftp) p = isLHS c rightp p && isLHS leftp c p