Mat4.hs

{-# LANGUAGE RecordWildCards #-}

module Support.Reference.Mat4
  ( rowMajor
  , colMajor
  , identity
  , compose
  , apply
  , det
  , inverse
  ) where

import Data.Foldable (toList)
import Linear (M44, V4(..), transpose)

import Support.Ball (Ball, sumAnyOrder)

rowMajor
  :: a -> a -> a -> a
  -> a -> a -> a -> a
  -> a -> a -> a -> a
  -> a -> a -> a -> a
  -> M44 a
rowMajor
  m00 m01 m02 m03
  m10 m11 m12 m13
  m20 m21 m22 m23
  m30 m31 m32 m33 =
    V4
      (V4 m00 m01 m02 m03)
      (V4 m10 m11 m12 m13)
      (V4 m20 m21 m22 m23)
      (V4 m30 m31 m32 m33)

colMajor
  :: a -> a -> a -> a
  -> a -> a -> a -> a
  -> a -> a -> a -> a
  -> a -> a -> a -> a
  -> M44 a
colMajor
  m00 m01 m02 m03
  m10 m11 m12 m13
  m20 m21 m22 m23
  m30 m31 m32 m33 =
    transpose $ rowMajor
      m00 m01 m02 m03
      m10 m11 m12 m13
      m20 m21 m22 m23
      m30 m31 m32 m33

identity :: Num a => M44 a
identity = rowMajor
  1 0 0 0
  0 1 0 0
  0 0 1 0
  0 0 0 1

compose :: RealFloat p => M44 (Ball p) -> M44 (Ball p) -> M44 (Ball p)
compose a b = fmap (\row -> fmap (dotAnyOrder row) (transpose a)) b

apply :: RealFloat p => M44 (Ball p) -> V4 (Ball p) -> V4 (Ball p)
apply m v = fmap (dotAnyOrder v) (transpose m)

dotAnyOrder :: RealFloat p => V4 (Ball p) -> V4 (Ball p) -> Ball p
dotAnyOrder a b = sumAnyOrder $ zipWith (*) (toList a) (toList b)

det :: Num a => M44 a -> a
det m =
    s0 * c5
  - s1 * c4
  + s2 * c3
  + s3 * c2
  - s4 * c1
  + s5 * c0
  where
    Cofactors{..} = cofactors m

inverse :: Fractional a => M44 a -> M44 a
inverse m@(V4
  (V4 m00 m01 m02 m03)
  (V4 m10 m11 m12 m13)
  (V4 m20 m21 m22 m23)
  (V4 m30 m31 m32 m33)) =
    rowMajor
      i00 i01 i02 i03
      i10 i11 i12 i13
      i20 i21 i22 i23
      i30 i31 i32 i33
  where
    Cofactors{..} = cofactors m

    invDet = recip (det m)

    i00 = ( m11 * c5 - m12 * c4 + m13 * c3) * invDet
    i01 = (-m01 * c5 + m02 * c4 - m03 * c3) * invDet
    i02 = ( m31 * s5 - m32 * s4 + m33 * s3) * invDet
    i03 = (-m21 * s5 + m22 * s4 - m23 * s3) * invDet

    i10 = (-m10 * c5 + m12 * c2 - m13 * c1) * invDet
    i11 = ( m00 * c5 - m02 * c2 + m03 * c1) * invDet
    i12 = (-m30 * s5 + m32 * s2 - m33 * s1) * invDet
    i13 = ( m20 * s5 - m22 * s2 + m23 * s1) * invDet

    i20 = ( m10 * c4 - m11 * c2 + m13 * c0) * invDet
    i21 = (-m00 * c4 + m01 * c2 - m03 * c0) * invDet
    i22 = ( m30 * s4 - m31 * s2 + m33 * s0) * invDet
    i23 = (-m20 * s4 + m21 * s2 - m23 * s0) * invDet

    i30 = (-m10 * c3 + m11 * c1 - m12 * c0) * invDet
    i31 = ( m00 * c3 - m01 * c1 + m02 * c0) * invDet
    i32 = (-m30 * s3 + m31 * s1 - m32 * s0) * invDet
    i33 = ( m20 * s3 - m21 * s1 + m22 * s0) * invDet

data Cofactors a = Cofactors
  { s0, s1, s2, s3, s4, s5 :: a
  , c0, c1, c2, c3, c4, c5 :: a
  }

cofactors :: Num a => M44 a -> Cofactors a
cofactors (V4
  (V4 m00 m01 m02 m03)
  (V4 m10 m11 m12 m13)
  (V4 m20 m21 m22 m23)
  (V4 m30 m31 m32 m33)) =
    Cofactors
      { s0 = m00 * m11 - m10 * m01
      , s1 = m00 * m12 - m10 * m02
      , s2 = m00 * m13 - m10 * m03
      , s3 = m01 * m12 - m11 * m02
      , s4 = m01 * m13 - m11 * m03
      , s5 = m02 * m13 - m12 * m03

      , c5 = m22 * m33 - m32 * m23
      , c4 = m21 * m33 - m31 * m23
      , c3 = m21 * m32 - m31 * m22
      , c2 = m20 * m33 - m30 * m23
      , c1 = m20 * m32 - m30 * m22
      , c0 = m20 * m31 - m30 * m21
      }