Quat.hs

module Mischief.Math.Quat where

import Data.Default (Default (def))
import Linear (M33, Quaternion (Quaternion), V3 (V3), axisAngle, fromQuaternion)
import Mischief.Math.Vec (Dir3 (Dir3))
import Mischief.Math.Vec.Orphans.FieldAccessors ()

newtype Quat = Quat (Quaternion Float) deriving newtype (Show, Eq, Ord, Num)

instance Default Quat where
  def = identity

identity :: Quat
identity = Quat $ Quaternion 1 $ V3 0 0 0

fromAxisAngle :: Dir3 Float -> Float -> Quat
fromAxisAngle (Dir3 dir) angle = Quat $ axisAngle dir angle

fromXYZW :: (Float, Float, Float, Float) -> Quat
fromXYZW (x, y, z, w) = Quat $ Quaternion w (V3 x y z)

fromRotationX :: Float -> Quat
fromRotationX angle =
  let (s, c) = (sin angle, cos angle)
   in fromXYZW (s, 0, 0, c)

fromRotationY :: Float -> Quat
fromRotationY angle =
  let (s, c) = (sin angle, cos angle)
   in fromXYZW (0, s, 0, c)

fromRotationZ :: Float -> Quat
fromRotationZ angle =
  let (s, c) = (sin angle, cos angle)
   in fromXYZW (0, 0, s, c)

fromAngles :: V3 Float -> Quat
fromAngles vec3 = rotateX vec3.x $ rotateY vec3.y $ rotateZ vec3.z identity

fromEuler :: Euler -> Quat
fromEuler Euler {x, y, z} = fromAngles $ V3 x y z

rotateAxis :: Dir3 Float -> Float -> Quat -> Quat
rotateAxis dir angle quat = quat * fromAxisAngle dir angle

rotateX :: Float -> Quat -> Quat
rotateX angle quat = quat * fromRotationX angle

rotateY :: Float -> Quat -> Quat
rotateY angle quat = quat * fromRotationY angle

rotateZ :: Float -> Quat -> Quat
rotateZ angle quat = quat * fromRotationZ angle

toMat3 :: Quat -> M33 Float
toMat3 (Quat quat) = fromQuaternion quat

toEuler :: Quat -> Euler
toEuler = m33ToEuler . toMat3

data Euler = Euler {x :: Float, y :: Float, z :: Float} deriving (Show, Eq)

m33ToEuler :: M33 Float -> Euler
m33ToEuler (V3 (V3 m11 m12 m13) (V3 _ _ m23) (V3 _ _ m33)) =
  Euler {x = atan2 (-m23) m33 * 0.5, y = asin m13 * 0.5, z = atan2 (-m12) m11 * 0.5}