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
    { collisionTime :: time
    , collisionDirection :: 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 = p1 + m1b1
        m1p2 = p1 + V2 m1b1x m1b2y
        m1p3 = p1 + m1b2
        m1p4 = p1 + V2 m1b2x m1b1y
        m2p1 = p2 + m2b1
        m2p2 = p2 + V2 m2b1x m2b2y
        m2p3 = p2 + m2b2
        m2p4 = p2 + V2 m2b2x m2b1y
        -- 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 = [g11, g12, g13, g14]
        -- g2s = [g21, g22, g23, g24]
        b1 =
          ( 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)))
          )
        b2 =
          ( 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 b11@(V2 b1minx b1miny) = fst b1
              b12@(V2 b1maxx b1maxy) = snd b1
              b21@(V2 b2minx b2miny) = fst b2
              b22@(V2 b2maxx b2maxy) = snd b2
              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
        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
          if tx <= ty
          then
            if tx < dt
            then
              if d1x <= 0
              then
                let (V2 _ g1s) = m1p1 + ((tx *) <$> d1)
                    (V2 _ g1e) = m1p2 + ((tx *) <$> d1)
                    (V2 _ g2s) = m2p4 + ((tx *) <$> d2)
                    (V2 _ g2e) = m2p3 + ((tx *) <$> d2)
                    s11 = (g1s - g2s) / (g2e - g2s)
                    s12 = (g1e - g2s) / (g2e - g2s)
                    s21 = (g2s - g1s) / (g1e - g1s)
                    s22 = (g2e - g1s) / (g1e - g1s)
                in
                  if any (\x -> x > 0 && x < 1) [s11, s12, s21 ,s22]
                  then Collision tx (V2 (floor $ signum d1x) 0)
                  else NoCollision
              else
                let (V2 _ g1s) = m1p4 + ((tx *) <$> d1)
                    (V2 _ g1e) = m1p3 + ((tx *) <$> d1)
                    (V2 _ g2s) = m2p1 + ((tx *) <$> d2)
                    (V2 _ g2e) = m2p2 + ((tx *) <$> d2)
                    s11 = (g1s - g2s) / (g2e - g2s)
                    s12 = (g1e - g2s) / (g2e - g2s)
                    s21 = (g2s - g1s) / (g1e - g1s)
                    s22 = (g2e - g1s) / (g1e - g1s)
                in
                  if any (\x -> x > 0 && x < 1) ((\s ->A.log A.Debug ("s: " <> fromString (show s)) s) [s11, s12, s21 ,s22])
                  then Collision tx (V2 (floor $ signum d1x) 0)
                  else NoCollision
            else
              NoCollision
          else
            if ty < dt
            then
              if d1y <= 0
              then
                let (V2 g1s _) = m1p1 + ((ty *) <$> d1)
                    (V2 g1e _) = m1p4 + ((ty *) <$> d1)
                    (V2 g2s _) = m2p2 + ((ty *) <$> d2)
                    (V2 g2e _) = m2p3 + ((ty *) <$> d2)
                    s11 = (g1s - g2s) / (g2e - g2s)
                    s12 = (g1e - g2s) / (g2e - g2s)
                    s21 = (g2s - g1s) / (g1e - g1s)
                    s22 = (g2e - g1s) / (g1e - g1s)
                in
                  if any (\x -> x > 0 && x < 1) [s11, s12, s21 ,s22]
                  then Collision ty (V2 0 (floor $ signum d1y))
                  else NoCollision
              else
                let (V2 g1s _) = m1p2 + ((ty *) <$> d1)
                    (V2 g1e _) = m1p3 + ((ty *) <$> d1)
                    (V2 g2s _) = m2p1 + ((ty *) <$> d2)
                    (V2 g2e _) = m2p4 + ((ty *) <$> d2)
                    s11 = (g1s - g2s) / (g2e - g2s)
                    s12 = (g1e - g2s) / (g2e - g2s)
                    s21 = (g2s - g1s) / (g1e - g1s)
                    s22 = (g2e - g1s) / (g1e - g1s)
                in
                  if any (\x -> x > 0 && x < 1) [s11, s12, s21 ,s22]
                  then Collision ty (V2 0 (floor $ signum d1y))
                  else NoCollision
            else
              NoCollision
        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 dirx > 0
            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 mo1
      -- ((positionUpdater mo1)
      --   (p1 + ((ddt *) <$> v1)
      --     -- if ddt == 0
      --     -- then
      --     --   if abs bx < abs by
      --     --   then
      --     --     V2 bx 0
      --     --   else
      --     --     V2 0 by
      --     -- else
      --     --   V2 0 0
      --     )
      --   )
      ) nvel