{-# LANGUAGE MagicHash #-}
module Physics.Contact.GJK where
import GHC.Prim
import GHC.Types (Double (D#), isTrue#)
import Physics.Contact.ConvexHull
import Physics.Linear
import Utils.Utils
data Simplex3 =
Simplex3 !Neighborhood
!Neighborhood
!Neighborhood
deriving (Show)
data Simplex2 =
Simplex2 !Neighborhood
!Neighborhood
deriving (Show)
data Simplex1 =
Simplex1 !Neighborhood
deriving (Show)
type Simplex12 = Either Simplex1 Simplex2
type Simplex23 = Either Simplex2 Simplex3
data Simplex
= Simplex' Simplex12
| Simplex3' Simplex3
deriving (Show)
closestSimplex :: ConvexHull -> P2 -> Simplex
closestSimplex hull origin = loop (Left $ Simplex1 a) d
where
a = _hullNeighborhood 0 hull
d = diffP2 origin $ _neighborhoodCenter a
loop :: Simplex12 -> V2 -> Simplex
loop simplex d =
case extendSimplex simplex aa of
Nothing -> Simplex' simplex
Just simplex ->
case shiftSimplex simplex origin of
Right simplex -> Simplex3' simplex
Left (simplex, d) -> loop simplex d
where
aa = support hull d
a = _neighborhoodCenter aa
ao = diffP2 a origin
extendSimplex :: Simplex12 -> Neighborhood -> Maybe Simplex23
extendSimplex (Left simplex) aa =
Left <$> extendSimplex1 simplex aa
extendSimplex (Right simplex) aa = Right <$> extendSimplex2 simplex aa
extendSimplex1 :: Simplex1 -> Neighborhood -> Maybe Simplex2
extendSimplex1 simplex@(Simplex1 bb) aa
| _neighborhoodIndex bb == _neighborhoodIndex aa = Nothing
| otherwise = Just $ mkSimplex2 aa simplex
extendSimplex2 :: Simplex2 -> Neighborhood -> Maybe Simplex3
extendSimplex2 simplex@(Simplex2 bb cc) aa
| bi == ai || ci == ai = Nothing
| otherwise = Just $ mkSimplex3 aa simplex
where ai = _neighborhoodIndex aa
bi = _neighborhoodIndex bb
ci = _neighborhoodIndex cc
shiftSimplex :: Simplex23 -> P2 -> Either (Simplex12, V2) Simplex3
shiftSimplex (Left simplex) origin = Left $ shiftSimplex2 simplex origin
shiftSimplex (Right simplex) origin =
case shiftSimplex3 simplex origin of
Nothing -> Right simplex
Just result -> Left result
shiftSimplex2 :: Simplex2
-> P2
-> (Simplex12, V2)
shiftSimplex2 aabb@(Simplex2 aa bb) origin
| sameDirection ab ao = (Right aabb, crossV2V2 ab ao ab)
| otherwise = (Left $ Simplex1 aa, ao)
where
a = _neighborhoodCenter aa
b = _neighborhoodCenter bb
ab = diffP2 b a
ao = diffP2 origin a
shiftSimplex3 :: Simplex3
-> P2
-> Maybe (Simplex12, V2)
shiftSimplex3 (Simplex3 aa bb cc) origin
| sameDirection abcac ao =
if sameDirection ac ao
then Just (Right $ Simplex2 aa cc, crossV2V2 ac ao ac)
else Just star
| sameDirection ababc ao = Just star
| otherwise = Nothing
where
a = _neighborhoodCenter aa
b = _neighborhoodCenter bb
c = _neighborhoodCenter cc
ab = diffP2 b a
ac = diffP2 c a
ao = diffP2 origin a
abc = ab `crossV2` ac
abcac = abc `zcrossV2` ac
ababc = ab `crosszV2` abc
star =
if sameDirection ab ao
then (Right $ Simplex2 aa bb, crossV2V2 ab ao ab)
else (Left $ Simplex1 aa, ao)
sameDirection :: V2 -> V2 -> Bool
sameDirection a b = isTrue# (a `dotV2` b >## 0.0##)
mkSimplex3 :: Neighborhood -> Simplex2 -> Simplex3
mkSimplex3 aa (Simplex2 bb cc) = Simplex3 aa bb cc
mkSimplex2 :: Neighborhood -> Simplex1 -> Simplex2
mkSimplex2 aa (Simplex1 bb) = Simplex2 aa bb