{-
   Haskell Dynamics Engine, Copyright (C) 2007 Ruben Henner Zilibowitz
   All rights reserved. Email: rubenz@cse.unsw.edu.au Web: www.cse.unsw.edu.au/~rubenz
   
   This is free software; you can redistribute it and/or modify it
   under the terms of the GNU General Public License that is include with this
   package in the file LICENSE-GPL.TXT.
-}

module Collisions where

import Matrix3x3
import Quaternions
import Objects
import Data.Maybe
import Data.Array

-- sweep takes a list of intervals sorted by start position
-- and returns a list of overlapping pairs of intervals.
sweep :: [Interval] -> [(Int,Int)]
sweep [] = []
sweep ((i,(b,e)) : is) = [(i,j) | (j,_) <- (takeWhile ((<= e).fst.snd) is)] ++ (sweep is)

-- does the collision detection
-- uses sort and sweep optimisation
findAllContacts :: World -> [Joint]
findAllContacts (World_ bodies islands _ _ sortedIntervals _ _ _) = (concat.catMaybes) [tryCollide i j (bodies!i) (bodies!j) | (i,j) <- (sweep sortedIntervals), not (or [(i `elem` island) && (j `elem` island) | island <- islands]), not ((anchored (bodies!i)) && (anchored (bodies!j)))]

{-
-- do collision detection
-- no optimisation - all pairs checked
findAllContacts :: World -> [Contact]
findAllContacts (World_ bodies islands _ _ _) = (concat.catMaybes) (map2' (\i j -> if (or [(i `elem` island) && (j `elem` island) | island <- islands]) then Nothing else (tryCollide i j (bodies!i) (bodies!j))) (indices bodies))

map2 :: (a -> b -> c) -> [a] -> [b] -> [c]
map2 f as bs = [f a b | a <- as, b <- bs]

map2' :: (a -> a -> c) -> [a] -> [c]
map2' _ [] = []
map2' f (a:as) = (map (f a) as) ++ (map2' f as)
-}

---
--- tryCollide
--- Try colliding two geometries together. If no result then do them the other way around.
---
tryCollide :: Int -> Int -> Body -> Body -> Maybe [Joint]
tryCollide i j b1 b2
   | (anchored b1 && anchored b2) = Nothing
   | (not (bodyBoundingSpheresIntersect b1 b2)) = Nothing
   | (isJust test1) = test1
   | otherwise      = test2
   where test1 = collide i j b1 b2
         test2 = collide j i b2 b1

boundingSpheresIntersect :: BoundingSphere -> BoundingSphere -> Bool
boundingSpheresIntersect (c1,r1) (c2,r2) = let d = c2-c1 in (dot d d) < (r1 + r2)^2

bodyBoundingSpheresIntersect :: Body -> Body -> Bool
bodyBoundingSpheresIntersect b1 b2 = boundingSpheresIntersect (boundingSphere b1) (boundingSphere b2)

---
--- collide
--- In the resulting contacts the normal vector should always point outwards from the second body.
---

collide :: Int -> Int -> Body -> Body -> Maybe [Joint]

collide i j body1@(Body_ {geometry = CompoundGeometry g}) body2
   = Just ((concat.catMaybes) [tryCollide i j (body1 {geometry = geom, orientation = (orientation body1)*rot, Objects.position = (Objects.position body1) + (orientation body1).*loc}) body2 | (geom,rot,loc) <- g])

-- sphere/sphere
-- works
collide i j body1@(Body_ {geometry = Sphere r}) body2@(Body_ {geometry = Sphere s}) = Just (map (makeContact i j) (maybeToList (sphereWithSphere (r,position body1) (s,position body2))))

-- plane/sphere
-- works
collide i j body1@(Body_ {geometry = Plane a b c}) body2@(Body_ {geometry = Sphere r}) = Just (map (makeContact i j) (maybeToList (planeWithSphere (a,b,c) (r,position body2))))

-- sphere/box
-- nyi
--collide (Sphere r,loc1,_) (Box x y z,loc2,rot2) = Just []

-- sphere/SphereCappedCylinder
-- test this
collide i j body1@(Body_ {geometry = Sphere r1}) body2@(Body_ {geometry = SphereCappedCylinder r2 h})
   | (isJust shell) = Just [reverseContactNormal (makeContact j i (fromJust shell))]
   | otherwise = Just (map (makeContact i j) caps)
   where
      loc1 = position body1
      c = (position body2) + ((orientation body2) .* (Vec 0 0 (0.5*h)))
      d = (position body2) + ((orientation body2) .* (Vec 0 0 (-0.5*h)))
      shell = cylinderShellWithSphere (r2,c,d) (r1,loc1)
      caps = catMaybes (map (sphereWithSphere (r1,loc1)) [(r2,c),(r2,d)])

-- SphereCappedCylinder/SphereCappedCylinder
-- test this
collide i j body1@(Body_ {geometry = SphereCappedCylinder r1 h1}) body2@(Body_ {geometry = SphereCappedCylinder r2 h2})
   | (isJust shells) = Just (map (makeContact i j) (maybeToList shells))
   | ((not.null) caps) = Just (map (makeContact i j) caps)
   | otherwise = Just (abCapsShell ++ cdCapsShell)
   where
      loc1 = position body1
      loc2 = position body2
      rot1 = orientation body1
      rot2 = orientation body2
      a = loc1 + (rot1 .* (Vec 0 0 (0.5*h1)))
      b = loc1 + (rot1 .* (Vec 0 0 (-0.5*h1)))
      c = loc2 + (rot2 .* (Vec 0 0 (0.5*h2)))
      d = loc2 + (rot2 .* (Vec 0 0 (-0.5*h2)))
      shells = cylinderShells (r1,a,b) (r2,c,d)
      abCapsShell = map reverseContactNormal (map (makeContact j i) (catMaybes (map (cylinderShellWithSphere (r2,c,d)) [(r1,a),(r1,b)])))
      cdCapsShell = map (makeContact i j) (catMaybes (map (cylinderShellWithSphere (r1,a,b)) [(r2,c),(r2,d)]))
      caps = catMaybes ((map (sphereWithSphere (r1,a)) [(r2,c),(r2,d)]) ++ (map (sphereWithSphere (r1,b)) [(r2,c),(r2,d)]))

-- Plane/SphereCappedCylinder
-- tested - seems to work
collide i j body1@(Body_ {geometry = Plane a b c}) body2@(Body_ {geometry = SphereCappedCylinder r h})
   = Just (map (makeContact i j) (catMaybes [planeWithSphere (a,b,c) (r,p),planeWithSphere (a,b,c) (r,q)]))
   where
      loc = position body2
      rot = orientation body2
      p = loc + (rot .* (Vec 0 0 (-0.5*h)))
      q = loc + (rot .* (Vec 0 0 (0.5*h)))

-- Box/Box
-- nyi
--collide b1@(Box _ _ _,_,_) b2@(Box _ _ _,_,_) = Just (boxbox b1 b2)

-- Box/Plane
-- test this
{-
collide (Box x y z,loc,rot) (Plane a b c,_,_) = Just (filter (\(Contact_ _ _ d) -> d >= 0) [Contact_ boxV n (-((dot n boxV) + p)) | boxV <- corners])
   where n = Matrix3x3.normalise (cross (c-a) (b-a))
         p = -(dot n a)
         corners = [(quatToVec (rot*(Q 0 x' y' z')/rot)) + loc | x'<-[-x,x],y'<-[-y,y],z'<-[-z,z]]
         quatToVec (Q _ x y z) = Vec x y z
-}
-- Plane/Plane (should be ignored)
-- works
collide _ _ (Body_ {geometry = Plane _ _ _}) (Body_ {geometry = Plane _ _ _}) = Just []

collide _ _ _ _ = Nothing

reverseContactNormal :: Joint -> Joint
reverseContactNormal (Joint_ i j (Contact p n d)) = Joint_ j i (Contact p (-n) d)

-----------

makeContact i j (ContactData_ loc norm depth) = Joint_ i j (Contact loc norm depth)

data ContactData = ContactData_ (Vector RealNum) (Vector RealNum) RealNum   -- location normal depth

-- collides a plane with a sphere
planeWithSphere :: (Vector RealNum,Vector RealNum,Vector RealNum) -> (RealNum,Vector RealNum) -> Maybe ContactData
planeWithSphere (a,b,c) (radius,centre)
   | (centreLen > radius)  = Nothing
   | otherwise      = Just (ContactData_ pos normal (centreLen - radius))
   where n = cross (c-a) (b-a)
         m = toMatrixCols (b-a) (c-a) n
         m' = Matrix3x3.inverse m
         (Vec x y _) = matXvec m' (centre-a)
         q = (b-a).*.x + (c-a).*.y + a
         centreVec = q - centre
         centreLen = Matrix3x3.magnitude centreVec
         normal = centreVec./.centreLen
         p = centre + normal.*.radius
         pos = (q + p).*.0.5

-- collides two spheres with one another
sphereWithSphere :: (RealNum,Vector RealNum) -> (RealNum,Vector RealNum) -> Maybe ContactData
sphereWithSphere (r,c) (s,d)
   | denom > ((r+s)^2) = Nothing
   | denom <= 0 = Just (ContactData_ c (Vec 1 0 0) (negate (r+s)))
   | otherwise  = Just (ContactData_ pos n (dist-r-s))
   where p = c - d
         denom = dot p p
         dist = sqrt denom
         n = p ./. dist
         pos = (d + n.*.(s-r) + c).*.0.5

-- collides a cylindrical shell with a sphere
-- nb: ignores case where sphere centre lies on the axis of the cylinder
cylinderShellWithSphere :: (RealNum,Vector RealNum,Vector RealNum) -> (RealNum,Vector RealNum) -> Maybe ContactData
cylinderShellWithSphere (r1,a@(Vec ax ay az),b@(Vec bx by bz)) (r2,c@(Vec cx cy cz))
   | (centreDist <= 0) = Nothing
   | (0 <= alpha && alpha <= 1 && depth <= 0) = Just (ContactData_ pos normal depth)
   | otherwise = Nothing
   where
      p = a - b
      --denom = (bz^2-2*az*bz+by^2-2*ay*by+bx^2-2*ax*bx+az^2+ay^2+ax^2)
      denom = dot p p
      alpha = -((bz-az)*cz+(by-ay)*cy+(bx-ax)*cx-bz^2+az*bz-by^2+ay*by-bx^2+ax*bx)/denom
      axisPos = b + p.*.alpha
      centreVec = axisPos - c
      centreDist = Matrix3x3.magnitude centreVec
      normal = centreVec ./. centreDist
      pos1 = axisPos - normal.*.r1
      pos2 = c + normal.*.r2
      pos = (pos1 + pos2).*.0.5
      depth = dot (pos1 - pos2) normal

-- collides two cylindrical shells
-- nb: ignores case where cylinders are parallel to one another
cylinderShells :: (RealNum,Vector RealNum,Vector RealNum) -> (RealNum,Vector RealNum,Vector RealNum) -> Maybe ContactData
cylinderShells (r1,a@(Vec ax ay az),b@(Vec bx by bz)) (r2,c@(Vec cx cy cz),d@(Vec dx dy dz))
   | (denom > 0 && 0 <= alpha && alpha <= 1 && 0 <= beta && beta <= 1 && depth <= 0) = Just (ContactData_ pos normal depth)
   | otherwise = Nothing
   where
      p@(Vec axmbx aymby azmbz) = a - b
      q@(Vec cxmdx cymdy czmdz) = c - d
      (Vec dxmbx dymby dzmbz) = d - b
      denom=((aymby^2+axmbx^2)*czmdz^2+((-2)*aymby*azmbz*cymdy-2*axmbx*azmbz*cxmdx)*czmdz+(azmbz^2+axmbx^2)*cymdy^2-2*axmbx*aymby*cxmdx*cymdy+(azmbz^2+aymby^2)*cxmdx^2)
      alpha=(-(((aymby*cymdy+axmbx*cxmdx)*czmdz-azmbz*cymdy^2-azmbz*cxmdx^2)*dzmbz+((-aymby)*czmdz^2+azmbz*cymdy*czmdz+axmbx*cxmdx*cymdy-aymby*cxmdx^2)*dymby+((-axmbx)*czmdz^2+azmbz*cxmdx*czmdz-axmbx*cymdy^2+aymby*cxmdx*cymdy)*dxmbx))/denom
      beta=(-(((aymby^2+axmbx^2)*czmdz-aymby*azmbz*cymdy-axmbx*azmbz*cxmdx)*dzmbz+((-aymby)*azmbz*czmdz+(azmbz^2+axmbx^2)*cymdy-axmbx*aymby*cxmdx)*dymby+((-axmbx)*azmbz*czmdz-axmbx*aymby*cymdy+(azmbz^2+aymby^2)*cxmdx)*dxmbx))/denom
      nearestPos1 = b + p.*.alpha
      nearestPos2 = d + q.*.beta
      pos1 = nearestPos1 - normal.*.r1
      pos2 = nearestPos2 + normal.*.r2
      pos = (pos1 + pos2).*.0.5
      depth = dot (pos1 - pos2) normal
      normal = Matrix3x3.normalise (nearestPos1 - nearestPos2)

-----------------

---------