{-
   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 Objects where

import Matrix3x3
import Quaternions
import Data.Array
import Data.List(partition)

---
--- Type definitions
---

type RealNum = Double   -- could be Double or Float
data Geometry = Sphere RealNum   -- radius
              | Box RealNum RealNum RealNum   -- x y z axes
              | Plane (Vector RealNum) (Vector RealNum) (Vector RealNum)
              | SphereCappedCylinder RealNum RealNum   -- radius height
              | CompoundGeometry [(Geometry,Quaternion RealNum,Vector RealNum)]   -- [(geometry,orientation,position)]
              deriving (Read, Show)

--data Contact = Contact_ Int Int (Vector RealNum) (Vector RealNum) RealNum   deriving Show   -- bodyAid bodyBid location, normal, penetration depth

data Body = Body_ { geometry :: Geometry,
                    mass :: RealNum,
                    invMass :: RealNum,
                    inertiaBody :: Matrix3x3 RealNum,
                    invInertiaBody :: Matrix3x3 RealNum,
                    position :: Vector RealNum,
                    orientation :: Quaternion RealNum,
                    momentum, angularMomentum :: Vector RealNum,
                    invInertia, rotationMat :: Matrix3x3 RealNum,
                    velocity, rotationalVel :: Vector RealNum,
                    force, torque :: Vector RealNum,
                    anchored :: Bool }   deriving Show

type Interval = (Int,(RealNum,RealNum))

type RGBColour = (Float,Float,Float)

data World = World_ Bodies [[Int]] [Joint] [[Joint]] [Interval] (Vector RealNum) RealNum [RGBColour]   deriving Show   -- <array of bodies> <list of islands> <joints> <joint groups> <sorted intervals> <gravity vector> <global restitution factor> <list of colours for bodies>

type Bodies = Array Int Body

--data Contact = Contact_ Int Int (Vector RealNum) (Vector RealNum) RealNum   deriving Show

data Joint = Joint_ Int Int JointType   deriving (Read,Show)

data JointType = BallSocket (Vector RealNum) (Vector RealNum) | Hinge | Slider | Universal |
                 Contact (Vector RealNum) (Vector RealNum) RealNum deriving (Read, Show)

-- Interval data gives the list of bounding boxes projected onto each axis (x,y,z) and sorted
-- by start position and end position for each axis. It also gives an array based look up table
-- for overlapping bounding box pairs. Entries in this table are updated whenever the corresponding
-- pair of intervals change position in any of the sorted lists. A set of overlapping pairs of boxes
-- is also maintained which can be used by the collision detection algorithms. Insertion sort is used
-- to sort the lists in the hope that it will run in near linear time as temporal coherence is present.
-- note to self: This algorithm is described in rough terms in a paper by David Baraff. It seems that
-- it would be usefull with few objects of high complexity. Perhaps it is less usefull with many objects
-- of low complexity however since it is more likely that the insertion sort step will produce swaps.
--data IntervalData = IntervalData_ ([Interval],[Interval]) ([Interval],[Interval]) ([Interval],[Interval]) (Array (Int,Int) Bool) (Set (Int,Int))

infixl 7 .*, ./

(.*) :: RealFloat a => Quaternion a -> Vector a -> Vector a
(.*) q (Vec x y z) = let (Q _ x' y' z') = q*(Q 0 x y z)/q in Vec x' y' z'

(./) :: RealFloat a => Quaternion a -> Vector a -> Vector a
(./) q (Vec x y z) = let (Q _ x' y' z') = (recip q)*(Q 0 x y z)*q in Vec x' y' z'

type BoundingSphere = (Vector RealNum,RealNum)   -- (centre,radius)

boundingSphere :: Body -> BoundingSphere
boundingSphere body =
   case (geometry body) of
      (Sphere r) -> (position body,r)
      (SphereCappedCylinder r h) -> (position body,h+r)
      (Plane v _ _) -> (v,1e9)  -- this is a bit of a hack - the plane should have infinite size


compareSnds (_,x) (_,y) = compare x y

xInterval :: Body -> (RealNum,RealNum)
xInterval body = let (Vec x _ _,r) = boundingSphere body in (x - r,x + r)

intervalOnLine :: Body -> (Vector RealNum,Vector RealNum) -> (RealNum,RealNum)
intervalOnLine body (a,d) = (t - r,t + r)
   where (c,r) = boundingSphere body
         t = dot (c-a) d

---
--- tests
---

makeBody :: Geometry -> RealNum -> Vector RealNum -> Quaternion RealNum -> Vector RealNum -> Vector RealNum -> Bool -> Body
makeBody geom theMass pos rot vel rvel anchor
   = Body_ { geometry = geom,
             mass = theMass,
             invMass = if anchor then 0 else recip theMass,
             inertiaBody = theInertiaBody,
             invInertiaBody = theInvInertiaBody,
             position = pos,
             orientation = Quaternions.normalise rot,
             velocity = vel,
             rotationalVel = rvel,
             momentum = vel .*. theMass,
             angularMomentum = theInertia `matXvec` rvel,
             invInertia = theRotMatrix * theInvInertiaBody * (transposeMat theRotMatrix),
             rotationMat = theRotMatrix,
             force = 0,
             torque = 0,
             anchored = anchor }
   where theInertiaBody = makeInertiaTensor geom theMass
         theInvInertiaBody = if anchor then 0 else Matrix3x3.inverse theInertia
         theRotMatrix = rotMatrix rot
         theInertia = theRotMatrix * theInertiaBody * (transposeMat theRotMatrix)

makeInertiaTensor :: Geometry -> RealNum -> Matrix3x3 RealNum
makeInertiaTensor (Sphere r) m = 1.*.(0.4*m*r*r)
makeInertiaTensor (Box x y z) m = (Mat (y*y + z*z,0,0) (0,x*x + z*z,0) (0,0,x*x + y*y)).*.(m/3)
makeInertiaTensor (Plane a b c) m = 1.*.m

-- this is not correct - it is only the inertia of a cylinder - should add in inertia for hemispherical caps.
makeInertiaTensor (SphereCappedCylinder r h) m = (Mat (h*h/6 + r*r/2,0,0) (0,h*h/6 + r*r/2,0) (0,0,r*r)).*.(m/2)

-- a hack is used - assume all subcomponents have equal masses all summing to the total mass
makeInertiaTensor (CompoundGeometry g) m
   = sum [let rotM = rotMatrix rot in rotM*(makeInertiaTensor geom m')*(transposeMat rotM) - (crossMatrix loc)^2.*.m' | (geom,rot,loc) <- g]
   where m' = m / (fromIntegral (length g))

-- rotMatrix: Takes a unit quaternion and gives the equivalent rotation matrix.
-- assumes quaternion is already normalised
rotMatrix :: (RealFloat a) => Quaternion a -> Matrix3x3 a
rotMatrix (Q s vx vy vz) =
   Mat (1 - 2*vy*vy - 2*vz*vz, 2*vx*vy - 2*s*vz, 2*vx*vz + 2*s*vy)
       (2*vx*vy + 2*s*vz, 1 - 2*vx*vx - 2*vz*vz, 2*vy*vz - 2*s*vx)
       (2*vx*vz - 2*s*vy, 2*vy*vz + 2*s*vx, 1 - 2*vx*vx - 2*vy*vy)
{-
from ODE source:
  // if the body is translated by `a' relative to its point of reference,
  // the new inertia about the point of reference is:
  //
  //   I + mass*(crossmat(c)^2 - crossmat(c+a)^2)
  //
  // where c is the existing center of mass and I is the old inertia.
-}
{-
  // if the body is rotated by `R' relative to its point of reference,
  // the new inertia about the point of reference is:
  //
  //   R * I * R'
  //
  // where I is the old inertia.
-}

makeJointGroups :: Bodies -> [Joint] -> [[Joint]]
makeJointGroups _ [] = []
makeJointGroups bodies (j@(Joint_ a b _):js) = (j:(concat jGroups)) : njGroups
   where
      groups = makeJointGroups bodies js
      (jGroups,njGroups) = partition (any (\(Joint_ a1 b1 _) -> a1==a||a1==b||b1==a||b1==b)) groups

{-
makeJointGroups :: Bodies -> [Joint] -> [[Joint]]
makeJointGroups bodies js = equivalenceClasses (connectedJoints bodies js) js

equivalenceClasses :: (a -> a -> Bool) -> [a] -> [[a]]
equivalenceClasses _ [] = []
equivalenceClasses r (x:xs) = (x:ys) : (equivalenceClasses r nys)
   where (ys,nys) = partition (r x) xs

connectedJoints bodies js (Joint_ a1 b1 _) (Joint_ a2 b2 _) =
   (connectedBodies bodies js a1 a2) || (connectedBodies bodies js a1 b2) || (connectedBodies bodies js b1 a2) || (connectedBodies bodies js b1 b2)

connectedBodies bodies js start end
   | (anchored (bodies!start) || anchored (bodies!end)) = False
   | (start == end) = True
   | otherwise = or ([connectedBodies bodies nbs b end | (Joint_ _ b _) <- bs] ++ [connectedBodies bodies nas a end | (Joint_ _ a _) <- as])
   where (bs,nbs) = partition (\(Joint_ a _ _) -> a == start) js
         (as,nas) = partition (\(Joint_ _ b _) -> b == start) js
-}

minVectorComponents :: RealFloat a => Vector a -> Vector a -> Vector a
minVectorComponents (Vec a b c) (Vec d e f) = Vec (min a d) (min b e) (min c f)

maxVectorComponents :: RealFloat a => Vector a -> Vector a -> Vector a
maxVectorComponents (Vec a b c) (Vec d e f) = Vec (max a d) (max b e) (max c f)