{-# LANGUAGE MagicHash #-}
{-# LANGUAGE RecordWildCards #-}
{-# LANGUAGE TupleSections #-}
module Physics.Contact.CircleVsHull where
import GHC.Types (Double (..))
import Data.Either
import Physics.Contact.Circle
import Physics.Contact.ConvexHull
import Physics.Contact.GJK
import Physics.Contact.Types
import Physics.Linear
import Utils.Utils
generateContacts :: Circle -> ConvexHull -> Maybe (Int, Contact')
generateContacts circle@Circle {..} hull = convertSimplex circle simplex
where
simplex = closestSimplex hull _circleCenter
convertSimplex :: Circle -> Simplex -> Maybe (Int, Contact')
convertSimplex circle (Simplex' simplex) = convertSimplex12 circle simplex
convertSimplex _ (Simplex3' _) = Nothing
convertSimplex12 :: Circle -> Simplex12 -> Maybe (Int, Contact')
convertSimplex12 circle = either (processSimplex1 circle) (processSimplex2 circle)
processSimplex1 :: Circle -> Simplex1 -> Maybe (Int, Contact')
processSimplex1 circle (Simplex1 aa) =
(_neighborhoodIndex aa, ) <$> processSimplex_ circle (_neighborhoodCenter aa)
processSimplex2 :: Circle -> Simplex2 -> Maybe (Int, Contact')
processSimplex2 circle@Circle {..} simplex@(Simplex2 feature _) =
(_neighborhoodIndex feature, ) <$> processSimplex_ circle a
where
a = _circleCenter `closestAlong` simplex
processSimplex_ :: Circle -> P2 -> Maybe Contact'
processSimplex_ Circle {..} a
| sqRadius < abSq = Nothing
| otherwise =
Just $
Contact'
{ _contactEdgeNormal' = negateV2 normal
, _contactPenetrator' = a
, _contactDepth' = _circleRadius - abLength
}
where
b = _circleCenter
sqRadius = _circleRadius * _circleRadius
ab = diffP2 b a
abSq = sqLengthV2 ab
abLength = sqrt abSq
normal = sdivV2 abLength ab
closestAlong :: P2
-> Simplex2
-> P2
o `closestAlong` (Simplex2 aa bb) = aoAlong `vplusP2` a
where a = _neighborhoodCenter aa
b = _neighborhoodCenter bb
ao = diffP2 o a
ab = diffP2 b a
abNorm = normalizeV2 ab
aoAlong = D# (ao `dotV2` abNorm) `smulV2` abNorm