content: module Lambency.Bounds (
    BoundingVolume,
    aabb,
    boundingSphere,
    containsPoint,
    colliding
    ) where
  
  --------------------------------------------------------------------------------
  
  import Lambency.Types
  import Lambency.Transform
  
  import Linear.Epsilon
  import Linear.Matrix
  import Linear.Metric
  import qualified Linear.Quaternion as Quat
  import Linear.Vector
  import Linear.V3
  
  #if __GLASGOW_HASKELL__ <= 708
  import Control.Applicative
  #endif
  --------------------------------------------------------------------------------
  
  -- Bounding Volumes
  
  data BoundingVolume = BoundingBox Float Float Float
                      | BoundingEllipse Float Float Float
                      | RotatedVolume Quatf BoundingVolume
                      | TranslatedVolume Vec3f BoundingVolume
                      | Union BoundingVolume BoundingVolume
  
  instance Transformable3D BoundingVolume where
    translate t (TranslatedVolume t' bv) = TranslatedVolume (t ^+^ t') bv
    translate t (Union bv1 bv2) = Union (translate t bv1) (translate t bv2)
    translate t bv = TranslatedVolume t bv
  
    rotate quat (RotatedVolume uq bv) = RotatedVolume (uq * quat) bv
    rotate quat (TranslatedVolume t bv) = TranslatedVolume t (rotate quat bv)
    rotate quat bv = RotatedVolume quat bv
  
    nonuniformScale (V3 sx sy sz) (BoundingBox x y z) =
      BoundingBox (x * sx) (y * sy) (z * sz)
    nonuniformScale (V3 sx sy sz) (BoundingEllipse x y z) =
      BoundingEllipse (x * sx) (y * sy) (z * sz)
  
    nonuniformScale s (RotatedVolume uq bv) = RotatedVolume uq (nonuniformScale s bv)
    nonuniformScale s (TranslatedVolume t bv) = TranslatedVolume t (nonuniformScale s bv)
    nonuniformScale s (Union b b') = Union (nonuniformScale s b) (nonuniformScale s b')
  
  -------------------------------------------------------------------------------
  
  aabb :: Vec3f -> Vec3f -> BoundingVolume
  aabb v1@(V3 x1 y1 z1) v2@(V3 x2 y2 z2) = let
    szx = abs (x2 - x1)
    szy = abs (y2 - y1)
    szz = abs (z2 - z1)
    c = (v1 ^+^ v2) ^* 0.5
    in translate c $ BoundingBox (szx * 0.5) (szy * 0.5) (szz * 0.5)
  
  boundingSphere :: Vec3f -> Float -> BoundingVolume
  boundingSphere c r = translate c $ BoundingEllipse r r r
  
  containsPoint :: BoundingVolume -> Vec3f -> Bool
  containsPoint (BoundingBox x y z) (V3 x' y' z') =
    and $ zipWith (\v w -> ((-w) >= v) && (v <= w)) [x', y', z'] [x, y, z]
  containsPoint (BoundingEllipse x y z) (V3 x' y' z') =
    sqr (x' / x) + sqr (y' / y) + sqr (z' / z) < 1
    where
      sqr :: Num a => a -> a
      sqr k = k * k
  containsPoint (Union bv1 bv2) v = containsPoint bv1 v || containsPoint bv2 v
  containsPoint (TranslatedVolume t bv) v = containsPoint bv (v ^-^ t)
  containsPoint (RotatedVolume q bv) v = containsPoint bv (Quat.rotate (negate q) v)
  
  -- An orientation is simply a position in world space, a rotation matrix, and
  -- and extents vector for each of the new coordinate axes
  type Orientation = (Vec3f, Mat3f, Vec3f)
  
  io :: Orientation
  io = (zero, Linear.Matrix.identity, V3 1 1 1)
  
  data OrientedVolume = Box Orientation
                      | Ellipse Orientation
  
  collideOriented :: OrientedVolume -> OrientedVolume -> Bool
  
  collideOriented (Box (t, o, s)) (Box (t', o', s')) =
    -- First we need to rotate and translate the first box into the coordinate
    -- space of the second.
    let
      rotateAtoB :: Mat3f
      rotateAtoB = adjoint o' !*! o
  
      cBwrtA :: Vec3f
      cBwrtA = (t' ^-^ t) *! adjoint o
  
      eBwrtA :: Vec3f
      eBwrtA = s' *! adjoint rotateAtoB
  
      absDot :: Vec3f -> Vec3f -> Float
      absDot (V3 x y z) (V3 x' y' z') =
        abs (x * x') + abs (y * y') + abs (z * z')
  
      testAxis :: Vec3f -> Bool
      testAxis v = abs (cBwrtA `dot` v) <= ((s `absDot` v) + (eBwrtA `absDot` v))
  
      axesA :: [Vec3f]
      axesA = [V3 1 0 0, V3 0 1 0, V3 0 0 1]
  
      axesB :: [Vec3f]
      axesB = map (*! rotateAtoB) axesA
  
    in
     any (testAxis . signorm) $
     filter (not . nearZero) $
     [v1 `cross` v2 | v1 <- axesA, v2 <- axesB] ++ axesA ++ axesB
  
  -- collideOriented (Ellipse (t, o, s)) (Box (t', o', s')) = False
  -- collideOriented (Ellipse (t, o, s)) (Ellipse (t', o', s')) = False
  
  -- The only pattern we're missing is box-ellipse
  collideOriented ov1 ov2 = collideOriented ov2 ov1
  
  colliding :: BoundingVolume -> BoundingVolume -> Bool
  colliding bv (Union bv1 bv2) = colliding bv bv1 || colliding bv bv2
  colliding (Union bv1 bv2) bv = colliding bv bv1 || colliding bv bv2
  colliding bv1 bv2 =
    or [collideOriented x y | x <- getO bv1 io, y <- getO bv2 io]
    where
      getO :: BoundingVolume -> Orientation -> [OrientedVolume]
      getO (BoundingBox x y z) (t, o, s) = [Box (t, o, (*) <$> s <*> V3 x y z)]
      getO (BoundingEllipse x y z) (t, o, s) = [Ellipse (t, o, (*) <$> s <*> V3 x y z)]
      getO (TranslatedVolume t bv) (t', o, s) = getO bv (t ^+^ t', o, s)
      getO (RotatedVolume q bv) (t, o, s) = getO bv (t, fromQuaternion q !*! o, s)
      getO (Union bv1' bv2') o = getO bv1' o ++ getO bv2' o
  