diff --git a/Data/Vect/Double/Interpolate.hs b/Data/Vect/Double/Interpolate.hs --- a/Data/Vect/Double/Interpolate.hs +++ b/Data/Vect/Double/Interpolate.hs @@ -2,25 +2,36 @@ #define Flt Double #define VECT_Double +-- TODO: interpolation for Ortho3 matrices using the (short) quaternion 'slerpU' + -- | Interpolation of vectors. -- Note: we interpolate unit vectors differently from ordinary vectors. module Data.Vect.Flt.Interpolate where +-------------------------------------------------------------------------------- + import Data.Vect.Flt.Base import Data.Vect.Flt.Util.Dim2 (sinCos',angle2') import Data.Vect.Flt.Util.Dim3 (rotate3') +-------------------------------------------------------------------------------- + class Interpolate v where interpolate :: Flt -> v -> v -> v instance Interpolate Flt where interpolate t x y = x + t*(y-x) +-------------------------------------------------------------------------------- + instance Interpolate Vec2 where interpolate t x y = x &+ t *& (y &- x) instance Interpolate Vec3 where interpolate t x y = x &+ t *& (y &- x) instance Interpolate Vec4 where interpolate t x y = x &+ t *& (y &- x) +-------------------------------------------------------------------------------- + +{- instance Interpolate Normal2 where interpolate t nx ny = sinCos' $ ax + t*adiff where ax = angle2' nx @@ -41,5 +52,30 @@ y = fromNormal ny axis = (x &^ y) maxAngle = acos (x &. y) +-} + +instance Interpolate Normal2 where interpolate = slerp +instance Interpolate Normal3 where interpolate = slerp +instance Interpolate Normal4 where interpolate = slerp +-------------------------------------------------------------------------------- +{-# SPECIALIZE slerp :: Flt -> Normal2 -> Normal2 -> Normal2 #-} +{-# SPECIALIZE slerp :: Flt -> Normal3 -> Normal3 -> Normal3 #-} +{-# SPECIALIZE slerp :: Flt -> Normal4 -> Normal4 -> Normal4 #-} + +-- | Spherical linear interpolation. +-- See +slerp :: (Interpolate v, UnitVector v u) => Flt -> u -> u -> u +slerp t n0 n1 = toNormalUnsafe v where + v = (p0 &* y0) &+ (p1 &* y1) + p0 = fromNormal n0 + p1 = fromNormal n1 + omega = acos (p0 &. p1) + s = sin omega + y0 = sin (omega*(1-t)) / s + y1 = sin (omega* t ) / s + +-------------------------------------------------------------------------------- + + diff --git a/Data/Vect/Double/Util/Quaternion.hs b/Data/Vect/Double/Util/Quaternion.hs new file mode 100644 --- /dev/null +++ b/Data/Vect/Double/Util/Quaternion.hs @@ -0,0 +1,263 @@ +{-# LANGUAGE CPP #-} +#define Flt Double +#define VECT_Double + +-- | The unit sphere in the space of quaternions has the group structure +-- SU(2) coming from the quaternion multiplication, which is the double +-- cover of the group SO(3) of rotations in R^3. Thus, unit quaternions can +-- be used to encode rotations in 3D, which is a more compact encoding +-- (4 floats) than a 3x3 matrix; however, there are /two/ quaternions +-- corresponding to each rotation. +-- +-- See and +-- +-- for more information. + +{-# LANGUAGE MultiParamTypeClasses, GeneralizedNewtypeDeriving #-} +module Data.Vect.Flt.Util.Quaternion where + +-------------------------------------------------------------------------------- + +import Data.Vect.Flt.Base +import Data.Vect.Flt.Interpolate + +import Foreign.Storable +import System.Random + +-------------------------------------------------------------------------------- +-- * types + +-- | The type for quaternions. +newtype Quaternion = Q Vec4 + deriving (Read,Show,Storable,AbelianGroup,Vector,DotProd,Random,Interpolate) + +-- | The type for unit quaternions. +newtype UnitQuaternion = U Vec4 + deriving (Read,Show,Storable,DotProd) + +-- | An abbreviated type synonym for quaternions +type Q = Quaternion + +-- | An abbreviated type synonym for unit quaternions +type U = UnitQuaternion + +-------------------------------------------------------------------------------- + +instance UnitVector Quaternion UnitQuaternion where + mkNormal (Q v) = U (normalize v) + toNormalUnsafe (Q v) = U v + fromNormal (U v) = Q v + fromNormalRadius r (U v) = Q (v &* r) + +-------------------------------------------------------------------------------- +-- * general quaternions + +unitQ :: Q +unitQ = Q (Vec4 1 0 0 0) + +zeroQ :: Q +zeroQ = Q (Vec4 0 0 0 0) + +multQ :: Q -> Q -> Q +multQ (Q (Vec4 a1 b1 c1 d1)) (Q (Vec4 a2 b2 c2 d2)) = Q $ Vec4 + (a1*a2 - b1*b2 - c1*c2 - d1*d2) + (a1*b2 + b1*a2 + c1*d2 - d1*c2) + (a1*c2 - b1*d2 + c1*a2 + d1*b2) + (a1*d2 + b1*c2 - c1*b2 + d1*a2) + +negQ :: Q -> Q +negQ (Q v) = Q (neg v) + +normalizeQ :: Q -> Q +normalizeQ (Q v) = Q (normalize v) + +-- | The inverse quaternion +invQ :: Q -> Q +invQ (Q (Vec4 a b c d)) = Q (v &* (1 / normsqr v)) where + v = Vec4 a (-b) (-c) (-d) + +fromQ :: Q -> Vec4 +fromQ (Q v) = v + +toQ :: Vec4 -> Q +toQ = Q + +-------------------------------------------------------------------------------- + +{- +-- we use newtype deriving instead + +instance AbelianGroup Quaternion where + (Q v1) &+ (Q v2) = Q (v1 &+ v2) + (Q v1) &- (Q v2) = Q (v1 &+ v2) + neg (Q v) = Q (neg v) + zero = zeroQ + +instance DotProd Quaternion where + dotprod (Q v1) (Q v2) = dotprod v1 v2 + +-} + +instance MultSemiGroup Quaternion where + one = unitQ + (.*.) = multQ + + +-------------------------------------------------------------------------------- +-- * unit quaternions + +unitU :: U +unitU = U (Vec4 1 0 0 0) + +multU :: U -> U -> U +multU (U (Vec4 a1 b1 c1 d1)) (U (Vec4 a2 b2 c2 d2)) = U $ Vec4 + (a1*a2 - b1*b2 - c1*c2 - d1*d2) + (a1*b2 + b1*a2 + c1*d2 - d1*c2) + (a1*c2 - b1*d2 + c1*a2 + d1*b2) + (a1*d2 + b1*c2 - c1*b2 + d1*a2) + +-- | The opposite quaternion (which encodes the same rotation) +negU :: U -> U +negU (U v) = U (neg v) + +-- | This is no-op, up to numerical imprecision. +-- However, if you multiply together a large number of unit quaternions, +-- it may be a good idea to normalize the end result. +normalizeU :: U -> U +normalizeU (U v) = U (normalize v) + +-- | The inverse of a unit quaternion +invU :: U -> U +invU (U (Vec4 a b c d)) = U $ Vec4 a (-b) (-c) (-d) + +-------------------------------------------------------------------------------- + +fromU :: U -> Vec4 +fromU (U v) = v + +fromU' :: U -> Normal4 +fromU' (U v) = toNormalUnsafe v + +mkU :: Vec4 -> U +mkU = U . normalize + +toU :: Normal4 -> U +toU = U . fromNormal + +unsafeToU :: Vec4 -> U +unsafeToU = U + +-------------------------------------------------------------------------------- + +{- +-- we use newtype deriving instead + +instance DotProd UnitQuaternion where + dotprod (Q v1) (Q v2) = dotprod v1 v2 + +-} + +instance MultSemiGroup UnitQuaternion where + one = unitU + (.*.) = multU + +instance LeftModule UnitQuaternion Vec3 where + lmul u v = actU u v + +instance Random UnitQuaternion where + random g = let (n, h) = random g + v = fromNormal n :: Vec4 + in (U v, h) + randomR _ = random + +-------------------------------------------------------------------------------- +-- * unit quaternions as rotations + +-- | The /left/ action of unit quaternions on 3D vectors. +-- That is, +-- +-- > actU q1 $ actU q2 v == actU (q1 `multU` q2) v +actU :: U -> Vec3 -> Vec3 +actU (U (Vec4 a b c d)) (Vec3 x y z) = Vec3 x' y' z' where + x' = x*(aa + bb - cc - dd) + y*( 2 * (bc - ad) ) + z*( 2 * (bd + ac) ) + y' = x*( 2 * (bc + ad) ) + y*(aa - bb + cc - dd) + z*( 2 * (cd - ab) ) + z' = x*( 2 * (bd - ac) ) + y*( 2 * (cd + ab) ) + z*(aa - bb - cc + dd) + -- + aa = a*a ; bb = b*b ; cc = c*c ; dd = d*d + ab = a*b ; ac = a*c ; ad = a*d + bc = b*c ; bd = b*d ; cd = c*d + +-- | The quaternion to encode rotation around an axis. Please note +-- that quaternions act on the /left/, that is +-- +-- > rotU axis1 angl1 *. rotU axis2 angl2 *. v == (rotU axis1 angl1 .*. rotU axis2 angl2) *. v +-- +rotU :: Vec3 -> Flt -> U +rotU axis angle = rotU' (mkNormal axis) angle + +rotU' {- ' CPP is sensitive to primes -} :: Normal3 -> Flt -> U +rotU' axis angle = U (Vec4 c (x*s) (y*s) (z*s)) where + Vec3 x y z = fromNormal axis + half = 0.5 * angle + c = cos half + s = sin half + +-- | Interpolation of unit quaternions. Note that when applied to rotations, +-- this may be not what you want, since it is possible that the shortest path +-- in the space of unit quaternions is not the shortest path in the space of +-- rotations; see 'slerpU'! +longSlerpU :: Flt -> U -> U -> U +longSlerpU t (U p0) (U p1) = U v where + v = (p0 &* y0) &+ (p1 &* y1) + omega = acos (p0 &. p1) + s = sin omega + y0 = sin (omega*(1-t)) / s + y1 = sin (omega* t ) / s + +-- | This is shortest path interpolation in the space of rotations; however +-- this is achieved by possibly flipping the first endpoint in the space of +-- quaternions. Thus @slerpU 0.001 q1 q2@ may be very far from @q1@ (and very +-- close to @negU q1@) in the space of quaternions (but they are very close +-- in the space of rotations). +slerpU :: Flt -> U -> U -> U +slerpU t (U p0') (U p1) = U v where + v = (p0 &* y0) &+ (p1 &* y1) + + d' = p0' &. p1 + (d,p0) = if d' >= 0 + then ( d', p0') + else (-d', neg p0') + + omega = acos d + s = sin omega + y0 = sin (omega*(1-t)) / s + y1 = sin (omega* t ) / s + +-- | Makes a rotation matrix (to be multiplied with on the /right/) out of a unit quaternion: +-- +-- > v .* rightOrthoU (rotU axis angl) == v .* rotMatrix3 axis angl +-- +-- Please note that while these matrices act on the /right/, quaternions act on the /left/; thus +-- +-- > rightOrthoU q1 .*. rightOrthoU q2 == rightOrthoU (q2 .*. q1) +-- +rightOrthoU :: U -> Ortho3 +rightOrthoU = toOrthoUnsafe . transpose . fromOrtho . leftOrthoU + +-- | Makes a rotation matrix (to be multiplied with on the /left/) out of a unit quaternion. +-- +-- > leftOrthoU (rotU axis angl) *. v == v .* rotMatrix3 axis angl +-- +leftOrthoU :: U -> Ortho3 +leftOrthoU (U (Vec4 a b c d)) = toOrthoUnsafe $ Mat3 row1 row2 row3 where + row1 = Vec3 (aa + bb - cc - dd) ( 2 * (bc - ad) ) ( 2 * (bd + ac) ) + row2 = Vec3 ( 2 * (bc + ad) ) (aa - bb + cc - dd) ( 2 * (cd - ab) ) + row3 = Vec3 ( 2 * (bd - ac) ) ( 2 * (cd + ab) ) (aa - bb - cc + dd) + -- + aa = a*a ; bb = b*b ; cc = c*c ; dd = d*d + ab = a*b ; ac = a*c ; ad = a*d + bc = b*c ; bd = b*d ; cd = c*d + +-------------------------------------------------------------------------------- + + diff --git a/Data/Vect/Float/Interpolate.hs b/Data/Vect/Float/Interpolate.hs --- a/Data/Vect/Float/Interpolate.hs +++ b/Data/Vect/Float/Interpolate.hs @@ -2,25 +2,36 @@ #define Flt Float #define VECT_Float +-- TODO: interpolation for Ortho3 matrices using the (short) quaternion 'slerpU' + -- | Interpolation of vectors. -- Note: we interpolate unit vectors differently from ordinary vectors. module Data.Vect.Flt.Interpolate where +-------------------------------------------------------------------------------- + import Data.Vect.Flt.Base import Data.Vect.Flt.Util.Dim2 (sinCos',angle2') import Data.Vect.Flt.Util.Dim3 (rotate3') +-------------------------------------------------------------------------------- + class Interpolate v where interpolate :: Flt -> v -> v -> v instance Interpolate Flt where interpolate t x y = x + t*(y-x) +-------------------------------------------------------------------------------- + instance Interpolate Vec2 where interpolate t x y = x &+ t *& (y &- x) instance Interpolate Vec3 where interpolate t x y = x &+ t *& (y &- x) instance Interpolate Vec4 where interpolate t x y = x &+ t *& (y &- x) +-------------------------------------------------------------------------------- + +{- instance Interpolate Normal2 where interpolate t nx ny = sinCos' $ ax + t*adiff where ax = angle2' nx @@ -41,5 +52,30 @@ y = fromNormal ny axis = (x &^ y) maxAngle = acos (x &. y) +-} + +instance Interpolate Normal2 where interpolate = slerp +instance Interpolate Normal3 where interpolate = slerp +instance Interpolate Normal4 where interpolate = slerp +-------------------------------------------------------------------------------- +{-# SPECIALIZE slerp :: Flt -> Normal2 -> Normal2 -> Normal2 #-} +{-# SPECIALIZE slerp :: Flt -> Normal3 -> Normal3 -> Normal3 #-} +{-# SPECIALIZE slerp :: Flt -> Normal4 -> Normal4 -> Normal4 #-} + +-- | Spherical linear interpolation. +-- See +slerp :: (Interpolate v, UnitVector v u) => Flt -> u -> u -> u +slerp t n0 n1 = toNormalUnsafe v where + v = (p0 &* y0) &+ (p1 &* y1) + p0 = fromNormal n0 + p1 = fromNormal n1 + omega = acos (p0 &. p1) + s = sin omega + y0 = sin (omega*(1-t)) / s + y1 = sin (omega* t ) / s + +-------------------------------------------------------------------------------- + + diff --git a/Data/Vect/Float/Util/Quaternion.hs b/Data/Vect/Float/Util/Quaternion.hs new file mode 100644 --- /dev/null +++ b/Data/Vect/Float/Util/Quaternion.hs @@ -0,0 +1,263 @@ +{-# LANGUAGE CPP #-} +#define Flt Float +#define VECT_Float + +-- | The unit sphere in the space of quaternions has the group structure +-- SU(2) coming from the quaternion multiplication, which is the double +-- cover of the group SO(3) of rotations in R^3. Thus, unit quaternions can +-- be used to encode rotations in 3D, which is a more compact encoding +-- (4 floats) than a 3x3 matrix; however, there are /two/ quaternions +-- corresponding to each rotation. +-- +-- See and +-- +-- for more information. + +{-# LANGUAGE MultiParamTypeClasses, GeneralizedNewtypeDeriving #-} +module Data.Vect.Flt.Util.Quaternion where + +-------------------------------------------------------------------------------- + +import Data.Vect.Flt.Base +import Data.Vect.Flt.Interpolate + +import Foreign.Storable +import System.Random + +-------------------------------------------------------------------------------- +-- * types + +-- | The type for quaternions. +newtype Quaternion = Q Vec4 + deriving (Read,Show,Storable,AbelianGroup,Vector,DotProd,Random,Interpolate) + +-- | The type for unit quaternions. +newtype UnitQuaternion = U Vec4 + deriving (Read,Show,Storable,DotProd) + +-- | An abbreviated type synonym for quaternions +type Q = Quaternion + +-- | An abbreviated type synonym for unit quaternions +type U = UnitQuaternion + +-------------------------------------------------------------------------------- + +instance UnitVector Quaternion UnitQuaternion where + mkNormal (Q v) = U (normalize v) + toNormalUnsafe (Q v) = U v + fromNormal (U v) = Q v + fromNormalRadius r (U v) = Q (v &* r) + +-------------------------------------------------------------------------------- +-- * general quaternions + +unitQ :: Q +unitQ = Q (Vec4 1 0 0 0) + +zeroQ :: Q +zeroQ = Q (Vec4 0 0 0 0) + +multQ :: Q -> Q -> Q +multQ (Q (Vec4 a1 b1 c1 d1)) (Q (Vec4 a2 b2 c2 d2)) = Q $ Vec4 + (a1*a2 - b1*b2 - c1*c2 - d1*d2) + (a1*b2 + b1*a2 + c1*d2 - d1*c2) + (a1*c2 - b1*d2 + c1*a2 + d1*b2) + (a1*d2 + b1*c2 - c1*b2 + d1*a2) + +negQ :: Q -> Q +negQ (Q v) = Q (neg v) + +normalizeQ :: Q -> Q +normalizeQ (Q v) = Q (normalize v) + +-- | The inverse quaternion +invQ :: Q -> Q +invQ (Q (Vec4 a b c d)) = Q (v &* (1 / normsqr v)) where + v = Vec4 a (-b) (-c) (-d) + +fromQ :: Q -> Vec4 +fromQ (Q v) = v + +toQ :: Vec4 -> Q +toQ = Q + +-------------------------------------------------------------------------------- + +{- +-- we use newtype deriving instead + +instance AbelianGroup Quaternion where + (Q v1) &+ (Q v2) = Q (v1 &+ v2) + (Q v1) &- (Q v2) = Q (v1 &+ v2) + neg (Q v) = Q (neg v) + zero = zeroQ + +instance DotProd Quaternion where + dotprod (Q v1) (Q v2) = dotprod v1 v2 + +-} + +instance MultSemiGroup Quaternion where + one = unitQ + (.*.) = multQ + + +-------------------------------------------------------------------------------- +-- * unit quaternions + +unitU :: U +unitU = U (Vec4 1 0 0 0) + +multU :: U -> U -> U +multU (U (Vec4 a1 b1 c1 d1)) (U (Vec4 a2 b2 c2 d2)) = U $ Vec4 + (a1*a2 - b1*b2 - c1*c2 - d1*d2) + (a1*b2 + b1*a2 + c1*d2 - d1*c2) + (a1*c2 - b1*d2 + c1*a2 + d1*b2) + (a1*d2 + b1*c2 - c1*b2 + d1*a2) + +-- | The opposite quaternion (which encodes the same rotation) +negU :: U -> U +negU (U v) = U (neg v) + +-- | This is no-op, up to numerical imprecision. +-- However, if you multiply together a large number of unit quaternions, +-- it may be a good idea to normalize the end result. +normalizeU :: U -> U +normalizeU (U v) = U (normalize v) + +-- | The inverse of a unit quaternion +invU :: U -> U +invU (U (Vec4 a b c d)) = U $ Vec4 a (-b) (-c) (-d) + +-------------------------------------------------------------------------------- + +fromU :: U -> Vec4 +fromU (U v) = v + +fromU' :: U -> Normal4 +fromU' (U v) = toNormalUnsafe v + +mkU :: Vec4 -> U +mkU = U . normalize + +toU :: Normal4 -> U +toU = U . fromNormal + +unsafeToU :: Vec4 -> U +unsafeToU = U + +-------------------------------------------------------------------------------- + +{- +-- we use newtype deriving instead + +instance DotProd UnitQuaternion where + dotprod (Q v1) (Q v2) = dotprod v1 v2 + +-} + +instance MultSemiGroup UnitQuaternion where + one = unitU + (.*.) = multU + +instance LeftModule UnitQuaternion Vec3 where + lmul u v = actU u v + +instance Random UnitQuaternion where + random g = let (n, h) = random g + v = fromNormal n :: Vec4 + in (U v, h) + randomR _ = random + +-------------------------------------------------------------------------------- +-- * unit quaternions as rotations + +-- | The /left/ action of unit quaternions on 3D vectors. +-- That is, +-- +-- > actU q1 $ actU q2 v == actU (q1 `multU` q2) v +actU :: U -> Vec3 -> Vec3 +actU (U (Vec4 a b c d)) (Vec3 x y z) = Vec3 x' y' z' where + x' = x*(aa + bb - cc - dd) + y*( 2 * (bc - ad) ) + z*( 2 * (bd + ac) ) + y' = x*( 2 * (bc + ad) ) + y*(aa - bb + cc - dd) + z*( 2 * (cd - ab) ) + z' = x*( 2 * (bd - ac) ) + y*( 2 * (cd + ab) ) + z*(aa - bb - cc + dd) + -- + aa = a*a ; bb = b*b ; cc = c*c ; dd = d*d + ab = a*b ; ac = a*c ; ad = a*d + bc = b*c ; bd = b*d ; cd = c*d + +-- | The quaternion to encode rotation around an axis. Please note +-- that quaternions act on the /left/, that is +-- +-- > rotU axis1 angl1 *. rotU axis2 angl2 *. v == (rotU axis1 angl1 .*. rotU axis2 angl2) *. v +-- +rotU :: Vec3 -> Flt -> U +rotU axis angle = rotU' (mkNormal axis) angle + +rotU' {- ' CPP is sensitive to primes -} :: Normal3 -> Flt -> U +rotU' axis angle = U (Vec4 c (x*s) (y*s) (z*s)) where + Vec3 x y z = fromNormal axis + half = 0.5 * angle + c = cos half + s = sin half + +-- | Interpolation of unit quaternions. Note that when applied to rotations, +-- this may be not what you want, since it is possible that the shortest path +-- in the space of unit quaternions is not the shortest path in the space of +-- rotations; see 'slerpU'! +longSlerpU :: Flt -> U -> U -> U +longSlerpU t (U p0) (U p1) = U v where + v = (p0 &* y0) &+ (p1 &* y1) + omega = acos (p0 &. p1) + s = sin omega + y0 = sin (omega*(1-t)) / s + y1 = sin (omega* t ) / s + +-- | This is shortest path interpolation in the space of rotations; however +-- this is achieved by possibly flipping the first endpoint in the space of +-- quaternions. Thus @slerpU 0.001 q1 q2@ may be very far from @q1@ (and very +-- close to @negU q1@) in the space of quaternions (but they are very close +-- in the space of rotations). +slerpU :: Flt -> U -> U -> U +slerpU t (U p0') (U p1) = U v where + v = (p0 &* y0) &+ (p1 &* y1) + + d' = p0' &. p1 + (d,p0) = if d' >= 0 + then ( d', p0') + else (-d', neg p0') + + omega = acos d + s = sin omega + y0 = sin (omega*(1-t)) / s + y1 = sin (omega* t ) / s + +-- | Makes a rotation matrix (to be multiplied with on the /right/) out of a unit quaternion: +-- +-- > v .* rightOrthoU (rotU axis angl) == v .* rotMatrix3 axis angl +-- +-- Please note that while these matrices act on the /right/, quaternions act on the /left/; thus +-- +-- > rightOrthoU q1 .*. rightOrthoU q2 == rightOrthoU (q2 .*. q1) +-- +rightOrthoU :: U -> Ortho3 +rightOrthoU = toOrthoUnsafe . transpose . fromOrtho . leftOrthoU + +-- | Makes a rotation matrix (to be multiplied with on the /left/) out of a unit quaternion. +-- +-- > leftOrthoU (rotU axis angl) *. v == v .* rotMatrix3 axis angl +-- +leftOrthoU :: U -> Ortho3 +leftOrthoU (U (Vec4 a b c d)) = toOrthoUnsafe $ Mat3 row1 row2 row3 where + row1 = Vec3 (aa + bb - cc - dd) ( 2 * (bc - ad) ) ( 2 * (bd + ac) ) + row2 = Vec3 ( 2 * (bc + ad) ) (aa - bb + cc - dd) ( 2 * (cd - ab) ) + row3 = Vec3 ( 2 * (bd - ac) ) ( 2 * (cd + ab) ) (aa - bb - cc + dd) + -- + aa = a*a ; bb = b*b ; cc = c*c ; dd = d*d + ab = a*b ; ac = a*c ; ad = a*d + bc = b*c ; bd = b*d ; cd = c*d + +-------------------------------------------------------------------------------- + + diff --git a/src/flt/Interpolate.hs b/src/flt/Interpolate.hs --- a/src/flt/Interpolate.hs +++ b/src/flt/Interpolate.hs @@ -1,23 +1,34 @@ +-- TODO: interpolation for Ortho3 matrices using the (short) quaternion 'slerpU' + -- | Interpolation of vectors. -- Note: we interpolate unit vectors differently from ordinary vectors. module Data.Vect.Flt.Interpolate where +-------------------------------------------------------------------------------- + import Data.Vect.Flt.Base import Data.Vect.Flt.Util.Dim2 (sinCos',angle2') import Data.Vect.Flt.Util.Dim3 (rotate3') +-------------------------------------------------------------------------------- + class Interpolate v where interpolate :: Flt -> v -> v -> v instance Interpolate Flt where interpolate t x y = x + t*(y-x) +-------------------------------------------------------------------------------- + instance Interpolate Vec2 where interpolate t x y = x &+ t *& (y &- x) instance Interpolate Vec3 where interpolate t x y = x &+ t *& (y &- x) instance Interpolate Vec4 where interpolate t x y = x &+ t *& (y &- x) +-------------------------------------------------------------------------------- + +{- instance Interpolate Normal2 where interpolate t nx ny = sinCos' $ ax + t*adiff where ax = angle2' nx @@ -38,5 +49,30 @@ y = fromNormal ny axis = (x &^ y) maxAngle = acos (x &. y) +-} + +instance Interpolate Normal2 where interpolate = slerp +instance Interpolate Normal3 where interpolate = slerp +instance Interpolate Normal4 where interpolate = slerp +-------------------------------------------------------------------------------- +{-# SPECIALIZE slerp :: Flt -> Normal2 -> Normal2 -> Normal2 #-} +{-# SPECIALIZE slerp :: Flt -> Normal3 -> Normal3 -> Normal3 #-} +{-# SPECIALIZE slerp :: Flt -> Normal4 -> Normal4 -> Normal4 #-} + +-- | Spherical linear interpolation. +-- See +slerp :: (Interpolate v, UnitVector v u) => Flt -> u -> u -> u +slerp t n0 n1 = toNormalUnsafe v where + v = (p0 &* y0) &+ (p1 &* y1) + p0 = fromNormal n0 + p1 = fromNormal n1 + omega = acos (p0 &. p1) + s = sin omega + y0 = sin (omega*(1-t)) / s + y1 = sin (omega* t ) / s + +-------------------------------------------------------------------------------- + + diff --git a/src/flt/Util/Quaternion.hs b/src/flt/Util/Quaternion.hs new file mode 100644 --- /dev/null +++ b/src/flt/Util/Quaternion.hs @@ -0,0 +1,260 @@ + +-- | The unit sphere in the space of quaternions has the group structure +-- SU(2) coming from the quaternion multiplication, which is the double +-- cover of the group SO(3) of rotations in R^3. Thus, unit quaternions can +-- be used to encode rotations in 3D, which is a more compact encoding +-- (4 floats) than a 3x3 matrix; however, there are /two/ quaternions +-- corresponding to each rotation. +-- +-- See and +-- +-- for more information. + +{-# LANGUAGE MultiParamTypeClasses, GeneralizedNewtypeDeriving #-} +module Data.Vect.Flt.Util.Quaternion where + +-------------------------------------------------------------------------------- + +import Data.Vect.Flt.Base +import Data.Vect.Flt.Interpolate + +import Foreign.Storable +import System.Random + +-------------------------------------------------------------------------------- +-- * types + +-- | The type for quaternions. +newtype Quaternion = Q Vec4 + deriving (Read,Show,Storable,AbelianGroup,Vector,DotProd,Random,Interpolate) + +-- | The type for unit quaternions. +newtype UnitQuaternion = U Vec4 + deriving (Read,Show,Storable,DotProd) + +-- | An abbreviated type synonym for quaternions +type Q = Quaternion + +-- | An abbreviated type synonym for unit quaternions +type U = UnitQuaternion + +-------------------------------------------------------------------------------- + +instance UnitVector Quaternion UnitQuaternion where + mkNormal (Q v) = U (normalize v) + toNormalUnsafe (Q v) = U v + fromNormal (U v) = Q v + fromNormalRadius r (U v) = Q (v &* r) + +-------------------------------------------------------------------------------- +-- * general quaternions + +unitQ :: Q +unitQ = Q (Vec4 1 0 0 0) + +zeroQ :: Q +zeroQ = Q (Vec4 0 0 0 0) + +multQ :: Q -> Q -> Q +multQ (Q (Vec4 a1 b1 c1 d1)) (Q (Vec4 a2 b2 c2 d2)) = Q $ Vec4 + (a1*a2 - b1*b2 - c1*c2 - d1*d2) + (a1*b2 + b1*a2 + c1*d2 - d1*c2) + (a1*c2 - b1*d2 + c1*a2 + d1*b2) + (a1*d2 + b1*c2 - c1*b2 + d1*a2) + +negQ :: Q -> Q +negQ (Q v) = Q (neg v) + +normalizeQ :: Q -> Q +normalizeQ (Q v) = Q (normalize v) + +-- | The inverse quaternion +invQ :: Q -> Q +invQ (Q (Vec4 a b c d)) = Q (v &* (1 / normsqr v)) where + v = Vec4 a (-b) (-c) (-d) + +fromQ :: Q -> Vec4 +fromQ (Q v) = v + +toQ :: Vec4 -> Q +toQ = Q + +-------------------------------------------------------------------------------- + +{- +-- we use newtype deriving instead + +instance AbelianGroup Quaternion where + (Q v1) &+ (Q v2) = Q (v1 &+ v2) + (Q v1) &- (Q v2) = Q (v1 &+ v2) + neg (Q v) = Q (neg v) + zero = zeroQ + +instance DotProd Quaternion where + dotprod (Q v1) (Q v2) = dotprod v1 v2 + +-} + +instance MultSemiGroup Quaternion where + one = unitQ + (.*.) = multQ + + +-------------------------------------------------------------------------------- +-- * unit quaternions + +unitU :: U +unitU = U (Vec4 1 0 0 0) + +multU :: U -> U -> U +multU (U (Vec4 a1 b1 c1 d1)) (U (Vec4 a2 b2 c2 d2)) = U $ Vec4 + (a1*a2 - b1*b2 - c1*c2 - d1*d2) + (a1*b2 + b1*a2 + c1*d2 - d1*c2) + (a1*c2 - b1*d2 + c1*a2 + d1*b2) + (a1*d2 + b1*c2 - c1*b2 + d1*a2) + +-- | The opposite quaternion (which encodes the same rotation) +negU :: U -> U +negU (U v) = U (neg v) + +-- | This is no-op, up to numerical imprecision. +-- However, if you multiply together a large number of unit quaternions, +-- it may be a good idea to normalize the end result. +normalizeU :: U -> U +normalizeU (U v) = U (normalize v) + +-- | The inverse of a unit quaternion +invU :: U -> U +invU (U (Vec4 a b c d)) = U $ Vec4 a (-b) (-c) (-d) + +-------------------------------------------------------------------------------- + +fromU :: U -> Vec4 +fromU (U v) = v + +fromU' :: U -> Normal4 +fromU' (U v) = toNormalUnsafe v + +mkU :: Vec4 -> U +mkU = U . normalize + +toU :: Normal4 -> U +toU = U . fromNormal + +unsafeToU :: Vec4 -> U +unsafeToU = U + +-------------------------------------------------------------------------------- + +{- +-- we use newtype deriving instead + +instance DotProd UnitQuaternion where + dotprod (Q v1) (Q v2) = dotprod v1 v2 + +-} + +instance MultSemiGroup UnitQuaternion where + one = unitU + (.*.) = multU + +instance LeftModule UnitQuaternion Vec3 where + lmul u v = actU u v + +instance Random UnitQuaternion where + random g = let (n, h) = random g + v = fromNormal n :: Vec4 + in (U v, h) + randomR _ = random + +-------------------------------------------------------------------------------- +-- * unit quaternions as rotations + +-- | The /left/ action of unit quaternions on 3D vectors. +-- That is, +-- +-- > actU q1 $ actU q2 v == actU (q1 `multU` q2) v +actU :: U -> Vec3 -> Vec3 +actU (U (Vec4 a b c d)) (Vec3 x y z) = Vec3 x' y' z' where + x' = x*(aa + bb - cc - dd) + y*( 2 * (bc - ad) ) + z*( 2 * (bd + ac) ) + y' = x*( 2 * (bc + ad) ) + y*(aa - bb + cc - dd) + z*( 2 * (cd - ab) ) + z' = x*( 2 * (bd - ac) ) + y*( 2 * (cd + ab) ) + z*(aa - bb - cc + dd) + -- + aa = a*a ; bb = b*b ; cc = c*c ; dd = d*d + ab = a*b ; ac = a*c ; ad = a*d + bc = b*c ; bd = b*d ; cd = c*d + +-- | The quaternion to encode rotation around an axis. Please note +-- that quaternions act on the /left/, that is +-- +-- > rotU axis1 angl1 *. rotU axis2 angl2 *. v == (rotU axis1 angl1 .*. rotU axis2 angl2) *. v +-- +rotU :: Vec3 -> Flt -> U +rotU axis angle = rotU' (mkNormal axis) angle + +rotU' {- ' CPP is sensitive to primes -} :: Normal3 -> Flt -> U +rotU' axis angle = U (Vec4 c (x*s) (y*s) (z*s)) where + Vec3 x y z = fromNormal axis + half = 0.5 * angle + c = cos half + s = sin half + +-- | Interpolation of unit quaternions. Note that when applied to rotations, +-- this may be not what you want, since it is possible that the shortest path +-- in the space of unit quaternions is not the shortest path in the space of +-- rotations; see 'slerpU'! +longSlerpU :: Flt -> U -> U -> U +longSlerpU t (U p0) (U p1) = U v where + v = (p0 &* y0) &+ (p1 &* y1) + omega = acos (p0 &. p1) + s = sin omega + y0 = sin (omega*(1-t)) / s + y1 = sin (omega* t ) / s + +-- | This is shortest path interpolation in the space of rotations; however +-- this is achieved by possibly flipping the first endpoint in the space of +-- quaternions. Thus @slerpU 0.001 q1 q2@ may be very far from @q1@ (and very +-- close to @negU q1@) in the space of quaternions (but they are very close +-- in the space of rotations). +slerpU :: Flt -> U -> U -> U +slerpU t (U p0') (U p1) = U v where + v = (p0 &* y0) &+ (p1 &* y1) + + d' = p0' &. p1 + (d,p0) = if d' >= 0 + then ( d', p0') + else (-d', neg p0') + + omega = acos d + s = sin omega + y0 = sin (omega*(1-t)) / s + y1 = sin (omega* t ) / s + +-- | Makes a rotation matrix (to be multiplied with on the /right/) out of a unit quaternion: +-- +-- > v .* rightOrthoU (rotU axis angl) == v .* rotMatrix3 axis angl +-- +-- Please note that while these matrices act on the /right/, quaternions act on the /left/; thus +-- +-- > rightOrthoU q1 .*. rightOrthoU q2 == rightOrthoU (q2 .*. q1) +-- +rightOrthoU :: U -> Ortho3 +rightOrthoU = toOrthoUnsafe . transpose . fromOrtho . leftOrthoU + +-- | Makes a rotation matrix (to be multiplied with on the /left/) out of a unit quaternion. +-- +-- > leftOrthoU (rotU axis angl) *. v == v .* rotMatrix3 axis angl +-- +leftOrthoU :: U -> Ortho3 +leftOrthoU (U (Vec4 a b c d)) = toOrthoUnsafe $ Mat3 row1 row2 row3 where + row1 = Vec3 (aa + bb - cc - dd) ( 2 * (bc - ad) ) ( 2 * (bd + ac) ) + row2 = Vec3 ( 2 * (bc + ad) ) (aa - bb + cc - dd) ( 2 * (cd - ab) ) + row3 = Vec3 ( 2 * (bd - ac) ) ( 2 * (cd + ab) ) (aa - bb - cc + dd) + -- + aa = a*a ; bb = b*b ; cc = c*c ; dd = d*d + ab = a*b ; ac = a*c ; ad = a*d + bc = b*c ; bd = b*d ; cd = c*d + +-------------------------------------------------------------------------------- + + diff --git a/vect.cabal b/vect.cabal --- a/vect.cabal +++ b/vect.cabal @@ -1,21 +1,21 @@ Name: vect -Version: 0.4.6 +Version: 0.4.7 Synopsis: A low-dimensional linear algebra library, tailored to computer graphics. Description: A low-dimensional (2, 3 and 4) linear algebra library, with lots of useful functions. Intended usage is primarily computer graphics (basic OpenGL support is included as a separate package). - Projective 4 dimensional operations, as used in eg. - OpenGL, are also supported. + Projective 4 dimensional operations, as used in eg. OpenGL, + are also supported; and so are quaternions. The base field is either Float or Double. License: BSD3 License-file: LICENSE Author: Balazs Komuves -Copyright: (c) 2008-2010 Balazs Komuves +Copyright: (c) 2008-2011 Balazs Komuves Maintainer: bkomuves (plus) hackage (at) gmail (dot) com Homepage: http://code.haskell.org/~bkomuves/ Stability: Experimental Category: Graphics, Math -Tested-With: GHC == 6.10.1 +Tested-With: GHC == 6.12.3 Cabal-Version: >= 1.6 Build-Type: Custom @@ -27,7 +27,12 @@ src/flt/Util/Dim3.hs, src/flt/Util/Dim4.hs, src/flt/Util/Projective.hs, + src/flt/Util/Quaternion.hs, src/flt/Instances.hs + +source-repository head + type: darcs + location: http://code.haskell.org/~bkomuves/projects/vect/ Flag splitBase Description: Choose the new smaller, split-up base package. @@ -49,6 +54,7 @@ Data.Vect.Float.Util.Dim3, Data.Vect.Float.Util.Dim4, Data.Vect.Float.Util.Projective, + Data.Vect.Float.Util.Quaternion, Data.Vect.Float.Instances Data.Vect.Double, @@ -59,6 +65,7 @@ Data.Vect.Double.Util.Dim3, Data.Vect.Double.Util.Dim4, Data.Vect.Double.Util.Projective, + Data.Vect.Double.Util.Quaternion, Data.Vect.Double.Instances Hs-Source-Dirs: .