module Data.Vect.Flt.Util.Dim3 where

import Data.Vect.Flt.Base

structVec3 :: [Flt] -> [Vec3]
structVec3 [] = []
structVec3 (x:y:z:ls) = (Vec3 x y z):(structVec3 ls) 
structVec3 _ = error "structVec3"

destructVec3 :: [Vec3] -> [Flt]
destructVec3 [] = []
destructVec3 ((Vec3 x y z):ls) = x:y:z:(destructVec3 ls)  

det3 :: Vec3 -> Vec3 -> Vec3 -> Flt
det3 u v w = det (u,v,w)

translate3X :: Flt -> Vec3 -> Vec3
translate3Y :: Flt -> Vec3 -> Vec3
translate3Z :: Flt -> Vec3 -> Vec3

translate3X t (Vec3 x y z) = Vec3 (x+t) y z 
translate3Y t (Vec3 x y z) = Vec3 x (y+t) z 
translate3Z t (Vec3 x y z) = Vec3 x y (z+t) 

vec3X :: Vec3
vec3Y :: Vec3
vec3Z :: Vec3

vec3X = Vec3 1 0 0
vec3Y = Vec3 0 1 0
vec3Z = Vec3 0 0 1

rotMatrixZ :: Flt -> Mat3
rotMatrixY :: Flt -> Mat3
rotMatrixX :: Flt -> Mat3

-- These are intended for multiplication on the /right/.
-- Should be consistent with the rotation around an arbitrary axis 
-- (eg, @rotMatrixY a == rotate3 a vec3Y@)
rotMatrixZ a = Mat3 (Vec3 c s 0) (Vec3 (-s) c 0) (Vec3 0 0 1) where c = cos a; s = sin a
rotMatrixY a = Mat3 (Vec3 c 0 (-s)) (Vec3 0 1 0) (Vec3 s 0 c) where c = cos a; s = sin a
rotMatrixX a = Mat3 (Vec3 1 0 0) (Vec3 0 c s) (Vec3 0 (-s) c) where c = cos a; s = sin a

rotate3' :: {- ' CPP is sensitive to primes -} Flt       -- ^ angle (in radians)
         -> Normal3   -- ^ axis (should be a /unit/ vector!) 
         -> Vec3      -- ^ vector
         -> Vec3      -- ^ result
rotate3' angle axis v = v .* (rotMatrix3' axis angle)

rotate3 :: Flt    -- ^ angle (in radians)
        -> Vec3   -- ^ axis (arbitrary nonzero vector)
        -> Vec3   -- ^ vector
        -> Vec3   -- ^ result
rotate3 angle axis v = v .* (rotMatrix3 axis angle)
      
-- |Rotation around an arbitrary 3D vector. The resulting 3x3 matrix is intended for multiplication on the /right/. 
rotMatrix3 :: Vec3 -> Flt -> Mat3
rotMatrix3 v a = rotMatrix3' (mkNormal v) a

-- |Rotation around an arbitrary 3D /unit/ vector. The resulting 3x3 matrix is intended for multiplication on the /right/. 
rotMatrix3' :: {- ' CPP is sensitive to primes -} Normal3 -> Flt -> Mat3
rotMatrix3' (Normal3 v) a = 
  let c = cos a
      s = sin a
      m1 = scalarMul (1-c) (outer v v)
      x = _1 v
      y = _2 v
      z = _3 v
      m2 = Mat3 (Vec3   c    ( s*z) (-s*y))
                (Vec3 (-s*z)   c    ( s*x))
                (Vec3 ( s*y) (-s*x)   c   )
  in (m1 &+ m2)