#if defined(__GLASGOW_HASKELL__) && __GLASGOW_HASKELL__ >= 702
#endif
#ifndef MIN_VERSION_vector
#define MIN_VERSION_vector(x,y,z) 1
#endif
module Linear.Plucker
( Plucker(..)
, squaredError
, isotropic
, (><)
, plucker
, plucker3D
, parallel
, intersects
, LinePass(..)
, passes
, quadranceToOrigin
, closestToOrigin
, isLine
, coincides
, coincides'
, p01, p02, p03
, p10, p12, p13
, p20, p21, p23
, p30, p31, p32
, e01, e02, e03, e12, e31, e23
) where
import Control.Applicative
import Control.DeepSeq (NFData(rnf))
import Control.Monad (liftM)
import Control.Monad.Fix
import Control.Monad.Zip
import Control.Lens hiding (index, (<.>))
import Data.Binary as Binary
import Data.Bytes.Serial
import Data.Distributive
import Data.Foldable as Foldable
import Data.Functor.Bind
import Data.Functor.Classes
import Data.Functor.Rep
import Data.Hashable
import Data.Semigroup
import Data.Semigroup.Foldable
import Data.Serialize as Cereal
import Foreign.Ptr (castPtr)
import Foreign.Storable (Storable(..))
import GHC.Arr (Ix(..))
#if defined(__GLASGOW_HASKELL__) && __GLASGOW_HASKELL__ >= 702
import GHC.Generics (Generic)
#endif
#if defined(__GLASGOW_HASKELL__) && __GLASGOW_HASKELL__ >= 706
import GHC.Generics (Generic1)
#endif
import qualified Data.Vector.Generic.Mutable as M
import qualified Data.Vector.Generic as G
import qualified Data.Vector.Unboxed.Base as U
import Linear.Epsilon
import Linear.Metric
import Linear.V2
import Linear.V3
import Linear.V4
import Linear.Vector
data Plucker a = Plucker !a !a !a !a !a !a deriving (Eq,Ord,Show,Read
#if defined(__GLASGOW_HASKELL__) && __GLASGOW_HASKELL__ >= 702
,Generic
#endif
#if defined(__GLASGOW_HASKELL__) && __GLASGOW_HASKELL__ >= 706
,Generic1
#endif
)
instance Functor Plucker where
fmap g (Plucker a b c d e f) = Plucker (g a) (g b) (g c) (g d) (g e) (g f)
instance Apply Plucker where
Plucker a b c d e f <.> Plucker g h i j k l =
Plucker (a g) (b h) (c i) (d j) (e k) (f l)
instance Applicative Plucker where
pure a = Plucker a a a a a a
Plucker a b c d e f <*> Plucker g h i j k l =
Plucker (a g) (b h) (c i) (d j) (e k) (f l)
instance Additive Plucker where
zero = pure 0
liftU2 = liftA2
liftI2 = liftA2
instance Bind Plucker where
Plucker a b c d e f >>- g = Plucker a' b' c' d' e' f' where
Plucker a' _ _ _ _ _ = g a
Plucker _ b' _ _ _ _ = g b
Plucker _ _ c' _ _ _ = g c
Plucker _ _ _ d' _ _ = g d
Plucker _ _ _ _ e' _ = g e
Plucker _ _ _ _ _ f' = g f
instance Monad Plucker where
return a = Plucker a a a a a a
Plucker a b c d e f >>= g = Plucker a' b' c' d' e' f' where
Plucker a' _ _ _ _ _ = g a
Plucker _ b' _ _ _ _ = g b
Plucker _ _ c' _ _ _ = g c
Plucker _ _ _ d' _ _ = g d
Plucker _ _ _ _ e' _ = g e
Plucker _ _ _ _ _ f' = g f
instance Distributive Plucker where
distribute f = Plucker (fmap (\(Plucker x _ _ _ _ _) -> x) f)
(fmap (\(Plucker _ x _ _ _ _) -> x) f)
(fmap (\(Plucker _ _ x _ _ _) -> x) f)
(fmap (\(Plucker _ _ _ x _ _) -> x) f)
(fmap (\(Plucker _ _ _ _ x _) -> x) f)
(fmap (\(Plucker _ _ _ _ _ x) -> x) f)
instance Representable Plucker where
type Rep Plucker = E Plucker
tabulate f = Plucker (f e01) (f e02) (f e03) (f e23) (f e31) (f e12)
index xs (E l) = view l xs
instance Foldable Plucker where
foldMap g (Plucker a b c d e f) =
g a `mappend` g b `mappend` g c `mappend` g d `mappend` g e `mappend` g f
instance Traversable Plucker where
traverse g (Plucker a b c d e f) =
Plucker <$> g a <*> g b <*> g c <*> g d <*> g e <*> g f
instance Foldable1 Plucker where
foldMap1 g (Plucker a b c d e f) =
g a <> g b <> g c <> g d <> g e <> g f
instance Traversable1 Plucker where
traverse1 g (Plucker a b c d e f) =
Plucker <$> g a <.> g b <.> g c <.> g d <.> g e <.> g f
instance Ix a => Ix (Plucker a) where
range (Plucker l1 l2 l3 l4 l5 l6,Plucker u1 u2 u3 u4 u5 u6) =
[Plucker i1 i2 i3 i4 i5 i6 | i1 <- range (l1,u1)
, i2 <- range (l2,u2)
, i3 <- range (l3,u3)
, i4 <- range (l4,u4)
, i5 <- range (l5,u5)
, i6 <- range (l6,u6)
]
unsafeIndex (Plucker l1 l2 l3 l4 l5 l6,Plucker u1 u2 u3 u4 u5 u6) (Plucker i1 i2 i3 i4 i5 i6) =
unsafeIndex (l6,u6) i6 + unsafeRangeSize (l6,u6) * (
unsafeIndex (l5,u5) i5 + unsafeRangeSize (l5,u5) * (
unsafeIndex (l4,u4) i4 + unsafeRangeSize (l4,u4) * (
unsafeIndex (l3,u3) i3 + unsafeRangeSize (l3,u3) * (
unsafeIndex (l2,u2) i2 + unsafeRangeSize (l2,u2) *
unsafeIndex (l1,u1) i1))))
inRange (Plucker l1 l2 l3 l4 l5 l6,Plucker u1 u2 u3 u4 u5 u6) (Plucker i1 i2 i3 i4 i5 i6) =
inRange (l1,u1) i1 && inRange (l2,u2) i2 &&
inRange (l3,u3) i3 && inRange (l4,u4) i4 &&
inRange (l5,u5) i5 && inRange (l6,u6) i6
instance Num a => Num (Plucker a) where
(+) = liftA2 (+)
() = liftA2 ()
(*) = liftA2 (*)
negate = fmap negate
abs = fmap abs
signum = fmap signum
fromInteger = pure . fromInteger
instance Fractional a => Fractional (Plucker a) where
recip = fmap recip
(/) = liftA2 (/)
fromRational = pure . fromRational
instance Floating a => Floating (Plucker a) where
pi = pure pi
exp = fmap exp
sqrt = fmap sqrt
log = fmap log
(**) = liftA2 (**)
logBase = liftA2 logBase
sin = fmap sin
tan = fmap tan
cos = fmap cos
asin = fmap asin
atan = fmap atan
acos = fmap acos
sinh = fmap sinh
tanh = fmap tanh
cosh = fmap cosh
asinh = fmap asinh
atanh = fmap atanh
acosh = fmap acosh
instance Hashable a => Hashable (Plucker a) where
hashWithSalt s (Plucker a b c d e f) = s `hashWithSalt` a `hashWithSalt` b `hashWithSalt` c `hashWithSalt` d `hashWithSalt` e `hashWithSalt` f
instance Storable a => Storable (Plucker a) where
sizeOf _ = 6 * sizeOf (undefined::a)
alignment _ = alignment (undefined::a)
poke ptr (Plucker a b c d e f) = do
poke ptr' a
pokeElemOff ptr' 1 b
pokeElemOff ptr' 2 c
pokeElemOff ptr' 3 d
pokeElemOff ptr' 4 e
pokeElemOff ptr' 5 f
where ptr' = castPtr ptr
peek ptr = Plucker <$> peek ptr'
<*> peekElemOff ptr' 1
<*> peekElemOff ptr' 2
<*> peekElemOff ptr' 3
<*> peekElemOff ptr' 4
<*> peekElemOff ptr' 5
where ptr' = castPtr ptr
instance Metric Plucker where
dot (Plucker a b c d e f) (Plucker g h i j k l) = a*g+b*h+c*i+d*j+e*k+f*l
instance Epsilon a => Epsilon (Plucker a) where
nearZero = nearZero . quadrance
plucker :: Num a => V4 a -> V4 a -> Plucker a
plucker (V4 a b c d)
(V4 e f g h) =
Plucker (a*fb*e)
(a*gc*e)
(b*gc*f)
(a*hd*e)
(b*hd*f)
(c*hd*g)
plucker3D :: Num a => V3 a -> V3 a -> Plucker a
plucker3D p q = Plucker a b c d e f
where V3 a b c = p q
V3 d e f = p `cross` q
p01, p02, p03, p23, p31, p12 :: Lens' (Plucker a) a
p01 g (Plucker a b c d e f) = (\a' -> Plucker a' b c d e f) <$> g a
p02 g (Plucker a b c d e f) = (\b' -> Plucker a b' c d e f) <$> g b
p03 g (Plucker a b c d e f) = (\c' -> Plucker a b c' d e f) <$> g c
p23 g (Plucker a b c d e f) = (\d' -> Plucker a b c d' e f) <$> g d
p31 g (Plucker a b c d e f) = (\e' -> Plucker a b c d e' f) <$> g e
p12 g (Plucker a b c d e f) = Plucker a b c d e <$> g f
p10, p20, p30, p32, p13, p21 :: (Functor f, Num a) => (a -> f a) -> Plucker a -> f (Plucker a)
p10 = anti p01
p20 = anti p02
p30 = anti p03
p32 = anti p23
p13 = anti p31
p21 = anti p21
anti :: (Functor f, Num a) => ((a -> f a) -> r) -> (a -> f a) -> r
anti k f = k (fmap negate . f . negate)
e01, e02, e03, e23, e31, e12 :: E Plucker
e01 = E p01
e02 = E p02
e03 = E p03
e23 = E p23
e31 = E p31
e12 = E p12
instance FunctorWithIndex (E Plucker) Plucker where
imap f (Plucker a b c d e g) = Plucker (f e01 a) (f e02 b) (f e03 c) (f e23 d) (f e31 e) (f e12 g)
instance FoldableWithIndex (E Plucker) Plucker where
ifoldMap f (Plucker a b c d e g) = f e01 a `mappend` f e02 b `mappend` f e03 c
`mappend` f e23 d `mappend` f e31 e `mappend` f e12 g
instance TraversableWithIndex (E Plucker) Plucker where
itraverse f (Plucker a b c d e g) = Plucker <$> f e01 a <*> f e02 b <*> f e03 c
<*> f e23 d <*> f e31 e <*> f e12 g
type instance Index (Plucker a) = E Plucker
type instance IxValue (Plucker a) = a
instance Ixed (Plucker a) where
ix = el
instance Each (Plucker a) (Plucker b) a b where
each = traverse
squaredError :: (Eq a, Num a) => Plucker a -> a
squaredError v = v >< v
infixl 5 ><
(><) :: Num a => Plucker a -> Plucker a -> a
Plucker a b c d e f >< Plucker g h i j k l = a*lb*k+c*j+d*ie*h+f*g
isotropic :: Epsilon a => Plucker a -> Bool
isotropic a = nearZero (a >< a)
intersects :: (Epsilon a, Ord a) => Plucker a -> Plucker a -> Bool
intersects a b = not (a `parallel` b) && passes a b == Coplanar
data LinePass = Coplanar
| Clockwise
| Counterclockwise
deriving (Eq, Show
#if defined(__GLASGOW_HASKELL__) && __GLASGOW_HASKELL__ >= 702
,Generic
#endif
)
passes :: (Epsilon a, Num a, Ord a) => Plucker a -> Plucker a -> LinePass
passes a b
| nearZero s = Coplanar
| s > 0 = Counterclockwise
| otherwise = Clockwise
where s = (u1 `dot` v2) + (u2 `dot` v1)
V2 u1 v1 = toUV a
V2 u2 v2 = toUV b
parallel :: Epsilon a => Plucker a -> Plucker a -> Bool
parallel a b = nearZero $ u1 `cross` u2
where V2 u1 _ = toUV a
V2 u2 _ = toUV b
toUV :: Plucker a -> V2 (V3 a)
toUV (Plucker a b c d e f) = V2 (V3 a b c) (V3 d e f)
coincides :: (Epsilon a, Fractional a) => Plucker a -> Plucker a -> Bool
coincides p1 p2 = Foldable.all nearZero $ (s *^ p2) p1
where s = maybe 1 getFirst . getOption . fold $ saveDiv <$> p1 <*> p2
saveDiv x y | nearZero y = Option Nothing
| otherwise = Option . Just $ First (x / y)
coincides' :: (Epsilon a, Fractional a, Ord a) => Plucker a -> Plucker a -> Bool
coincides' p1 p2 = Foldable.all nearZero ((s *^ p2) p1) && s > 0
where s = maybe 1 getFirst . getOption . fold $ saveDiv <$> p1 <*> p2
saveDiv x y | nearZero y = Option Nothing
| otherwise = Option . Just $ First (x / y)
quadranceToOrigin :: Fractional a => Plucker a -> a
quadranceToOrigin p = (v `dot` v) / (u `dot` u)
where V2 u v = toUV p
closestToOrigin :: Fractional a => Plucker a -> V3 a
closestToOrigin p = normalizePoint $ V4 x y z (u `dot` u)
where V2 u v = toUV p
V3 x y z = v `cross` u
isLine :: Epsilon a => Plucker a -> Bool
isLine p = nearZero $ u `dot` v
where V2 u v = toUV p
data instance U.Vector (Plucker a) = V_Plucker !Int (U.Vector a)
data instance U.MVector s (Plucker a) = MV_Plucker !Int (U.MVector s a)
instance U.Unbox a => U.Unbox (Plucker a)
instance U.Unbox a => M.MVector U.MVector (Plucker a) where
basicLength (MV_Plucker n _) = n
basicUnsafeSlice m n (MV_Plucker _ v) = MV_Plucker n (M.basicUnsafeSlice (6*m) (6*n) v)
basicOverlaps (MV_Plucker _ v) (MV_Plucker _ u) = M.basicOverlaps v u
basicUnsafeNew n = liftM (MV_Plucker n) (M.basicUnsafeNew (6*n))
basicUnsafeRead (MV_Plucker _ a) i =
do let o = 6*i
x <- M.basicUnsafeRead a o
y <- M.basicUnsafeRead a (o+1)
z <- M.basicUnsafeRead a (o+2)
w <- M.basicUnsafeRead a (o+3)
v <- M.basicUnsafeRead a (o+4)
u <- M.basicUnsafeRead a (o+5)
return (Plucker x y z w v u)
basicUnsafeWrite (MV_Plucker _ a) i (Plucker x y z w v u) =
do let o = 6*i
M.basicUnsafeWrite a o x
M.basicUnsafeWrite a (o+1) y
M.basicUnsafeWrite a (o+2) z
M.basicUnsafeWrite a (o+3) w
M.basicUnsafeWrite a (o+4) v
M.basicUnsafeWrite a (o+5) u
#if MIN_VERSION_vector(0,11,0)
basicInitialize (MV_Plucker _ v) = M.basicInitialize v
#endif
instance U.Unbox a => G.Vector U.Vector (Plucker a) where
basicUnsafeFreeze (MV_Plucker n v) = liftM ( V_Plucker n) (G.basicUnsafeFreeze v)
basicUnsafeThaw ( V_Plucker n v) = liftM (MV_Plucker n) (G.basicUnsafeThaw v)
basicLength ( V_Plucker n _) = n
basicUnsafeSlice m n (V_Plucker _ v) = V_Plucker n (G.basicUnsafeSlice (6*m) (6*n) v)
basicUnsafeIndexM (V_Plucker _ a) i =
do let o = 6*i
x <- G.basicUnsafeIndexM a o
y <- G.basicUnsafeIndexM a (o+1)
z <- G.basicUnsafeIndexM a (o+2)
w <- G.basicUnsafeIndexM a (o+3)
v <- G.basicUnsafeIndexM a (o+4)
u <- G.basicUnsafeIndexM a (o+5)
return (Plucker x y z w v u)
instance MonadZip Plucker where
mzipWith = liftA2
instance MonadFix Plucker where
mfix f = Plucker (let Plucker a _ _ _ _ _ = f a in a)
(let Plucker _ a _ _ _ _ = f a in a)
(let Plucker _ _ a _ _ _ = f a in a)
(let Plucker _ _ _ a _ _ = f a in a)
(let Plucker _ _ _ _ a _ = f a in a)
(let Plucker _ _ _ _ _ a = f a in a)
instance NFData a => NFData (Plucker a) where
rnf (Plucker a b c d e f) = rnf a `seq` rnf b `seq` rnf c
`seq` rnf d `seq` rnf e `seq` rnf f
instance Serial1 Plucker where
serializeWith = traverse_
deserializeWith k = Plucker <$> k <*> k <*> k <*> k <*> k <*> k
instance Serial a => Serial (Plucker a) where
serialize = serializeWith serialize
deserialize = deserializeWith deserialize
instance Binary a => Binary (Plucker a) where
put = serializeWith Binary.put
get = deserializeWith Binary.get
instance Serialize a => Serialize (Plucker a) where
put = serializeWith Cereal.put
get = deserializeWith Cereal.get
instance Eq1 Plucker where eq1 = (==)
instance Ord1 Plucker where compare1 = compare
instance Show1 Plucker where showsPrec1 = showsPrec
instance Read1 Plucker where readsPrec1 = readsPrec