#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(..))
#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 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
#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 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
#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)
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