module GJK where
import Linear
newtype Convex = Convex { support :: Double -> V2 Double }
minkowskiDifference :: Convex -> Convex -> Convex
minkowskiDifference a b = Convex $ () <$> support a <*> (support b . (+ pi))
unAngle :: V2 Double -> Double
unAngle (V2 ax ay) =
let alpha = asin $ ay / (ax * ax + ay * ay)
in if ax < 0
then pi alpha
else alpha
originInside :: Convex -> Bool
originInside s =
if dot a b > 0
then False
else doSimplex (support s) a b
where
a = support s 0
b = support s $ unAngle (a)
doSimplex :: (Double -> V2 Double) -> V2 Double -> V2 Double -> Bool
doSimplex s a b =
nearZero (b a) ||
(if (axb > 0) /= (bxc > 0)
then doSimplex s b c
else (if (axc > 0) /= (cxb > 0)
then doSimplex s a c
else True))
where
c = s $ unAngle (b a) + (if axb > 0 then pi / 2 else pi / 2)
axb = crossZ a b
axc = crossZ a c
bxc = crossZ b c
cxb = bxc
convexIntersect :: Convex -> Convex -> Bool
convexIntersect a b = originInside $ minkowskiDifference a b