module Linear.Plucker
( Plucker(..)
, squaredError
, isotropic
, (><)
, plucker
, intersects
) where
import Control.Applicative
import Data.Distributive
import Data.Foldable as Foldable
import Data.Monoid
import Data.Traversable
import Linear.Epsilon
import Linear.Metric
import Control.Lens
import Linear.V4
data Plucker a = Plucker a a a a a a deriving (Eq,Ord,Show,Read)
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 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 Monad Plucker where
return a = Plucker a a a a a a
(>>=) = bindRep
instance Distributive Plucker where
distribute = distributeRep
instance Representable Plucker where
rep f = Plucker (f p01) (f p02) (f p03) (f p23) (f p31) (f p12)
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 Num a => Num (Plucker a) where
(+) = 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
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)
(a*dh*e)
(c*hd*g)
(d*fb*h)
(b*gc*f)
p01, p02, p03, p23, p31, p12 :: Functor f => (a -> f a) -> Plucker a -> f (Plucker 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
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*g+b*h+c*id*je*kf*l
isotropic :: Epsilon a => Plucker a -> Bool
isotropic a = nearZero (a >< a)
intersects :: Epsilon a => Plucker a -> Plucker a -> Bool
intersects a b = nearZero (a >< b)
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