DQuaternion.hs

{-# LANGUAGE BlockArguments #-}
{-# LANGUAGE ImportQualifiedPost #-}
{-# LANGUAGE OverloadedStrings #-}

module Spec.DQuaternion (tests) where

import Geomancy
import Hedgehog
import Test.Tasty (TestTree, testGroup)
import Test.Tasty.Hedgehog (testPropertyNamed)

import Hedgehog.Gen qualified as Gen
import Hedgehog.Range qualified as Range
import Linear (V3(..))
import Linear.Quaternion qualified

import Geomancy.DQuaternion qualified
import Geomancy.DTransform qualified as DTransform
import Geomancy.Quaternion qualified

import Support.Ball (balls, consistent, enclosed, exactly, narrow, widen)
import Support.Fixtures (intrinsicComposedD, intrinsicComposedRef, quarterTurn, rotationsMatch)
import Support.Gen (genAngle, genAngleD, genDVec3, genSmallDVec3, genVec3)
import Support.Linear (toQ, toQD, toV3D)
import Support.Reference.Linear qualified as Ref.Linear
import Support.Reference.Quaternion qualified as Ref.Quaternion
import Support.Reference.Transform qualified as Ref.Transform

tests :: TestTree
tests = testGroup "DQuaternion"
  [ testPropertyNamed "ref" "prop_dquaternion_ref" prop_dquaternion_ref
  , testPropertyNamed "rotate_matrix" "prop_dquaternion_rotate_matrix" prop_dquaternion_rotate_matrix
  , testPropertyNamed "associativity" "prop_dquaternion_associativity" prop_dquaternion_associativity
  , testPropertyNamed "intrinsic" "prop_dquaternion_intrinsic" prop_dquaternion_intrinsic
  , testPropertyNamed "extrinsic" "prop_dquaternion_extrinsic" prop_dquaternion_extrinsic
  , testPropertyNamed "intrinsic_ref" "prop_dquaternion_intrinsic_ref" prop_dquaternion_intrinsic_ref
  , testPropertyNamed "float_parity" "prop_dquaternion_float_parity" prop_dquaternion_float_parity
  ]

prop_dquaternion_ref :: Property
prop_dquaternion_ref = property do
  annotate "Construction"
  (axis, angle) <- forAll $ (,) <$> genDVec3 <*> genAngleD
  let
    qg = Geomancy.DQuaternion.axisAngle axis angle
    ql = Linear.Quaternion.axisAngle (toV3D axis) angle
    qgBall = Ref.Quaternion.axisAngle (balls (toV3D axis)) (exactly angle)
    qlBall = Linear.Quaternion.axisAngle (balls (toV3D axis)) (exactly angle)
  replicate 4 Nothing === enclosed qgBall (toQD qg)
  replicate 4 Nothing === enclosed qlBall ql
  replicate 4 Nothing === consistent qgBall qlBall

  annotate "Rotation"
  p <- forAll genSmallDVec3
  let
    rpg = Geomancy.DQuaternion.rotate qg p
    rpl = Linear.Quaternion.rotate ql (toV3D p)
    rpgBall = Ref.Quaternion.rotate qgBall (balls (toV3D p))
    rplBall = Ref.Linear.rotate qlBall (balls (toV3D p))
  replicate 3 Nothing === enclosed rpgBall (toV3D rpg)
  replicate 3 Nothing === enclosed rplBall rpl
  replicate 3 Nothing === consistent rpgBall rplBall

  annotate "Multiplication"
  (axisB, angleB) <- forAll $ (,) <$> genDVec3 <*> genAngleD
  let
    qgB = Geomancy.DQuaternion.axisAngle axisB angleB
    qlB = Linear.Quaternion.axisAngle (toV3D axisB) angleB
    productGBall =
      qgBall `Ref.Quaternion.multiply`
        Ref.Quaternion.axisAngle (balls (toV3D axisB)) (exactly angleB)
    productLBall =
      qlBall `Ref.Linear.multiply`
        Linear.Quaternion.axisAngle (balls (toV3D axisB)) (exactly angleB)
  replicate 4 Nothing === enclosed productGBall (toQD (qg * qgB))
  replicate 4 Nothing === enclosed productLBall (ql * qlB)
  replicate 4 Nothing === consistent productGBall productLBall

prop_dquaternion_rotate_matrix :: Property
prop_dquaternion_rotate_matrix = property do
  q <- forAll $ dquaternion <$> genUnitish <*> genUnitish <*> genUnitish <*> genUnitish
  p <- forAll genSmallDVec3
  let
    viaRotate = Geomancy.DQuaternion.rotate q p
    viaMatrix = DTransform.rotateQ q DTransform.!. p
    viaRotateBall = Ref.Quaternion.rotate (balls (toQD q)) (balls (toV3D p))
    viaMatrixBall = Ref.Transform.rotateQ (balls (toQD q)) Ref.Transform.!. balls (toV3D p)
  replicate 3 Nothing === enclosed viaRotateBall (toV3D viaRotate)
  replicate 3 Nothing === enclosed viaMatrixBall (toV3D viaMatrix)
  replicate 3 Nothing === consistent viaRotateBall viaMatrixBall
  where
    genUnitish = Gen.double (Range.linearFracFrom 0.0 (-1) 1)

prop_dquaternion_associativity :: Property
prop_dquaternion_associativity = property do
  (a, b, c) <- forAll $ (,,)
    <$> (Geomancy.DQuaternion.axisAngle <$> genDVec3 <*> genAngleD)
    <*> (Geomancy.DQuaternion.axisAngle <$> genDVec3 <*> genAngleD)
    <*> (Geomancy.DQuaternion.axisAngle <$> genDVec3 <*> genAngleD)
  let ab'c = (a * b) * c
  annotateShow ab'c
  let a'bc = a * (b * c)
  annotateShow a'bc
  let
    aBall = balls (toQD a)
    bBall = balls (toQD b)
    cBall = balls (toQD c)
    ab'cBall = (aBall `Ref.Quaternion.multiply` bBall) `Ref.Quaternion.multiply` cBall
    a'bcBall = aBall `Ref.Quaternion.multiply` (bBall `Ref.Quaternion.multiply` cBall)
  replicate 4 Nothing === enclosed ab'cBall (toQD ab'c)
  replicate 4 Nothing === enclosed a'bcBall (toQD a'bc)
  replicate 4 Nothing === consistent ab'cBall a'bcBall

prop_dquaternion_intrinsic :: Property
prop_dquaternion_intrinsic = withTests 1 $ property do
  let
    q = Geomancy.DQuaternion.intrinsic 0 quarterTurn quarterTurn
    qBall = Ref.Quaternion.intrinsic 0 quarterTurn quarterTurn
  rotationsMatch toV3D
    (Geomancy.DQuaternion.rotate q)
    (Ref.Quaternion.rotate qBall)
    [ (dvec3 0 0 1, dvec3 0 (-1) 0)
    , (dvec3 1 0 0, dvec3 0 0 (-1))
    , (dvec3 0 (-1) 0, dvec3 (-1) 0 0)
    ]

prop_dquaternion_extrinsic :: Property
prop_dquaternion_extrinsic = withTests 1 $ property do
  let
    q = Geomancy.DQuaternion.extrinsic quarterTurn quarterTurn 0
    qBall = Ref.Quaternion.extrinsic quarterTurn quarterTurn 0
  rotationsMatch toV3D
    (Geomancy.DQuaternion.rotate q)
    (Ref.Quaternion.rotate qBall)
    [ (dvec3 0 0 1, dvec3 1 0 0)
    , (dvec3 1 0 0, dvec3 0 1 0)
    , (dvec3 0 (-1) 0, dvec3 0 0 (-1))
    ]

prop_dquaternion_intrinsic_ref :: Property
prop_dquaternion_intrinsic_ref = property do
  (roll, pitch, yaw) <- forAll $ (,,) <$> genAngleD <*> genAngleD <*> genAngleD
  let fused = Geomancy.DQuaternion.intrinsic roll pitch yaw
  annotateShow fused
  let composed = intrinsicComposedD roll pitch yaw
  annotateShow composed
  let
    viaFused = Geomancy.DQuaternion.rotate fused (dvec3 0 0 1)
    viaComposed = Geomancy.DQuaternion.rotate composed (dvec3 0 0 1)
    viaFusedBall =
      Ref.Quaternion.rotate
        (Ref.Quaternion.intrinsic (exactly roll) (exactly pitch) (exactly yaw))
        (V3 0 0 1)
    viaComposedBall =
      Ref.Quaternion.rotate
        (intrinsicComposedRef (exactly roll) (exactly pitch) (exactly yaw))
        (V3 0 0 1)
  replicate 3 Nothing === enclosed viaFusedBall (toV3D viaFused)
  replicate 3 Nothing === enclosed viaComposedBall (toV3D viaComposed)
  replicate 3 Nothing === consistent viaFusedBall viaComposedBall

prop_dquaternion_float_parity :: Property
prop_dquaternion_float_parity = property do
  (a, b) <- forAll $ (,)
    <$> (Geomancy.Quaternion.axisAngle <$> genVec3 <*> genAngle)
    <*> (Geomancy.Quaternion.axisAngle <$> genVec3 <*> genAngle)
  a === Geomancy.DQuaternion.toQuaternion (Geomancy.DQuaternion.fromQuaternion a)
  let
    viaDouble =
      Geomancy.DQuaternion.toQuaternion $
        Geomancy.DQuaternion.fromQuaternion a * Geomancy.DQuaternion.fromQuaternion b
    aBall = balls (toQ a)
    bBall = balls (toQ b)
    viaDoubleBall =
      fmap narrow $
        fmap widen aBall `Ref.Quaternion.multiply` fmap widen bBall
    viaFloatBall = aBall `Ref.Quaternion.multiply` bBall
  replicate 4 Nothing === enclosed viaDoubleBall (toQ viaDouble)
  replicate 4 Nothing === enclosed viaFloatBall (toQ (a * b))
  replicate 4 Nothing === consistent viaDoubleBall viaFloatBall