pituicat/src/Classes/Physics/Collidible.hs

{-# LANGUAGE AllowAmbiguousTypes #-}
{-# LANGUAGE OverloadedStrings #-}
module Classes.Physics.Collidible where

import Affection as A

import Linear

import Data.String (fromString)

import Data.List (minimumBy)

-- internal imports

import Classes.Physics.Mass

data CollisionResult time direction
  = NoCollision
  | Collision time direction
  deriving (Show, Eq)

-- | Typeclass for implementing collision results on objects.
class (Show c, Mass c) => Collidible c where

  -- | returns the bottom left and top right corners relative to the objects
  -- positional vector of the axis aligned bounding box (AABB) serving here
  -- as collision boundaries.
  boundary
    :: c            -- ^ Object
    -> ( V2 Double  -- ^ Bottom left corner of AABB relative to position
       , V2 Double  -- ^ Top right corner of AABB relative to position
       )

  collisionCheck
    :: (Collidible other)
    => Double                          -- ^ Time step length
    -> c                               -- ^ First object
    -> other                           -- ^ second object
    -> CollisionResult Double (V2 Int) -- ^ Do the objects collide?
  collisionCheck dt m1 m2 =
    let d1@(V2 d1x d1y) = velocity m1
        d2@(V2 d2x d2y) = velocity m2
        p1 = position m1
        p2 = position m2
        (m1b1@(V2 m1b1x m1b1y), m1b2@(V2 m1b2x m1b2y)) = boundary m1
        (m2b1@(V2 m2b1x m2b1y), m2b2@(V2 m2b2x m2b2y)) = boundary m2
        m1p1 = m1b1
        m1p2 = V2 m1b1x m1b2y
        m1p3 = m1b2
        m1p4 = V2 m1b2x m1b1y
        m2p1 = m2b1
        m2p2 = V2 m2b1x m2b2y
        m2p3 = m2b2
        m2p4 = V2 m2b2x m2b1y
        quad1 = map (p1 +) [m1p1, m1p2, m1p3, m1p4]
        quad2 = map (p2 +) [m2p1, m2p2, m2p3, m2p4]
        g11 = (m1p1, m1p2)
        g12 = (m1p2, m1p3)
        g13 = (m1p3, m1p4)
        g14 = (m1p4, m1p1)
        g21 = (m2p1, m2p2)
        g22 = (m2p2, m2p3)
        g23 = (m2p3, m2p4)
        g24 = (m2p4, m2p1)
        g1s = map (\(s1, s2) -> (p1 + s1, p1 + s2)) [g11, g12, g13, g14]
        g2s = map (\(s1, s2) -> (p2 + s1, p2 + s2)) [g21, g22, g23, g24]
        t1 q@(V2 mqpx mqpy) (ps@(V2 mspx mspy), pt@(V2 mtpx mtpy)) =
          if d1x == 0 && d1y == 0
          then dt
          else
            let (V2 cx cy) = ps - q
                (V2 rx ry) = pt - ps
            in
              (cx * (-ry) - (-rx) * cy) /
              (d1x * (-ry) - (-rx) * d1y)
        t2 q@(V2 mqpx mqpy) (ps@(V2 mspx mspy), pt@(V2 mtpx mtpy)) =
          if d2x == 0 && d2y == 0
          then dt
          else
            let (V2 cx cy) = ps - q
                (V2 rx ry) = pt - ps
            in
              (cx * (-ry) - (-rx) * cy) /
              (d2x * (-ry) - (-rx) * d2y)
        s1 q@(V2 mqpx mqpy) (ps@(V2 mspx mspy), pt@(V2 mtpx mtpy)) =
          let (V2 cx cy) = ps - q
              (V2 rx ry) = pt - ps
          in
            (d1x * cy - cx * d1y) /
            (d1x * (-ry) - (-rx) * d1y)
        s2 q@(V2 mqpx mqpy) (ps@(V2 mspx mspy), pt@(V2 mtpx mtpy)) =
          let (V2 cx cy) = ps - q
              (V2 rx ry) = pt - ps
          in
            (d2x * cy - cx * d2y) /
            (d2x * (-ry) - (-rx) * d2y)
        inside1 m =
          let dm = m - (p1 + m1p4)
              dc = (p1 + m1p3) - (p1 + m1p4)
              da = (p1 + m1p1) - (p1 + m1p4)
          in
            (0 < dm `dot` dc && dm `dot` dc < dc `dot` dc) &&
            (0 < dm `dot` da && dm `dot` da < da `dot` da)
        inside2 m =
          let dm = m - (p2 + m2p4)
              dc = (p2 + m2p3) - (p2 + m2p4)
              da = (p2 + m2p1) - (p2 + m2p4)
          in
            (0 < dm `dot` dc && dm `dot` dc < dc `dot` dc) &&
            (0 < dm `dot` da && dm `dot` da < da `dot` da)
    in
      if any inside1 quad2 || any inside2 quad1
      then Collision 0
        (if abs d1x > abs d1y
          then V2 (round $ signum d1x) 0
          else V2 0 (round $ signum d1y)
          )
      else
        let res1 = foldl
              (\acc1 gx ->
                let gt = foldl
                      (\acc2 q ->
                        let qt = t1 q gx
                            qs = s1 q gx
                        in
                          if (qs > 0 && qs < 1) && (qt > 0 && qt < acc2)
                          then qt
                          else acc2
                         )
                      (fst acc1)
                      quad1
                in
                  if gt < fst acc1
                  then
                    ( gt
                    , if gx == (p2 + fst g21, p2 + snd g22) ||
                        gx == (p2 + fst g23, p2 + snd g23)
                      then V2 (round $ signum d1x) 0
                      else V2 0 (round $ signum d1y)
                    )
                  else acc1
                )
              (dt, V2 0 0)
              g2s
            res2 = foldl
              (\acc1 gx ->
                let gt = foldl
                      (\acc2 q ->
                        let qt = t2 q gx
                            qs = s2 q gx
                        in
                          if (qs > 0 && qs < 1) && (qt > 0 && qt < acc2)
                          then qt
                          else acc2
                         )
                      (fst acc1)
                      quad1
                in
                  if gt < fst acc1
                  then
                    ( gt
                    , if gx == (p1 + fst g11, p2 + snd g12) ||
                        gx == (p1 + fst g13, p2 + snd g13)
                      then V2 (round $ signum d1x) 0
                      else V2 0 (round $ signum d1y)
                    )
                  else acc1
                )
              (dt, V2 0 0)
              g1s
            res = minimumBy (\a b -> fst a `compare` fst b) [res1, res2]
        in
          if fst res < dt
          then Collision `uncurry`
            A.log Debug ("Collision Imminent!!! " <> fromString (show res)) res
          else NoCollision

  -- | This Function is called for every collision on both colliding objects.
  collide
    :: (Collidible other)
    => c                               -- ^ Original object
    -> other                            -- ^ Collision partner
    -> CollisionResult Double (V2 Int) -- ^ Collision reaction
    -> c                               -- ^ Updated original object
  collide = elasticCollision 1

-- | Implementation of a dampened elastic collision used as default collision
-- implementation of the collision reaction
elasticCollision
  :: (Collidible c1, Collidible c2)
  => Double
  -> c1
  -> c2
  -> CollisionResult Double (V2 Int)
  -> c1
elasticCollision _ mo1 _ NoCollision = mo1
elasticCollision damping mo1 mo2 (Collision ddt (V2 dirx diry)) =
  let v1@(V2 v1x v1y) = velocity mo1
      (V2 v2x v2y) = velocity mo2
      p1@(V2 p1x p1y) = position mo1
      p2 = position mo2
      (V2 m1x1 m1y1, V2 m1x2 m1y2) = boundary mo1
      (V2 m2x1 m2y1, V2 m2x2 m2y2) = boundary mo2
      m1 = mass mo1
      m2 = mass mo2
      v1x' = 2 * (m1 * v1x + m2 * v2x) / (m1 + m2) - v1x
      v1y' = 2 * (m1 * v1y + m2 * v2y) / (m1 + m2) - v1y
      (V2 dx dy) = p2 - p1
      nvel@(V2 nvx nvy) = if m1 == recip 0
        then V2 0 0
        else (damping *) <$>
          if m2 == recip 0
          then
            if abs dy < abs dx
            then (V2 (-v1x) v1y)
            else (V2 v1x (-v1y))
          else (V2 v1x' v1y')
      bx = if dx < 0
        then
          if abs dx > abs m1x1 + abs m2x2 &&
            (if dy > 0
              then abs dy > abs m1y2 + abs m2y1
              else abs dy > abs m1y1 + abs m2y2
              )
          then 0
          else (abs m1x1 + abs m2x2) - abs dx
        else
          if abs dx > abs m1x2 + abs m2x1 &&
            (if dy > 0
              then abs dy > abs m1y2 + abs m2y1
              else abs dy > abs m1y1 + abs m2y2
              )
          then 0
          else -((abs m1x2 + abs m2x1) - abs dx)
      by = if dy < 0
        then 
          if abs dy > abs m1y1 + abs m2y2 &&
            (if dx > 0
              then abs dx > abs m1x2 + abs m2x1
              else abs dx > abs m1x1 + abs m2x2
              )
          then 0
          else (abs m1y1 + abs m2y2) - abs dy
        else
          if abs dy > abs m1y2 + abs m2y1 &&
            (if dx > 0
              then abs dx > abs m1x2 + abs m2x1
              else abs dx > abs m1x1 + abs m2x2
              )
          then 0
          else -((abs m1y2 + abs m2y1) - abs dy)
  in
    (velocityUpdater
      ((positionUpdater mo1)
        (p1 +
          if ddt == 0
          then
            if abs bx < abs by
            then
              V2 bx 0
            else
              V2 0 by
          else
            V2 0 0
          )
        )
      ) nvel