geomancy-0.4.0.0: src/Geomancy/DMat4.hs

{-# LANGUAGE BangPatterns #-}
{-# LANGUAGE BlockArguments #-}
{-# LANGUAGE FlexibleContexts #-}
{-# LANGUAGE MagicHash #-}
{-# LANGUAGE ScopedTypeVariables #-}
{-# LANGUAGE TypeApplications #-}
{-# LANGUAGE UnboxedTuples #-}
{-# LANGUAGE UnliftedFFITypes #-}
{-# LANGUAGE ViewPatterns #-}

{-# OPTIONS_GHC -Wno-orphans #-}

module Geomancy.DMat4
  ( DMat4
    -- * Operations
  , identity
  , transpose
  , inverse
  , det
  , pointwise
  , zipWith
  , matrixProduct
  , scalarMultiply
  , (!*)

    -- * Construction

    -- ** Col-major
  , colMajor
  , withColMajor
  , toListColMajor
  , toListColMajor2d
  , showColumns

    -- ** Row-major
  , rowMajor
  , withRowMajor
  , toListRowMajor
  , toListRowMajor2d
  , fromRowMajor2d
  , showRows

    -- * Precision conversion
  , fromMat4
  , toMat4
  ) where

import Prelude hiding (zipWith)
import GHC.Exts hiding (VecCount(..), toList)
import GHC.Float (double2Float, float2Double)
import Geomancy.DMat4.Internal

import System.IO.Unsafe (unsafePerformIO)
import Text.Printf (printf)

import qualified Data.Foldable as Foldable

import Geomancy.DVec4 (DVec4(..), newDVec4)
import Geomancy.Mat4.Internal (Mat4)
import qualified Geomancy.Mat4.Internal as Mat4

{-# INLINE fromMat4 #-}
fromMat4 :: Coercible a Mat4 => a -> DMat4
fromMat4 m = Mat4.withMemory (coerce m)
  \ e0 e1 e2 e3
    e4 e5 e6 e7
    e8 e9 eA eB
    eC eD eE eF ->
  fromMemory
    (float2Double e0) (float2Double e1) (float2Double e2) (float2Double e3)
    (float2Double e4) (float2Double e5) (float2Double e6) (float2Double e7)
    (float2Double e8) (float2Double e9) (float2Double eA) (float2Double eB)
    (float2Double eC) (float2Double eD) (float2Double eE) (float2Double eF)

{-# INLINE toMat4 #-}
toMat4 :: DMat4 -> Mat4
toMat4 m = withMemory m
  \ e0 e1 e2 e3
    e4 e5 e6 e7
    e8 e9 eA eB
    eC eD eE eF ->
  Mat4.fromMemory
    (double2Float e0) (double2Float e1) (double2Float e2) (double2Float e3)
    (double2Float e4) (double2Float e5) (double2Float e6) (double2Float e7)
    (double2Float e8) (double2Float e9) (double2Float eA) (double2Float eB)
    (double2Float eC) (double2Float eD) (double2Float eE) (double2Float eF)

toListColMajor :: Coercible a DMat4 => a -> [Double]
toListColMajor = toListTrans . coerce

toListColMajor2d :: Coercible a DMat4 => a -> [[Double]]
toListColMajor2d = toList2dTrans . coerce

rowMajor
  :: Coercible DMat4 a
  => Double -> Double -> Double -> Double
  -> Double -> Double -> Double -> Double
  -> Double -> Double -> Double -> Double
  -> Double -> Double -> Double -> Double
  -> a
rowMajor
  e0 e1 e2 e3
  e4 e5 e6 e7
  e8 e9 eA eB
  eC eD eE eF =
    coerce $ fromMemory
      e0 e1 e2 e3
      e4 e5 e6 e7
      e8 e9 eA eB
      eC eD eE eF

fromRowMajor2d
  :: forall t a
  .  ( Foldable t
     , Coercible DMat4 a
     )
  => t (t Double)
  -> Maybe a
fromRowMajor2d rows =
  case Foldable.toList rows of
    [r0, r1, r2, r3] ->
      withRow r0 \e0 e1 e2 e3 ->
      withRow r1 \e4 e5 e6 e7 ->
      withRow r2 \e8 e9 eA eB ->
      withRow r3 \eC eD eE eF ->
        Just . coerce @a $ rowMajor
          e0 e1 e2 e3
          e4 e5 e6 e7
          e8 e9 eA eB
          eC eD eE eF
    _ ->
      Nothing
  where
    withRow row f =
      case Foldable.toList row of
        [c0, c1, c2, c3] ->
          f c0 c1 c2 c3
        _ ->
          Nothing

{-# INLINE withRowMajor #-}
withRowMajor
  :: Coercible a DMat4
  => a
  ->
    ( Double -> Double -> Double -> Double ->
      Double -> Double -> Double -> Double ->
      Double -> Double -> Double -> Double ->
      Double -> Double -> Double -> Double ->
      r
    )
  -> r
withRowMajor m f = withMemory (coerce m)
  \ e0 e1 e2 e3
    e4 e5 e6 e7
    e8 e9 eA eB
    eC eD eE eF ->
  f
    e0 e1 e2 e3
    e4 e5 e6 e7
    e8 e9 eA eB
    eC eD eE eF

{-# INLINE colMajor #-}
colMajor
  :: forall a . Coercible DMat4 a
  => Double -> Double -> Double -> Double
  -> Double -> Double -> Double -> Double
  -> Double -> Double -> Double -> Double
  -> Double -> Double -> Double -> Double
  -> a
colMajor
  e0 e4 e8 eC
  e1 e5 e9 eD
  e2 e6 eA eE
  e3 e7 eB eF =
    coerce $ fromMemory
      e0 e1 e2 e3
      e4 e5 e6 e7
      e8 e9 eA eB
      eC eD eE eF

{-# INLINE withColMajor #-}
withColMajor
  :: Coercible a DMat4
  => a
  ->
    ( Double -> Double -> Double -> Double ->
      Double -> Double -> Double -> Double ->
      Double -> Double -> Double -> Double ->
      Double -> Double -> Double -> Double ->
      r
    )
  -> r
withColMajor m f = withMemory (coerce m)
  \ e0 e4 e8 eC
    e1 e5 e9 eD
    e2 e6 eA eE
    e3 e7 eB eF ->
  f
    e0 e1 e2 e3
    e4 e5 e6 e7
    e8 e9 eA eB
    eC eD eE eF

toListRowMajor :: Coercible a DMat4 => a -> [Double]
toListRowMajor = toListMemory . coerce

toListRowMajor2d :: Coercible a DMat4 => a -> [[Double]]
toListRowMajor2d = toList2dMemory . coerce

det :: Coercible DMat4 a => a -> Double
det m =
  withRowMajor m
    \ m00 m01 m02 m03
      m10 m11 m12 m13
      m20 m21 m22 m23
      m30 m31 m32 m33 ->
        let
          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
        in
            s0 * c5
          - s1 * c4
          + s2 * c3
          + s3 * c2
          - s4 * c1
          + s5 * c0

inverse :: Coercible DMat4 a => a -> a
inverse m =
  withRowMajor m
    \ m00 m01 m02 m03
      m10 m11 m12 m13
      m20 m21 m22 m23
      m30 m31 m32 m33 ->
        let
          invDet = recip $
              s0 * c5
            - s1 * c4
            + s2 * c3
            + s3 * c2
            - s4 * c1
            + s5 * c0

          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

          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
        in
          rowMajor
            i00 i01 i02 i03
            i10 i11 i12 i13
            i20 i21 i22 i23
            i30 i31 i32 i33

foreign import ccall unsafe "DMat4xDMat4_SIMD" m4m4simd :: ByteArray# -> ByteArray# -> ByteArray# -> IO ()

{-# INLINE matrixProduct #-}
matrixProduct :: DMat4 -> DMat4 -> DMat4
matrixProduct (DMat4 l) (DMat4 r) = unsafePerformIO do
  result@(DMat4 m) <- newDMat4
  m4m4simd l r m
  pure result

foreign import ccall unsafe "DMat4xDVec4_SIMD" m4v4simd :: ByteArray# -> ByteArray# -> ByteArray# -> IO ()

(!*) :: Coercible a DMat4 => a -> DVec4 -> DVec4
(!*) (coerce -> DMat4 m) (DVec4 v) = unsafePerformIO do
  result@(DVec4 o) <- newDVec4
  !() <- m4v4simd m v o
  pure result

infixr 5 !*

instance Semigroup DMat4 where
  {-# INLINE (<>) #-}
  (<>) = matrixProduct

instance Monoid DMat4 where
  {-# INLINE mempty #-}
  mempty = identity

instance Show DMat4 where
  show = showColumns

showColumns :: DMat4 -> String
showColumns cm = withColMajor cm
  \ e0 e4 e8 eC
    e1 e5 e9 eD
    e2 e6 eA eE
    e3 e7 eB eF ->
  unlines
    [ printf  "/ %.4f %.4f %.4f %.4f \\" e0 e4 e8 eC
    , printf  "| %.4f %.4f %.4f %.4f |"  e1 e5 e9 eD
    , printf  "| %.4f %.4f %.4f %.4f |"  e2 e6 eA eE
    , printf "\\ %.4f %.4f %.4f %.4f /"  e3 e7 eB eF
    ]

showRows :: DMat4 -> String
showRows rm = withRowMajor rm
  \ e0 e1 e2 e3
    e4 e5 e6 e7
    e8 e9 eA eB
    eC eD eE eF ->
  unlines
    [ printf "[ %.4f %.4f %.4f %.4f |" e0 e1 e2 e3
    , printf "| %.4f %.4f %.4f %.4f |" e4 e5 e6 e7
    , printf "| %.4f %.4f %.4f %.4f |" e8 e9 eA eB
    , printf "| %.4f %.4f %.4f %.4f ]" eC eD eE eF
    ]