#if defined(__GLASGOW_HASKELL__) && __GLASGOW_HASKELL__ >= 702
#endif
module Linear.Plucker
( Plucker(..)
, squaredError
, isotropic
, (><)
, plucker
, plucker3D
, parallel
, intersects
, LinePass(..)
, passes
, quadranceToOrigin
, closestToOrigin
, isLine
, Coincides(..)
, p01, p02, p03
, p10, p12, p13
, p20, p21, p23
, p30, p31, p32
) where
import Control.Applicative
import Data.Distributive
import Data.Foldable as Foldable
import Data.Functor.Bind
import Data.Semigroup
import Data.Semigroup.Foldable
import Data.Semigroup.Traversable
import Data.Traversable
import Foreign.Ptr (castPtr)
import Foreign.Storable (Storable(..))
import GHC.Arr (Ix(..))
import Linear.Core
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)
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 Core Plucker where
core 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 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 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 :: 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
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)
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)
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)
data Coincides a where
Line :: (Epsilon a, Fractional a) => Plucker a -> Coincides a
Ray :: (Epsilon a, Fractional a, Ord a) => Plucker a -> Coincides a
instance Eq (Coincides a) where
Line a == Line b = coincides a b
Line a == Ray b = coincides a b
Ray a == Line b = coincides a b
Ray a == Ray b = coincides' a b
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