pituicat/src/Physics/Classes/Collidible.hs

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

import Affection as A

import Linear

import Data.String (fromString)

import Data.List (sortBy)

-- internal imports

import Physics.Classes.Mass

data CollisionResult time direction
  = NoCollision
  | CollisionImminent
    { collisionTime :: time
    , collisionDirection :: direction
    }
  | OverlapCollision
    { collisionDepth :: 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
       , V2 Double
       ) -- ^ Bottom left and 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@(V2 p1x p1y) = position m1
        p2@(V2 p2x p2y) = position m2
        (m1b1@(V2 m1b1x m1b1y), m1b2@(V2 m1b2x m1b2y)) = boundary m1
        (m2b1@(V2 m2b1x m2b1y), m2b2@(V2 m2b2x m2b2y)) = boundary m2
        (V2 pm1b1x pm1b1y, V2 pm1b2x pm1b2y) = (p1 +) <$> boundary m1
        (V2 pm2b1x pm2b1y, V2 pm2b2x pm2b2y) = (p2 +) <$> boundary m2
        m1p1@(V2 m1p1x m1p1y) = p1 + m1b1
        m1p2 = p1 + V2 m1b1x m1b2y
        m1p3@(V2 m1p3x m1p3y) = p1 + m1b2
        m1p4@(V2 m1p4x _) = p1 + V2 m1b2x m1b1y
        m2p1@(V2 m2p1x m2p1y) = p2 + m2b1
        m2p2 = p2 + V2 m2b1x m2b2y
        m2p3@(V2 m2p3x m2p3y) = p2 + m2b2
        m2p4 = p2 + V2 m2b2x m2b1y
        (V2 b1minx b1miny, V2 b1maxx b1maxy) =
          ( V2
            ((\(V2 x _) -> x) (if d1x < 0 then m1p1 + ((dt *) <$> d1) else m1p1))
            ((\(V2 _ y) -> y) (if d1y < 0 then m1p1 + ((dt *) <$> d1) else m1p1))
          , V2
            ((\(V2 x _) -> x) (if d1x < 0 then m1p3 else m1p3 + ((dt *) <$> d1)))
            ((\(V2 _ y) -> y) (if d1y < 0 then m1p3 else m1p3 + ((dt *) <$> d1)))
          )
        (V2 b2minx b2miny, V2 b2maxx b2maxy) =
          ( V2
            ((\(V2 x _) -> x) (if d2x < 0 then m2p1 + ((dt *) <$> d2) else m2p1))
            ((\(V2 _ y) -> y) (if d2y < 0 then m2p1 + ((dt *) <$> d2) else m2p1))
          , V2
            ((\(V2 x _) -> x) (if d2x < 0 then m2p3 else m2p3 + ((dt *) <$> d2)))
            ((\(V2 _ y) -> y) (if d2y < 0 then m2p3 else m2p3 + ((dt *) <$> d2)))
          )
        broadphaseOverlap =
          let in2 =
                (b1minx > b2minx && b1minx < b2maxx &&
                  b1miny > b2miny && b1miny < b2maxy) ||
                (b1maxx > b2minx && b1maxx < b2maxx &&
                  b1miny > b2miny && b1miny < b2maxy) ||
                (b1minx > b2minx && b1minx < b2maxx &&
                  b1maxy > b2miny && b1maxy < b2maxy) ||
                (b1maxx > b2minx && b1maxx < b2maxx &&
                  b1maxy > b2miny && b1maxy < b2maxy)
              in1 =
                (b2minx > b1minx && b2minx < b1maxx &&
                  b2miny > b1miny && b2miny < b1maxy) ||
                (b2maxx > b1minx && b2maxx < b1maxx &&
                  b2miny > b1miny && b2miny < b1maxy) ||
                (b2minx > b1minx && b2minx < b1maxx &&
                  b2maxy > b1miny && b2maxy < b1maxy) ||
                (b2maxx > b1minx && b2maxx < b1maxx &&
                  b2maxy > b1miny && b2maxy < b1maxy)
          in
            in2 || in1
        overlap =
          let in1 =
                (pm1b1x > pm2b1x && pm1b1x < pm2b2x &&
                  pm1b1y > pm2b1y && pm1b1y < pm2b2y) ||
                (pm1b2x > pm2b1x && pm1b2x < pm2b2x &&
                  pm1b1y > pm2b1y && pm1b1y < pm2b2y) ||
                (pm1b1x > pm2b1x && pm1b2x < pm2b2x &&
                  pm1b2y > pm2b1y && pm1b2y < pm2b2y) ||
                (pm1b2x > pm2b1x && pm1b2x < pm2b2x &&
                  pm1b2y > pm2b1y && pm1b2y < pm2b2y)
              in2 =
                (pm2b1x > pm1b1x && pm2b1x < pm1b2x &&
                  pm2b1y > pm1b1y && pm2b1y < pm1b2y) ||
                (pm2b2x > pm1b1x && pm2b2x < pm1b2x &&
                  pm2b1y > pm1b1y && pm2b1y < pm1b2y) ||
                (pm2b1x > pm1b1x && pm2b2x < pm1b2x &&
                  pm2b2y > pm1b1y && pm2b2y < pm1b2y) ||
                (pm2b2x > pm1b1x && pm2b2x < pm1b2x &&
                  pm2b2y > pm1b1y && pm2b2y < pm1b2y)
          in
            in2 || in1
        tx =
          let p1x = (\(V2 x _) -> x) (if d1x < 0 then m1p1 else m1p4)
              p2x = (\(V2 x _) -> x) (if d1x < 0 then m2p4 else m2p1)
          in
            if d2x - d1x == 0 then dt else (p1x - p2x) / (d2x - d1x)
        ty =
          let p1y = (\(V2 _ y) -> y) (if d1y < 0 then m1p1 else m1p2)
              p2y = (\(V2 _ y) -> y) (if d1y < 0 then m2p2 else m2p1)
          in
            if d2y - d1y == 0 then dt else (p1y - p2y) / (d2y - d1y)
    in
      if broadphaseOverlap
        then
          let coll xdir =
                let (p11, p12, p21, p22)
                      | xdir =
                        if d1x < 0
                        then
                          (m1p1, m1p2, m2p4, m2p3)
                        else
                          (m1p4, m1p3, m2p1, m2p2)
                      | otherwise =
                        if d1y < 0
                        then
                          (m1p1, m1p4, m2p2, m2p3)
                        else
                          (m1p2, m1p3, m2p1, m2p4)
                    vselector (V2 x y) = if xdir then y else x
                    tick = if xdir then tx else ty
                    g1s = vselector $ p11 + ((tick *) <$> d1)
                    g1e = vselector $ p12 + ((tick *) <$> d1)
                    g2s = vselector $ p21 + ((tick *) <$> d2)
                    g2e = vselector $ p22 + ((tick *) <$> d2)
                    s11 = (g1s - g2s) / (g2e - g2s)
                    s12 = (g1e - g2s) / (g2e - g2s)
                    s21 = (g2s - g1s) / (g1e - g1s)
                    s22 = (g2e - g1s) / (g1e - g1s)
                in
                  any (\x -> x > 0 && x < 1) [s11, s12, s21 ,s22]
              res = case (tx < dt, ty < dt, tx < ty, coll True, coll False) of
                  (True, _, True, True, _) ->
                    CollisionImminent tx (V2 (floor $ signum d1x) 0)
                  (_, True, False, _, True) ->
                    CollisionImminent ty (V2 0 (floor $ signum d1y))
                  (True, _, False, True, False) ->
                    CollisionImminent tx (V2 (floor $ signum d1x) 0)
                  (_, True, True, False, True) ->
                    CollisionImminent ty (V2 0 (floor $ signum d1y))
                  (_, _, _, False, False) ->
                    NoCollision
                  (_, _, True, _, _) ->
                    A.log A.Debug "CORNER CASE!" NoCollision
                  (False, False, _, _, _) ->
                    NoCollision
                  x -> error $
                    "Unhandled combination of collision check results: "
                    <> fromString (show x)
          in
            case res of
              CollisionImminent _ _ ->
                res
              NoCollision ->
                if overlap
                then
                  let xoverlap = floor (minimum
                        [ pm2b2x - pm2b1x
                        , pm2b1x - pm1b2x
                        ])
                      yoverlap = floor (minimum
                        [ pm2b1y - pm1b2y
                        , pm2b2y - pm1b1y
                        ])
                  in
                    if xoverlap < yoverlap
                    then
                      if xoverlap < 0
                      then OverlapCollision
                        (V2 xoverlap 0)
                      else
                        NoCollision
                    else
                      if yoverlap < 0
                      then OverlapCollision
                        (V2 yoverlap 0)
                      else NoCollision
                else NoCollision
              _ -> A.log
                A.Error
                "Premature overlap collision detection"
                NoCollision
        else
          NoCollision

  -- | This Function is called for every collision on both colliding objects.
  collide
    :: (Collidible other)
    => c                                          -- ^ Original object
    -> [(other, CollisionResult Double (V2 Int))] -- ^ Collision partners and  results
    -> Double                                     -- ^ Timestep length
    -> c                                          -- ^ Updated original object
  collide coll1 collrs = elasticCollision 0.9 coll1 (head collrs)

-- | 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))
  -> Double                          -- ^ Timestep length
  -> c1
elasticCollision _ mo1 (_, NoCollision) _ = mo1
elasticCollision damping mo1 (mo2, CollisionImminent ddt (V2 dirx diry)) dt =
  let v1@(V2 v1x v1y) = velocity mo1
      (V2 v2x v2y) = velocity mo2
      p1 = position mo1
      m1 = mass mo1
      m2 = mass mo2
      v1x' = 2 * (m1 * v1x + m2 * v2x) / (m1 + m2) - v1x
      v1y' = 2 * (m1 * v1y + m2 * v2y) / (m1 + m2) - v1y
      nvel = if m1 == recip 0
        then V2 0 0
        else (damping *) <$>
          if m2 == recip 0
          then
            if dirx /= 0
            then V2 (-v1x) v1y
            else V2 v1x (-v1y)
          else V2 v1x' v1y'
  in
    velocityUpdater
      mo1
      nvel
elasticCollision damping mo1 (mo2, OverlapCollision depth) dt =
  let m1 = mass mo1
      m2 = mass mo2
      (V2 dx dy) = (/ dt) . fromIntegral <$> depth
      v1x' = 2 * (m1 * dx + m2 * (-dx)) / (m1 + m2) - dx
      v1y' = 2 * (m1 * dy + m2 * (-dy)) / (m1 + m2) - dy
      nvel = if m1 == recip 0
        then V2 0 0
        else (damping *) <$> V2 v1x' v1y'
  in
    velocityUpdater mo1 nvel