{-
   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.
-}

-- Matrix3x3
-- Code by Ruben Henner Zilibowitz
-- Created on 20/3/07

module Matrix3x3 where

infixl 7 .*., ./.

data (RealFloat a) => Matrix3x3 a = Mat !(a,a,a) !(a,a,a) !(a,a,a)   deriving (Eq, Read, Show)
data (RealFloat a) => Vector a = Vec !a !a !a   deriving (Eq, Read, Show)

class Scalar a where
   (.*.) :: RealFloat b => a b -> b -> a b
   (./.) :: RealFloat b => a b -> b -> a b

instance Scalar Matrix3x3 where
   (Mat (a,b,c) (d,e,f) (g,h,i)) .*. x = (Mat (a*x,b*x,c*x) (d*x,e*x,f*x) (g*x,h*x,i*x))
   m ./. x = let x' = recip x in m .*. x'

instance Scalar Vector where
   (Vec a b c) .*. x = Vec (a*x) (b*x) (c*x)
   v ./. x = let x' = recip x in v .*. x'

---
--- Matrix3x3
---

instance RealFloat a => Num (Matrix3x3 a) where
   fromInteger a = let x = fromInteger a in Mat (x,0,0) (0,x,0) (0,0,x)
   (Mat (a,b,c) (d,e,f) (g,h,i)) + (Mat (a',b',c') (d',e',f') (g',h',i')) = Mat (a+a',b+b',c+c') (d+d',e+e',f+f') (g+g',h+h',i+i')
   (Mat (a,b,c) (d,e,f) (g,h,i)) - (Mat (a',b',c') (d',e',f') (g',h',i')) = Mat (a-a',b-b',c-c') (d-d',e-e',f-f') (g-g',h-h',i-i')
   (Mat (a,b,c) (d,e,f) (g,h,i)) * (Mat (a',b',c') (d',e',f') (g',h',i')) = Mat (a*a'+b*d'+c*g',a*b'+b*e'+c*h',a*c'+b*f'+c*i')
                                                                                (d*a'+e*d'+f*g',d*b'+e*e'+f*h',d*c'+e*f'+f*i')
                                                                                (g*a'+h*d'+i*g',g*b'+h*e'+i*h',g*c'+h*f'+i*i')
   abs m = let x = det m in Mat (x,0,0) (0,x,0) (0,0,x)
   signum m | (d == 0)  = 0
            | otherwise = m .*. (recip d)
      where d = det m

instance  (RealFloat a) => Fractional (Matrix3x3 a)  where
   q / r = q * (inverse r)
   fromRational a = let x = fromRational a in Mat (x,0,0) (0,x,0) (0,0,x)

inverse (Mat (a00,a01,a02) (a10,a11,a12) (a20,a21,a22))
   = Mat ((a11*a22-a12*a21)/denom,(a02*a21-a01*a22)/denom,(a01*a12-a02*a11)/denom)
         ((a12*a20-a10*a22)/denom,(a00*a22-a02*a20)/denom,(a02*a10-a00*a12)/denom)
         ((a10*a21-a11*a20)/denom,(a01*a20-a00*a21)/denom,(a00*a11-a01*a10)/denom)
   where denom = a00*(a11*a22-a12*a21)+a01*(a12*a20-a10*a22)+a02*(a10*a21-a11*a20)

det (Mat (a00,a01,a02) (a10,a11,a12) (a20,a21,a22)) = a00*(a11*a22-a12*a21)-a01*(a10*a22-a12*a20)+a02*(a10*a21-a11*a20)

transposeMat (Mat (a,b,c) (d,e,f) (g,h,i)) = Mat (a,d,g) (b,e,h) (c,f,i)

normalise v = v ./. (magnitude v)

-- crossMatrix: Vector a becomes matrix A with the property: A*b = cross a b
crossMatrix :: (RealFloat a) => Vector a -> Matrix3x3 a
crossMatrix (Vec x y z) = Mat (0,-z,y) (z,0,-x) (-y,x,0)

-- crossMatrix2: Vector b becomes matrix B with the property: B*a = cross a b
crossMatrix2 :: (RealFloat a) => Vector a -> Matrix3x3 a
crossMatrix2 b = negate (crossMatrix b)

scaleMatrix :: (RealFloat a) => a -> Matrix3x3 a
scaleMatrix s = Mat (s,0,0) (0,s,0) (0,0,s)

matToList (Mat (a,b,c) (d,e,f) (g,h,i)) = [[a,b,c],[d,e,f],[g,h,i]]

catMatrices :: (RealFloat a) => [[Matrix3x3 a]] -> [[a]]
catMatrices [] = []
catMatrices (r:rs) = (foldl1 (zipWith (++)) (map matToList r)) ++ (catMatrices rs)

---
--- Vector
---

dot (Vec a b c) (Vec x y z) = a*x + b*y + c*z

cross (Vec a b c) (Vec x y z) = Vec (b*z-c*y) (c*x-a*z) (a*y-b*x)

magnitude (Vec a b c) = sqrt (a*a + b*b + c*c)

norm v = dot v v

instance RealFloat a => Num (Vector a) where
   fromInteger a = Vec (fromInteger a) 0 0
   (Vec a b c) + (Vec x y z) = Vec (a+x) (b+y) (c+z)
   (Vec a b c) - (Vec x y z) = Vec (a-x) (b-y) (c-z)
   x * y = cross x y
   abs m = Vec (magnitude m) 0 0
   signum m = error "undefined"

matXvec (Mat (a,b,c) (d,e,f) (g,h,i)) (Vec x y z) = Vec (x*a+y*b+z*c) (x*d+y*e+z*f) (x*g+y*h+z*i)

toMatrixCols (Vec a b c) (Vec d e f) (Vec g h i) = Mat (a,d,g) (b,e,h) (c,f,i)

col 0 (Mat (a,_,_) (d,_,_) (g,_,_)) = Vec a d g
col 1 (Mat (_,b,_) (_,e,_) (_,h,_)) = Vec b e h
col 2 (Mat (_,_,c) (_,_,f) (_,_,i)) = Vec c f i

row 0 (Mat (a,b,c) _ _) = Vec a b c
row 1 (Mat _ (a,b,c) _) = Vec a b c
row 2 (Mat _ _ (a,b,c)) = Vec a b c

coord 0 (Vec x _ _) = x
coord 1 (Vec _ y _) = y
coord 2 (Vec _ _ z) = z

vecAbs (Vec x y z) = Vec (abs x) (abs y) (abs z)

matAbs (Mat (a,b,c) (d,e,f) (g,h,i)) = (Mat (abs a,abs b,abs c) (abs d,abs e,abs f) (abs g,abs h,abs i))

vecToList (Vec a b c) = [a,b,c]

---
--- tests
---

test1 = Mat (2,1,1) (0,-1,2) (0,2,-1)