never executed always true always false
    1 {-|
    2 Module: Flaw.Math.Transform
    3 Description: Geometric functions.
    4 License: MIT
    5 -}
    6 
    7 {-# LANGUAGE DeriveGeneric, ScopedTypeVariables, Trustworthy #-}
    8 
    9 module Flaw.Math.Transform
   10   (
   11   -- * Transform class
   12     Transform(..)
   13   -- * Quaternion plus offset
   14   , QuatOffset(..)
   15   , FloatQO, DoubleQO
   16   -- * Dual quaternion
   17   , DualQuat(..)
   18   , FloatDQ, DoubleDQ
   19   -- * Helper functions
   20   , quatAxisRotation
   21   ) where
   22 
   23 import qualified Data.Serialize as S
   24 import Foreign.Ptr
   25 import Foreign.Storable
   26 import GHC.Generics(Generic)
   27 
   28 import Flaw.Math
   29 
   30 class Transform t where
   31   identityTransform :: Quaternionized a => t a
   32   applyTransform :: Quaternionized a => t a -> Vec3 a -> Vec3 a
   33   combineTransform :: Quaternionized a => t a -> t a -> t a
   34   transformToMatrix :: Quaternionized a => t a -> Mat4x4 a
   35   transformFromMatrix :: Quaternionized a => Mat4x4 a -> t a
   36   transformTranslation :: Quaternionized a => Vec3 a -> t a
   37   transformAxisRotation :: Quaternionized a => Vec3 a -> a -> t a
   38 
   39 instance Transform Mat4x4 where
   40   {-# INLINE identityTransform #-}
   41   identityTransform = Mat4x4
   42     1 0 0 0
   43     0 1 0 0
   44     0 0 1 0
   45     0 0 0 1
   46   {-# INLINE applyTransform #-}
   47   applyTransform m (Vec3 x y z) = Vec3 rx ry rz where
   48     Vec4 rx ry rz _rw = mul m (Vec4 x y z 1)
   49   {-# INLINE combineTransform #-}
   50   combineTransform = mul
   51   {-# INLINE transformToMatrix #-}
   52   transformToMatrix = id
   53   {-# INLINE transformFromMatrix #-}
   54   transformFromMatrix = id
   55   {-# INLINE transformTranslation #-}
   56   transformTranslation (Vec3 x y z) = Mat4x4
   57     1 0 0 x
   58     0 1 0 y
   59     0 0 1 z
   60     0 0 0 1
   61   transformAxisRotation axis angle = transformToMatrix $ QuatOffset (quatAxisRotation axis angle) (Vec3 0 0 0)
   62 
   63 -- | Quaternion representing rotation around specified axis.
   64 -- Axis should be normalized.
   65 {-# INLINE quatAxisRotation #-}
   66 quatAxisRotation :: Quaternionized a => Vec3 a -> a -> Quat a
   67 quatAxisRotation (Vec3 x y z) angle = r where
   68   ha = angle * 0.5
   69   sa = sin ha
   70   ca = cos ha
   71   r = Quat $ Vec4 (x * sa) (y * sa) (z * sa) ca
   72 
   73 -- | 3D transformation represented by normalized quaternion and offset.
   74 data QuatOffset a = QuatOffset (Quat a) (Vec3 a) deriving (Eq, Ord, Show, Generic)
   75 
   76 instance (Quaternionized a, S.Serialize a) => S.Serialize (QuatOffset a)
   77 
   78 instance (Quaternionized a, Storable a) => Storable (QuatOffset a) where
   79   sizeOf _ = sizeOf (undefined :: Quat a) + sizeOf (undefined :: Vec3 a)
   80   alignment _ = alignment (undefined :: Quat a)
   81   peek ptr = do
   82     q <- peek $ castPtr ptr
   83     p <- peek $ castPtr $ plusPtr ptr $ sizeOf q
   84     return $ QuatOffset q p
   85   poke ptr (QuatOffset q p) = do
   86     poke (castPtr ptr) q
   87     poke (castPtr $ plusPtr ptr $ sizeOf q) p
   88 
   89 type FloatQO = QuatOffset Float
   90 type DoubleQO = QuatOffset Double
   91 
   92 instance Transform QuatOffset where
   93   {-# INLINE identityTransform #-}
   94   identityTransform = QuatOffset (Quat (Vec4 0 0 0 1)) (Vec3 0 0 0)
   95   {-# INLINE applyTransform #-}
   96   applyTransform (QuatOffset q p) (Vec3 x y z) = Vec3 rx ry rz + p where
   97     Quat (Vec4 rx ry rz _rw) = q * Quat (Vec4 x y z 0) * conjugate q
   98   {-# INLINE combineTransform #-}
   99   combineTransform t2@(QuatOffset q2 _p2) (QuatOffset q1 p1) = QuatOffset (q2 * q1) (applyTransform t2 p1)
  100   {-# INLINE transformToMatrix #-}
  101   transformToMatrix (QuatOffset (Quat (Vec4 x y z w)) (Vec3 px py pz)) = r where
  102     ww = w * w
  103     xx = x * x
  104     yy = y * y
  105     zz = z * z
  106     wx2 = w * x * 2
  107     wy2 = w * y * 2
  108     wz2 = w * z * 2
  109     xy2 = x * y * 2
  110     xz2 = x * z * 2
  111     yz2 = y * z * 2
  112     r = Mat4x4
  113       (ww + xx - yy - zz) (xy2 - wz2) (xz2 + wy2) px
  114       (xy2 + wz2) (ww - xx + yy - zz) (yz2 - wx2) py
  115       (xz2 - wy2) (yz2 + wx2) (ww - xx - yy + zz) pz
  116       0 0 0 1
  117   {-# INLINE transformFromMatrix #-}
  118   transformFromMatrix (Mat4x4
  119     m11 m12 m13 m14
  120     m21 m22 m23 m24
  121     m31 m32 m33 m34
  122     _41 _42 _43 _44) = QuatOffset (normalize $ Quat (Vec4 x y z w)) (Vec3 m14 m24 m34) where
  123     k = sqrt (1 + m11 + m22 + m33)
  124     kk = 0.5 / k
  125     x = (m32 - m23) * kk
  126     y = (m13 - m31) * kk
  127     z = (m21 - m12) * kk
  128     w = k * 0.5
  129   {-# INLINE transformTranslation #-}
  130   transformTranslation = QuatOffset (Quat (Vec4 0 0 0 1))
  131   {-# INLINE transformAxisRotation #-}
  132   transformAxisRotation axis angle = QuatOffset (quatAxisRotation axis angle) (Vec3 0 0 0)
  133 
  134 -- | Dual quaternion representing transforms in 3D space.
  135 data DualQuat a = DualQuat !(Quat a) !(Quat a) deriving (Eq, Ord, Show, Generic)
  136 
  137 instance (Quaternionized a, S.Serialize a) => S.Serialize (DualQuat a)
  138 
  139 instance (Quaternionized a, Storable a) => Storable (DualQuat a) where
  140   sizeOf _ = sizeOf (undefined :: Quat a) * 2
  141   alignment _ = alignment (undefined :: Quat a)
  142   peek ptr = do
  143     q <- peek $ castPtr ptr
  144     p <- peek $ castPtr $ plusPtr ptr $ sizeOf q
  145     return $ DualQuat q p
  146   poke ptr (DualQuat q p) = do
  147     poke (castPtr ptr) q
  148     poke (castPtr $ plusPtr ptr $ sizeOf q) p
  149 
  150 type FloatDQ = DualQuat Float
  151 type DoubleDQ = DualQuat Double
  152 
  153 instance Quaternionized a => Num (DualQuat a) where
  154   {-# INLINE (+) #-}
  155   (DualQuat q1 p1) + (DualQuat q2 p2) = DualQuat (q1 + q2) (p1 + p2)
  156   {-# INLINE (-) #-}
  157   (DualQuat q1 p1) - (DualQuat q2 p2) = DualQuat (q1 - q2) (p1 - p2)
  158   {-# INLINE (*) #-}
  159   (DualQuat q1 p1) * (DualQuat q2 p2) = DualQuat (q1 * q2) (q1 * p2 + p1 * q2)
  160   {-# INLINE negate #-}
  161   negate (DualQuat q p) = DualQuat (negate q) (negate p)
  162   {-# INLINE abs #-}
  163   abs (DualQuat q p) = DualQuat (abs q) (abs p)
  164   {-# INLINE signum #-}
  165   signum = error "signum for DualQuat is not implemented"
  166   {-# INLINE fromInteger #-}
  167   fromInteger = error "fromInteger for DualQuat is not implemented"
  168 
  169 instance Quaternionized a => Conjugate (DualQuat a) where
  170   {-# INLINE conjugate #-}
  171   conjugate (DualQuat q p) = DualQuat (conjugate q) (conjugate p)
  172 
  173 {-# INLINE dualConjugate #-}
  174 dualConjugate :: Quaternionized a => DualQuat a -> DualQuat a
  175 dualConjugate (DualQuat q p) = DualQuat q (negate p)
  176 
  177 -- | Equivalent of (dualConjugate . conjugate)
  178 {-# INLINE dualConjugateConjugate #-}
  179 dualConjugateConjugate :: Quaternionized a => DualQuat a -> DualQuat a
  180 dualConjugateConjugate (DualQuat (Quat (Vec4 qx qy qz qw)) (Quat (Vec4 px py pz pw))) =
  181   DualQuat (Quat (Vec4 (-qx) (-qy) (-qz) qw)) (Quat (Vec4 px py pz (-pw)))
  182 
  183 instance Transform DualQuat where
  184   {-# INLINE identityTransform #-}
  185   identityTransform = DualQuat (Quat (Vec4 0 0 0 1)) (Quat (Vec4 0 0 0 0))
  186   {-# INLINE applyTransform #-}
  187   applyTransform q (Vec3 x y z) = Vec3 rx ry rz where
  188     DualQuat _rq (Quat (Vec4 rx ry rz _rw)) = q * a * dualConjugateConjugate q
  189     a = DualQuat (Quat (Vec4 0 0 0 1)) (Quat (Vec4 x y z 0))
  190   {-# INLINE combineTransform #-}
  191   combineTransform = (*)
  192   {-# INLINE transformToMatrix #-}
  193   transformToMatrix (DualQuat q p) = transformToMatrix $ QuatOffset q (Vec3 (x * 2) (y * 2) (z * 2)) where
  194     Quat (Vec4 x y z _w) = p * conjugate q
  195   {-# INLINE transformFromMatrix #-}
  196   transformFromMatrix (Mat4x4
  197     m11 m12 m13 m14
  198     m21 m22 m23 m24
  199     m31 m32 m33 m34
  200     _41 _42 _43 _44) = DualQuat q (Quat (Vec4 (m14 * 0.5) (m24 * 0.5) (m34 * 0.5) 0) * q) where
  201     k = sqrt (1 + m11 + m22 + m33)
  202     kk = 0.5 / k
  203     x = (m32 - m23) * kk
  204     y = (m13 - m31) * kk
  205     z = (m21 - m12) * kk
  206     w = k * 0.5
  207     q = normalize $ Quat $ Vec4 x y z w
  208   {-# INLINE transformTranslation #-}
  209   transformTranslation (Vec3 x y z) = DualQuat (Quat (Vec4 0 0 0 1)) (Quat (Vec4 (x * 0.5) (y * 0.5) (z * 0.5) 0))
  210   {-# INLINE transformAxisRotation #-}
  211   transformAxisRotation axis angle = DualQuat (quatAxisRotation axis angle) (Quat (Vec4 0 0 0 0))