85 lines
2.2 KiB
Haskell
85 lines
2.2 KiB
Haskell
{-# LANGUAGE ScopedTypeVariables #-}
|
|
{-# OPTIONS_GHC -fno-warn-orphans #-}
|
|
|
|
{- |
|
|
WARNING: orphan instances concerning Aeson classes and Linear.Quaternion datatypes have been introduced.
|
|
The warnings have been disabled.
|
|
-}
|
|
module Quaternion (
|
|
qid,
|
|
qz,
|
|
qToV3,
|
|
qToV2,
|
|
qToAng,
|
|
rotateToZ,
|
|
vToQuat,
|
|
comp,
|
|
apply,
|
|
module Linear.Quaternion,
|
|
qNoRoll,
|
|
) where
|
|
|
|
import Geometry.Vector
|
|
import Data.Aeson
|
|
import Geometry.Data
|
|
import Geometry.Vector3D
|
|
import Linear.Quaternion
|
|
import qualified Linear.Quaternion as Q
|
|
import Control.Lens
|
|
import Linear
|
|
|
|
instance ToJSON a => ToJSON (Q.Quaternion a) where
|
|
toEncoding = genericToEncoding defaultOptions
|
|
|
|
instance FromJSON a => FromJSON (Q.Quaternion a)
|
|
|
|
-- apply a rotation as if the z axis moves to the new point.
|
|
-- i think this may instead do as if the new point moves to be on z axis
|
|
rotateToZ :: Point3 -> Point3 -> Point3
|
|
rotateToZ z1
|
|
| cprod == V3 0 0 0 = id
|
|
| otherwise = Q.rotate $ Q.axisAngle cprod (angleVV3 z1 (V3 0 0 1))
|
|
where
|
|
cprod = crossProd z1 (V3 0 0 1)
|
|
|
|
vToQuat :: Point3 -> Point3 -> Q.Quaternion Float
|
|
vToQuat a b
|
|
| cprod == V3 0 0 0 = Q.axisAngle (V3 0 0 1) 0
|
|
| otherwise = Q.axisAngle cprod (angleVV3 a b)
|
|
where
|
|
cprod = crossProd a b
|
|
|
|
qNoRoll :: Point3 -> Q.Quaternion Float
|
|
--qNoRoll x = qz (argV $ x ^. _xy) * Q.axisAngle (V3 0 (-1) 0) (argV (V2 (x ^. _z) (norm (x ^. _xy))))
|
|
qNoRoll x = qNoRoll2 x * qNoRoll1 x
|
|
--qNoRoll x = Q.axisAngle (V3 0 1 0) (argV (V2 (x ^. _z) (norm (x ^. _xy)))) * qz (argV $ x ^. _xy)
|
|
--
|
|
qNoRoll1 :: Point3 -> Q.Quaternion Float
|
|
qNoRoll1 x = Q.axisAngle (V3 0 (-1) 0) . argV $ V2 (norm (x ^. _xy)) (x ^. _z)
|
|
|
|
qNoRoll2 :: Point3 -> Q.Quaternion Float
|
|
qNoRoll2 x = qz (argV $ x ^. _xy)
|
|
|
|
qToV3 :: Q.Quaternion Float -> Point3
|
|
qToV3 q = Q.rotate q (V3 1 0 0)
|
|
|
|
qToV2 :: Q.Quaternion Float -> Point2
|
|
qToV2 = (\(V3 x y _) -> V2 x y) . qToV3
|
|
|
|
qToAng :: Quaternion Float -> Float
|
|
qToAng = argV . qToV2
|
|
|
|
qid :: Q.Quaternion Float
|
|
qid = Q.axisAngle (V3 1 0 0) 0
|
|
|
|
comp :: Point3Q -> Point3Q -> Point3Q
|
|
comp (p,q) (p1,q1) = (p + Q.rotate q p1, q * q1)
|
|
|
|
apply :: Point3Q -> Point3 -> Point3
|
|
apply (p,q) p1 = p + Q.rotate q p1
|
|
|
|
qz :: Float -> Q.Quaternion Float
|
|
qz = Q.axisAngle (V3 0 0 1)
|
|
|
|
--deriving instance (Flat a => Flat (Quaternion a))
|