{-# LANGUAGE Trustworthy #-}
module GJK
  ( Convex (..)
  , convexIntersect
  )
where

import Linear (Epsilon (nearZero), Metric (dot), V2, crossZ, unangle)

-- | Convex is represented by its supprot function
newtype Convex = Convex { Convex -> Double -> V2 Double
support :: Double -> V2 Double }

-- | Minkowski Difference between 2 Convexes
minkowskiDifference :: Convex -> Convex -> Convex
minkowskiDifference :: Convex -> Convex -> Convex
minkowskiDifference Convex
a Convex
b = (Double -> V2 Double) -> Convex
Convex ((Double -> V2 Double) -> Convex)
-> (Double -> V2 Double) -> Convex
forall a b. (a -> b) -> a -> b
$ (-) (V2 Double -> V2 Double -> V2 Double)
-> (Double -> V2 Double) -> Double -> V2 Double -> V2 Double
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> Convex -> Double -> V2 Double
support Convex
a (Double -> V2 Double -> V2 Double)
-> (Double -> V2 Double) -> Double -> V2 Double
forall (f :: * -> *) a b. Applicative f => f (a -> b) -> f a -> f b
<*> (Convex -> Double -> V2 Double
support Convex
b (Double -> V2 Double) -> (Double -> Double) -> Double -> V2 Double
forall b c a. (b -> c) -> (a -> b) -> a -> c
. (Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
forall a. Floating a => a
pi))

-- | Core Logic: Detect if origin point is contained inside a Convex
originInside :: Convex -> Bool
originInside :: Convex -> Bool
originInside Convex
s =
  if V2 Double -> V2 Double -> Double
forall (f :: * -> *) a. (Metric f, Num a) => f a -> f a -> a
dot V2 Double
a V2 Double
b Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
> Double
0
    then Bool
False
    else (Double -> V2 Double) -> V2 Double -> V2 Double -> Bool
doSimplex (Convex -> Double -> V2 Double
support Convex
s) V2 Double
a V2 Double
b
  where
    a :: V2 Double
a = Convex -> Double -> V2 Double
support Convex
s Double
0
    b :: V2 Double
b = Convex -> Double -> V2 Double
support Convex
s (Double -> V2 Double) -> Double -> V2 Double
forall a b. (a -> b) -> a -> b
$ V2 Double -> Double
forall a. (Floating a, Ord a) => V2 a -> a
unangle (-V2 Double
a)

-- | Simplex addition till nearZero
doSimplex :: (Double -> V2 Double) -> V2 Double -> V2 Double -> Bool
doSimplex :: (Double -> V2 Double) -> V2 Double -> V2 Double -> Bool
doSimplex Double -> V2 Double
s V2 Double
a V2 Double
b =
  V2 Double -> Bool
forall a. Epsilon a => a -> Bool
nearZero (V2 Double
b V2 Double -> V2 Double -> V2 Double
forall a. Num a => a -> a -> a
- V2 Double
a) Bool -> Bool -> Bool
||
  (if (Double
axb Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
> Double
0) Bool -> Bool -> Bool
forall a. Eq a => a -> a -> Bool
/= (Double
bxc Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
> Double
0)
     then (Double -> V2 Double) -> V2 Double -> V2 Double -> Bool
doSimplex Double -> V2 Double
s V2 Double
b V2 Double
c
     else (if (Double
axc Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
> Double
0) Bool -> Bool -> Bool
forall a. Eq a => a -> a -> Bool
/= (Double
cxb Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
> Double
0)
             then (Double -> V2 Double) -> V2 Double -> V2 Double -> Bool
doSimplex Double -> V2 Double
s V2 Double
a V2 Double
c
             else Bool
True))
  where
    c :: V2 Double
c = Double -> V2 Double
s (Double -> V2 Double) -> Double -> V2 Double
forall a b. (a -> b) -> a -> b
$ V2 Double -> Double
forall a. (Floating a, Ord a) => V2 a -> a
unangle (V2 Double
b V2 Double -> V2 Double -> V2 Double
forall a. Num a => a -> a -> a
- V2 Double
a) Double -> Double -> Double
forall a. Num a => a -> a -> a
+ (if Double
axb Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
> Double
0 then Double
forall a. Floating a => a
pi Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
2 else -Double
forall a. Floating a => a
pi Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
2)
    axb :: Double
axb = V2 Double -> V2 Double -> Double
forall a. Num a => V2 a -> V2 a -> a
crossZ V2 Double
a V2 Double
b
    axc :: Double
axc = V2 Double -> V2 Double -> Double
forall a. Num a => V2 a -> V2 a -> a
crossZ V2 Double
a V2 Double
c
    bxc :: Double
bxc = V2 Double -> V2 Double -> Double
forall a. Num a => V2 a -> V2 a -> a
crossZ V2 Double
b V2 Double
c
    cxb :: Double
cxb = -Double
bxc

-- | If two Convex colliside
convexIntersect :: Convex -> Convex -> Bool
convexIntersect :: Convex -> Convex -> Bool
convexIntersect Convex
a Convex
b = Convex -> Bool
originInside (Convex -> Bool) -> Convex -> Bool
forall a b. (a -> b) -> a -> b
$ Convex -> Convex -> Convex
minkowskiDifference Convex
a Convex
b