-- SPDX-License-Identifier: MPL-2.0

{- | Bounded two-dimensional convex collision reference algorithms. Inputs
are finite point sets whose convex hulls are the shapes. GJK uses support
mapping and simplex reduction; EPA expands a support polytope for penetration;
SAT tests every polygon edge normal. Touching counts as collision. Caller
iteration budgets prevent unbounded work; EPA reports an estimated support-gap
error, not a formal floating-point error bound. Curved 3D primitives have
separate exact signed-distance APIs.
-}
module HMath.Geometry.Collision (
    gjk,
    Penetration (..),
    epa,
    separatingAxis,
) where

import Data.List (minimumBy, nub)
import Data.Ord (comparing)
import HMath.Error (MathError (..))
import HMath.Geometry.Internal
import HMath.Geometry.Polygon (HullAlgorithm (..), convexHull)

validate :: [Point2] -> [Point2] -> Either MathError ()
validate :: [Point2] -> [Point2] -> Either MathError ()
validate [Point2]
a [Point2]
b =
    Bool -> String -> Either MathError ()
require
        (Bool -> Bool
not ([Point2] -> Bool
forall a. [a] -> Bool
forall (t :: * -> *) a. Foldable t => t a -> Bool
null [Point2]
a Bool -> Bool -> Bool
|| [Point2] -> Bool
forall a. [a] -> Bool
forall (t :: * -> *) a. Foldable t => t a -> Bool
null [Point2]
b) Bool -> Bool -> Bool
&& [Point2] -> Int
forall a. [a] -> Int
forall (t :: * -> *) a. Foldable t => t a -> Int
length [Point2]
a Int -> Int -> Bool
forall a. Ord a => a -> a -> Bool
<= Int
256 Bool -> Bool -> Bool
&& [Point2] -> Int
forall a. [a] -> Int
forall (t :: * -> *) a. Foldable t => t a -> Int
length [Point2]
b Int -> Int -> Bool
forall a. Ord a => a -> a -> Bool
<= Int
256)
        String
"convex collision requires 1..256 points per operand"
        Either MathError () -> Either MathError () -> Either MathError ()
forall a b.
Either MathError a -> Either MathError b -> Either MathError b
forall (m :: * -> *) a b. Monad m => m a -> m b -> m b
>> (Point2 -> Either MathError ()) -> [Point2] -> Either MathError ()
forall (t :: * -> *) (m :: * -> *) a b.
(Foldable t, Monad m) =>
(a -> m b) -> t a -> m ()
mapM_ Point2 -> Either MathError ()
check2 ([Point2]
a [Point2] -> [Point2] -> [Point2]
forall a. [a] -> [a] -> [a]
++ [Point2]
b)
support :: [Point2] -> [Point2] -> Point2 -> Point2
support :: [Point2] -> [Point2] -> Point2 -> Point2
support [Point2]
a [Point2]
b Point2
direction = Point2 -> Point2 -> Point2
sub2 ([Point2] -> Point2 -> Point2
forall {t :: * -> *}. Foldable t => t Point2 -> Point2 -> Point2
extreme [Point2]
a Point2
direction) ([Point2] -> Point2 -> Point2
forall {t :: * -> *}. Foldable t => t Point2 -> Point2 -> Point2
extreme [Point2]
b (Double -> Point2 -> Point2
scale2 (-Double
1) Point2
direction))
  where
    extreme :: t Point2 -> Point2 -> Point2
extreme t Point2
ps Point2
d = (Point2 -> Point2 -> Point2) -> t Point2 -> Point2
forall a. (a -> a -> a) -> t a -> a
forall (t :: * -> *) a. Foldable t => (a -> a -> a) -> t a -> a
foldl1 (\Point2
x Point2
y -> if Point2 -> Point2 -> Double
dot2 Point2
y Point2
d Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
> Point2 -> Point2 -> Double
dot2 Point2
x Point2
d then Point2
y else Point2
x) t Point2
ps

-- Rational support and simplex signs prevent thin/small configurations from
-- spuriously containing the origin after Double products underflow. This is
-- still GJK support iteration, with exact arithmetic only for its topology.
type R2 = (Rational, Rational)
toR2 :: Point2 -> R2
toR2 :: Point2 -> R2
toR2 (P2 Double
x Double
y) = (Double -> Rational
forall a. Real a => a -> Rational
toRational Double
x, Double -> Rational
forall a. Real a => a -> Rational
toRational Double
y)
rsub :: R2 -> R2 -> R2
rsub :: R2 -> R2 -> R2
rsub (Rational
x, Rational
y) (Rational
a, Rational
b) = (Rational
x Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
- Rational
a, Rational
y Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
- Rational
b)
rscale :: Rational -> R2 -> R2
rscale :: Rational -> R2 -> R2
rscale Rational
a (Rational
x, Rational
y) = (Rational
a Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
* Rational
x, Rational
a Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
* Rational
y)
rdot :: R2 -> R2 -> Rational
rdot :: R2 -> R2 -> Rational
rdot (Rational
x, Rational
y) (Rational
a, Rational
b) = Rational
x Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
* Rational
a Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
+ Rational
y Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
* Rational
b
rcross :: R2 -> R2 -> Rational
rcross :: R2 -> R2 -> Rational
rcross (Rational
x, Rational
y) (Rational
a, Rational
b) = Rational
x Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
* Rational
b Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
- Rational
y Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
* Rational
a
rtriple :: R2 -> R2 -> R2 -> R2
rtriple :: R2 -> R2 -> R2 -> R2
rtriple R2
a R2
b R2
c = R2 -> R2 -> R2
rsub (Rational -> R2 -> R2
rscale (R2 -> R2 -> Rational
rdot R2
a R2
c) R2
b) (Rational -> R2 -> R2
rscale (R2 -> R2 -> Rational
rdot R2
b R2
c) R2
a)
reduce :: [R2] -> (Bool, [R2], R2)
reduce :: [R2] -> (Bool, [R2], R2)
reduce [R2
a, R2
b] =
    let ab :: R2
ab = R2 -> R2 -> R2
rsub R2
b R2
a; ao :: R2
ao = Rational -> R2 -> R2
rscale (-Rational
1) R2
a; projection :: Rational
projection = R2 -> R2 -> Rational
rdot R2
ab R2
ao
     in if Rational
projection Rational -> Rational -> Bool
forall a. Ord a => a -> a -> Bool
<= Rational
0
            then (R2
a R2 -> R2 -> Bool
forall a. Eq a => a -> a -> Bool
== (Rational
0, Rational
0), [R2
a], R2
ao)
            else
                if R2 -> R2 -> Rational
rcross R2
ab R2
ao Rational -> Rational -> Bool
forall a. Eq a => a -> a -> Bool
== Rational
0
                    then
                        if Rational
projection Rational -> Rational -> Bool
forall a. Ord a => a -> a -> Bool
<= R2 -> R2 -> Rational
rdot R2
ab R2
ab
                            then (Bool
True, [R2
a, R2
b], (Rational
0, Rational
0))
                            else (R2
b R2 -> R2 -> Bool
forall a. Eq a => a -> a -> Bool
== (Rational
0, Rational
0), [R2
b], Rational -> R2 -> R2
rscale (-Rational
1) R2
b)
                    else (Bool
False, [R2
a, R2
b], R2 -> R2 -> R2 -> R2
rtriple R2
ab R2
ao R2
ab)
reduce [R2
a, R2
b, R2
c] =
    let ab :: R2
ab = R2 -> R2 -> R2
rsub R2
b R2
a; ac :: R2
ac = R2 -> R2 -> R2
rsub R2
c R2
a; ao :: R2
ao = Rational -> R2 -> R2
rscale (-Rational
1) R2
a; left :: R2
left = R2 -> R2 -> R2 -> R2
rtriple R2
ac R2
ab R2
ab; right :: R2
right = R2 -> R2 -> R2 -> R2
rtriple R2
ab R2
ac R2
ac
     in if R2 -> R2 -> Rational
rdot R2
left R2
ao Rational -> Rational -> Bool
forall a. Ord a => a -> a -> Bool
> Rational
0
            then [R2] -> (Bool, [R2], R2)
reduce [R2
a, R2
b]
            else if R2 -> R2 -> Rational
rdot R2
right R2
ao Rational -> Rational -> Bool
forall a. Ord a => a -> a -> Bool
> Rational
0 then [R2] -> (Bool, [R2], R2)
reduce [R2
a, R2
c] else (Bool
True, [R2
a, R2
b, R2
c], (Rational
0, Rational
0))
reduce [R2
a] = (R2
a R2 -> R2 -> Bool
forall a. Eq a => a -> a -> Bool
== (Rational
0, Rational
0), [R2
a], Rational -> R2 -> R2
rscale (-Rational
1) R2
a)
reduce [R2]
_ = (Bool
False, [], (Rational
1, Rational
0))
gjkSimplex :: Int -> [Point2] -> [Point2] -> Either MathError (Bool, [Point2])
gjkSimplex :: Int -> [Point2] -> [Point2] -> Either MathError (Bool, [Point2])
gjkSimplex Int
budget [Point2]
a [Point2]
b = do
    [Point2] -> [Point2] -> Either MathError ()
validate [Point2]
a [Point2]
b
    Bool -> String -> Either MathError ()
require (Int
budget Int -> Int -> Bool
forall a. Ord a => a -> a -> Bool
> Int
0 Bool -> Bool -> Bool
&& Int
budget Int -> Int -> Bool
forall a. Ord a => a -> a -> Bool
<= Int
100000) String
"invalid GJK iteration budget"
    let aa :: [R2]
aa = (Point2 -> R2) -> [Point2] -> [R2]
forall a b. (a -> b) -> [a] -> [b]
map Point2 -> R2
toR2 [Point2]
a
        bb :: [R2]
bb = (Point2 -> R2) -> [Point2] -> [R2]
forall a b. (a -> b) -> [a] -> [b]
map Point2 -> R2
toR2 [Point2]
b
        extreme :: t R2 -> R2 -> R2
extreme t R2
ps R2
direction = (R2 -> R2 -> R2) -> t R2 -> R2
forall a. (a -> a -> a) -> t a -> a
forall (t :: * -> *) a. Foldable t => (a -> a -> a) -> t a -> a
foldl1 (\R2
x R2
y -> if R2 -> R2 -> Rational
rdot R2
y R2
direction Rational -> Rational -> Bool
forall a. Ord a => a -> a -> Bool
> R2 -> R2 -> Rational
rdot R2
x R2
direction then R2
y else R2
x) t R2
ps
        supporting :: R2 -> R2
supporting R2
direction = R2 -> R2 -> R2
rsub ([R2] -> R2 -> R2
forall {t :: * -> *}. Foldable t => t R2 -> R2 -> R2
extreme [R2]
aa R2
direction) ([R2] -> R2 -> R2
forall {t :: * -> *}. Foldable t => t R2 -> R2 -> R2
extreme [R2]
bb (Rational -> R2 -> R2
rscale (-Rational
1) R2
direction))
        initial :: R2
initial = R2 -> R2
supporting (Rational
1, Rational
0)
        step :: Int -> [R2] -> R2 -> Either MathError (Bool, [R2])
step Int
used [R2]
simplex R2
direction
            | R2
direction R2 -> R2 -> Bool
forall a. Eq a => a -> a -> Bool
== (Rational
0, Rational
0) = (Bool, [R2]) -> Either MathError (Bool, [R2])
forall a b. b -> Either a b
Right (Bool
True, [R2]
simplex)
            | Int
used Int -> Int -> Bool
forall a. Ord a => a -> a -> Bool
>= Int
budget = MathError -> Either MathError (Bool, [R2])
forall a b. a -> Either a b
Left (String -> Int -> MathError
NonConvergence String
"GJK" Int
used)
            | Bool
otherwise =
                let p :: R2
p = R2 -> R2
supporting R2
direction
                 in if R2 -> R2 -> Rational
rdot R2
p R2
direction Rational -> Rational -> Bool
forall a. Ord a => a -> a -> Bool
< Rational
0
                        then (Bool, [R2]) -> Either MathError (Bool, [R2])
forall a b. b -> Either a b
Right (Bool
False, [R2]
simplex)
                        else
                            if R2
p R2 -> [R2] -> Bool
forall a. Eq a => a -> [a] -> Bool
forall (t :: * -> *) a. (Foldable t, Eq a) => a -> t a -> Bool
`elem` [R2]
simplex
                                then (Bool, [R2]) -> Either MathError (Bool, [R2])
forall a b. b -> Either a b
Right (Bool
False, [R2]
simplex)
                                else
                                    let (Bool
hit, [R2]
remaining, R2
next) = [R2] -> (Bool, [R2], R2)
reduce (R2
p R2 -> [R2] -> [R2]
forall a. a -> [a] -> [a]
: [R2]
simplex)
                                     in if Bool
hit then (Bool, [R2]) -> Either MathError (Bool, [R2])
forall a b. b -> Either a b
Right (Bool
True, [R2]
remaining) else Int -> [R2] -> R2 -> Either MathError (Bool, [R2])
step (Int
used Int -> Int -> Int
forall a. Num a => a -> a -> a
+ Int
1) [R2]
remaining R2
next
    (hit, points) <-
        if R2
initial R2 -> R2 -> Bool
forall a. Eq a => a -> a -> Bool
== (Rational
0, Rational
0) then (Bool, [R2]) -> Either MathError (Bool, [R2])
forall a b. b -> Either a b
Right (Bool
True, [R2
initial]) else Int -> [R2] -> R2 -> Either MathError (Bool, [R2])
step Int
0 [R2
initial] (Rational -> R2 -> R2
rscale (-Rational
1) R2
initial)
    Right (hit, [P2 (fromRational x) (fromRational y) | (x, y) <- points])

-- | Convex intersection by Gilbert-Johnson-Keerthi support/simplex iterations.
gjk :: Int -> [Point2] -> [Point2] -> Either MathError Bool
gjk :: Int -> [Point2] -> [Point2] -> Either MathError Bool
gjk Int
budget [Point2]
a [Point2]
b = (Bool, [Point2]) -> Bool
forall a b. (a, b) -> a
fst ((Bool, [Point2]) -> Bool)
-> Either MathError (Bool, [Point2]) -> Either MathError Bool
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> Int -> [Point2] -> [Point2] -> Either MathError (Bool, [Point2])
gjkSimplex Int
budget [Point2]
a [Point2]
b

{- | Normal moves the second shape away from the first by penetrationDepth.
A zero-depth touch may have the deterministic normal (1,0).
-}
data Penetration = Penetration
    { Penetration -> Double
penetrationDepth :: Double
    -- ^ Estimated overlap depth along penetrationNormal, in coordinate units.
    , Penetration -> Point2
penetrationNormal :: Point2
    -- ^ Separating direction of the returned EPA penetration estimate.
    , Penetration -> Double
penetrationError :: Double
    -- ^ Remaining absolute support gap at the stopping iteration.
    , Penetration -> Int
penetrationIterations :: Int
    -- ^ Completed expanding-polytope iterations.
    }
    deriving (Penetration -> Penetration -> Bool
(Penetration -> Penetration -> Bool)
-> (Penetration -> Penetration -> Bool) -> Eq Penetration
forall a. (a -> a -> Bool) -> (a -> a -> Bool) -> Eq a
$c== :: Penetration -> Penetration -> Bool
== :: Penetration -> Penetration -> Bool
$c/= :: Penetration -> Penetration -> Bool
/= :: Penetration -> Penetration -> Bool
Eq, Int -> Penetration -> ShowS
[Penetration] -> ShowS
Penetration -> String
(Int -> Penetration -> ShowS)
-> (Penetration -> String)
-> ([Penetration] -> ShowS)
-> Show Penetration
forall a.
(Int -> a -> ShowS) -> (a -> String) -> ([a] -> ShowS) -> Show a
$cshowsPrec :: Int -> Penetration -> ShowS
showsPrec :: Int -> Penetration -> ShowS
$cshow :: Penetration -> String
show :: Penetration -> String
$cshowList :: [Penetration] -> ShowS
showList :: [Penetration] -> ShowS
Show)

{- | Expanding polytope penetration estimate. The tolerance is an absolute
support-gap stopping threshold, positive finite and <=1e50.
-}
epa :: Int -> Double -> [Point2] -> [Point2] -> Either MathError (Maybe Penetration)
epa :: Int
-> Double
-> [Point2]
-> [Point2]
-> Either MathError (Maybe Penetration)
epa Int
budget Double
tolerance [Point2]
a [Point2]
b = do
    Bool -> String -> Either MathError ()
require
        (Int
budget Int -> Int -> Bool
forall a. Ord a => a -> a -> Bool
> Int
0 Bool -> Bool -> Bool
&& Int
budget Int -> Int -> Bool
forall a. Ord a => a -> a -> Bool
<= Int
100000 Bool -> Bool -> Bool
&& Double -> Bool
coord Double
tolerance Bool -> Bool -> Bool
&& Double
tolerance Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
> Double
0)
        String
"invalid EPA tolerance/budget"
    (hit, simplex) <- Int -> [Point2] -> [Point2] -> Either MathError (Bool, [Point2])
gjkSimplex Int
budget [Point2]
a [Point2]
b
    if not hit
        then Right Nothing
        else do
            poly <-
                convexHull
                    MonotoneChain
                    (nub (simplex ++ [support a b d | d <- [P2 1 0, P2 (-1) 0, P2 0 1, P2 0 (-1)]]))
            if length poly < 3 then Right (Just (Penetration 0 (P2 1 0) 0 0)) else expand poly 0
  where
    expand :: [Point2] -> Int -> Either MathError (Maybe Penetration)
expand [Point2]
poly Int
iteration
        | Int
iteration Int -> Int -> Bool
forall a. Ord a => a -> a -> Bool
>= Int
budget = MathError -> Either MathError (Maybe Penetration)
forall a b. a -> Either a b
Left (String -> Int -> MathError
NonConvergence String
"EPA" Int
iteration)
        | Bool
otherwise =
            let candidate :: (b, (Point2, Point2)) -> (Double, b, Point2)
candidate (b
edgeIndex, (Point2
u, Point2
v)) =
                    let P2 Double
dx Double
dy = Point2 -> Point2 -> Point2
sub2 Point2
v Point2
u; size :: Double
size = Point2 -> Double
norm2 (Point2 -> Point2 -> Point2
sub2 Point2
v Point2
u); n :: Point2
n = Double -> Double -> Point2
P2 (Double
dy Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
size) (-Double
dx Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
size); edgeDepth :: Double
edgeDepth = Point2 -> Point2 -> Double
dot2 Point2
n Point2
u
                     in (Double
edgeDepth, b
edgeIndex, Point2
n)
                (Double
depth, Int
index, Point2
normal) = ((Double, Int, Point2) -> (Double, Int, Point2) -> Ordering)
-> [(Double, Int, Point2)] -> (Double, Int, Point2)
forall (t :: * -> *) a.
Foldable t =>
(a -> a -> Ordering) -> t a -> a
minimumBy (((Double, Int, Point2) -> Double)
-> (Double, Int, Point2) -> (Double, Int, Point2) -> Ordering
forall a b. Ord a => (b -> a) -> b -> b -> Ordering
comparing (\(Double
d, Int
_, Point2
_) -> Double
d)) (((Int, (Point2, Point2)) -> (Double, Int, Point2))
-> [(Int, (Point2, Point2))] -> [(Double, Int, Point2)]
forall a b. (a -> b) -> [a] -> [b]
map (Int, (Point2, Point2)) -> (Double, Int, Point2)
forall {b}. (b, (Point2, Point2)) -> (Double, b, Point2)
candidate ([Int] -> [(Point2, Point2)] -> [(Int, (Point2, Point2))]
forall a b. [a] -> [b] -> [(a, b)]
zip [Int
0 :: Int ..] ([Point2] -> [(Point2, Point2)]
forall a. [a] -> [(a, a)]
edges [Point2]
poly)))
                p :: Point2
p = [Point2] -> [Point2] -> Point2 -> Point2
support [Point2]
a [Point2]
b Point2
normal
                gap :: Double
gap = Point2 -> Point2 -> Double
dot2 Point2
normal Point2
p Double -> Double -> Double
forall a. Num a => a -> a -> a
- Double
depth
             in if Double
gap Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
<= Double
tolerance Bool -> Bool -> Bool
|| Point2
p Point2 -> [Point2] -> Bool
forall a. Eq a => a -> [a] -> Bool
forall (t :: * -> *) a. (Foldable t, Eq a) => a -> t a -> Bool
`elem` [Point2]
poly
                    then Maybe Penetration -> Either MathError (Maybe Penetration)
forall a b. b -> Either a b
Right (Penetration -> Maybe Penetration
forall a. a -> Maybe a
Just (Double -> Point2 -> Double -> Int -> Penetration
Penetration (Double -> Double -> Double
forall a. Ord a => a -> a -> a
max Double
0 Double
depth) Point2
normal (Double -> Double -> Double
forall a. Ord a => a -> a -> a
max Double
0 Double
gap) Int
iteration))
                    else [Point2] -> Int -> Either MathError (Maybe Penetration)
expand (Int -> [Point2] -> [Point2]
forall a. Int -> [a] -> [a]
take (Int
index Int -> Int -> Int
forall a. Num a => a -> a -> a
+ Int
1) [Point2]
poly [Point2] -> [Point2] -> [Point2]
forall a. [a] -> [a] -> [a]
++ [Point2
p] [Point2] -> [Point2] -> [Point2]
forall a. [a] -> [a] -> [a]
++ Int -> [Point2] -> [Point2]
forall a. Int -> [a] -> [a]
drop (Int
index Int -> Int -> Int
forall a. Num a => a -> a -> a
+ Int
1) [Point2]
poly) (Int
iteration Int -> Int -> Int
forall a. Num a => a -> a -> a
+ Int
1)

{- | Separating-axis collision and minimum overlap for convex polygons. Shapes
must have nonzero area; containment depth uses the distance to the nearer
separating interval endpoint, rather than only the interval intersection.
-}
separatingAxis :: [Point2] -> [Point2] -> Either MathError (Maybe Penetration)
separatingAxis :: [Point2] -> [Point2] -> Either MathError (Maybe Penetration)
separatingAxis [Point2]
a [Point2]
b = do
    [Point2] -> [Point2] -> Either MathError ()
validate [Point2]
a [Point2]
b
    x <- HullAlgorithm -> [Point2] -> Either MathError [Point2]
convexHull HullAlgorithm
MonotoneChain [Point2]
a
    y <- convexHull MonotoneChain b
    require (length x >= 3 && length y >= 3) "SAT requires nonzero-area convex polygons"
    let axes =
            [ let P2 Double
dx Double
dy = Point2 -> Point2 -> Point2
sub2 Point2
v Point2
u; size :: Double
size = Point2 -> Double
norm2 (Point2 -> Point2 -> Point2
sub2 Point2
v Point2
u) in Double -> Double -> Point2
P2 (Double
dy Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
size) (-Double
dx Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
size)
            | (Point2
u, Point2
v) <- [Point2] -> [(Point2, Point2)]
forall a. [a] -> [(a, a)]
edges [Point2]
x [(Point2, Point2)] -> [(Point2, Point2)] -> [(Point2, Point2)]
forall a. [a] -> [a] -> [a]
++ [Point2] -> [(Point2, Point2)]
forall a. [a] -> [(a, a)]
edges [Point2]
y
            ]
        project Point2
axis [Point2]
ps = let vals :: [Double]
vals = (Point2 -> Double) -> [Point2] -> [Double]
forall a b. (a -> b) -> [a] -> [b]
map (Point2 -> Point2 -> Double
dot2 Point2
axis) [Point2]
ps in ([Double] -> Double
forall a. Ord a => [a] -> a
forall (t :: * -> *) a. (Foldable t, Ord a) => t a -> a
minimum [Double]
vals, [Double] -> Double
forall a. Ord a => [a] -> a
forall (t :: * -> *) a. (Foldable t, Ord a) => t a -> a
maximum [Double]
vals)
        test Point2
axis =
            let (Double
al, Double
ah) = Point2 -> [Point2] -> (Double, Double)
project Point2
axis [Point2]
x; (Double
bl, Double
bh) = Point2 -> [Point2] -> (Double, Double)
project Point2
axis [Point2]
y; positive :: Double
positive = Double
ah Double -> Double -> Double
forall a. Num a => a -> a -> a
- Double
bl; negative :: Double
negative = Double
bh Double -> Double -> Double
forall a. Num a => a -> a -> a
- Double
al
             in if Double
positive Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
< Double
0 Bool -> Bool -> Bool
|| Double
negative Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
< Double
0
                    then Maybe (Double, Point2)
forall a. Maybe a
Nothing
                    else if Double
positive Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
<= Double
negative then (Double, Point2) -> Maybe (Double, Point2)
forall a. a -> Maybe a
Just (Double
positive, Point2
axis) else (Double, Point2) -> Maybe (Double, Point2)
forall a. a -> Maybe a
Just (Double
negative, Double -> Point2 -> Point2
scale2 (-Double
1) Point2
axis)
    case mapM test axes of
        Maybe [(Double, Point2)]
Nothing -> Maybe Penetration -> Either MathError (Maybe Penetration)
forall a b. b -> Either a b
Right Maybe Penetration
forall a. Maybe a
Nothing
        Just [(Double, Point2)]
hits ->
            let (Double
depth, Point2
normal) = ((Double, Point2) -> (Double, Point2) -> Ordering)
-> [(Double, Point2)] -> (Double, Point2)
forall (t :: * -> *) a.
Foldable t =>
(a -> a -> Ordering) -> t a -> a
minimumBy (((Double, Point2) -> Double)
-> (Double, Point2) -> (Double, Point2) -> Ordering
forall a b. Ord a => (b -> a) -> b -> b -> Ordering
comparing (Double, Point2) -> Double
forall a b. (a, b) -> a
fst) [(Double, Point2)]
hits
             in Maybe Penetration -> Either MathError (Maybe Penetration)
forall a b. b -> Either a b
Right (Penetration -> Maybe Penetration
forall a. a -> Maybe a
Just (Double -> Point2 -> Double -> Int -> Penetration
Penetration Double
depth Point2
normal Double
0 ([Point2] -> Int
forall a. [a] -> Int
forall (t :: * -> *) a. Foldable t => t a -> Int
length [Point2]
axes)))