{-# LANGUAGE Unsafe #-}
{-# LANGUAGE UndecidableInstances #-}
--------------------------------------------------------------------------------
-- |
-- Module      :  Data.Geometry.Transformation
-- Copyright   :  (C) Frank Staals
-- Maintainer  :  Frank Staals
--------------------------------------------------------------------------------
module Data.Geometry.Transformation where

import           Control.Lens (iso,set,Iso,imap)
import           Data.Geometry.Matrix
import           Data.Geometry.Matrix.Internal (mkRow)
import           Data.Geometry.Point
import           Data.Geometry.Properties
import           Data.Geometry.Vector
import qualified Data.Geometry.Vector as V
import           Data.Proxy
import           GHC.TypeLits

{- \$setup
>>> import Data.Geometry.LineSegment
>>> import Data.Ext
-}

--------------------------------------------------------------------------------
-- * Transformations

-- | A type representing a Transformation for d dimensional objects
newtype Transformation d r = Transformation { Transformation d r -> Matrix (d + 1) (d + 1) r
_transformationMatrix :: Matrix (d + 1) (d + 1) r }

-- | Transformations and Matrices are isomorphic.
transformationMatrix :: Iso (Transformation d r)       (Transformation d       s)
(Matrix (d + 1) (d + 1) r) (Matrix (d + 1) (d + 1) s)
transformationMatrix :: p (Matrix (d + 1) (d + 1) r) (f (Matrix (d + 1) (d + 1) s))
-> p (Transformation d r) (f (Transformation d s))
transformationMatrix = (Transformation d r -> Matrix (d + 1) (d + 1) r)
-> (Matrix (d + 1) (d + 1) s -> Transformation d s)
-> Iso
(Transformation d r)
(Transformation d s)
(Matrix (d + 1) (d + 1) r)
(Matrix (d + 1) (d + 1) s)
forall s a b t. (s -> a) -> (b -> t) -> Iso s t a b
iso Transformation d r -> Matrix (d + 1) (d + 1) r
forall (d :: Nat) r. Transformation d r -> Matrix (d + 1) (d + 1) r
_transformationMatrix Matrix (d + 1) (d + 1) s -> Transformation d s
forall (d :: Nat) r. Matrix (d + 1) (d + 1) r -> Transformation d r
Transformation

deriving instance (Show r, Arity (d + 1)) => Show (Transformation d r)
deriving instance (Eq r, Arity (d + 1))   => Eq (Transformation d r)
deriving instance (Ord r, Arity (d + 1))  => Ord (Transformation d r)
deriving instance Arity (d + 1)           => Functor (Transformation d)
deriving instance Arity (d + 1)           => Foldable (Transformation d)
deriving instance Arity (d + 1)           => Traversable (Transformation d)

type instance NumType (Transformation d r) = r

-- | Compose transformations (right to left)
(|.|) :: (Num r, Arity (d + 1)) => Transformation d r -> Transformation d r -> Transformation d r
(Transformation Matrix (d + 1) (d + 1) r
f) |.| :: Transformation d r -> Transformation d r -> Transformation d r
|.| (Transformation Matrix (d + 1) (d + 1) r
g) = Matrix (d + 1) (d + 1) r -> Transformation d r
forall (d :: Nat) r. Matrix (d + 1) (d + 1) r -> Transformation d r
Transformation (Matrix (d + 1) (d + 1) r -> Transformation d r)
-> Matrix (d + 1) (d + 1) r -> Transformation d r
forall a b. (a -> b) -> a -> b
\$ Matrix (d + 1) (d + 1) r
f Matrix (d + 1) (d + 1) r
-> Matrix (d + 1) (d + 1) r -> Matrix (d + 1) (d + 1) r
forall (r :: Nat) (c :: Nat) (c' :: Nat) a.
(Arity r, Arity c, Arity c', Num a) =>
Matrix r c a -> Matrix c c' a -> Matrix r c' a
`multM` Matrix (d + 1) (d + 1) r
g

-- if it exists?

-- | Compute the inverse transformation
--
-- >>> inverseOf \$ translation (Vector2 (10.0) (5.0))
-- Transformation {_transformationMatrix = Matrix (Vector3 (Vector3 1.0 0.0 (-10.0)) (Vector3 0.0 1.0 (-5.0)) (Vector3 0.0 0.0 1.0))}
inverseOf :: (Fractional r, Invertible (d + 1) r)
=> Transformation d r -> Transformation d r
inverseOf :: Transformation d r -> Transformation d r
inverseOf = Matrix (d + 1) (d + 1) r -> Transformation d r
forall (d :: Nat) r. Matrix (d + 1) (d + 1) r -> Transformation d r
Transformation (Matrix (d + 1) (d + 1) r -> Transformation d r)
-> (Transformation d r -> Matrix (d + 1) (d + 1) r)
-> Transformation d r
-> Transformation d r
forall b c a. (b -> c) -> (a -> b) -> a -> c
. Matrix (d + 1) (d + 1) r -> Matrix (d + 1) (d + 1) r
forall (n :: Nat) r. Invertible n r => Matrix n n r -> Matrix n n r
inverse' (Matrix (d + 1) (d + 1) r -> Matrix (d + 1) (d + 1) r)
-> (Transformation d r -> Matrix (d + 1) (d + 1) r)
-> Transformation d r
-> Matrix (d + 1) (d + 1) r
forall b c a. (b -> c) -> (a -> b) -> a -> c
. Transformation d r -> Matrix (d + 1) (d + 1) r
forall (d :: Nat) r. Transformation d r -> Matrix (d + 1) (d + 1) r
_transformationMatrix

--------------------------------------------------------------------------------
-- * Transformable geometry objects

-- | A class representing types that can be transformed using a transformation
class IsTransformable g where
transformBy :: Transformation (Dimension g) (NumType g) -> g -> g

-- | Apply a transformation to a collection of objects.
--
-- >>> transformAllBy (uniformScaling 2) [Point1 1, Point1 2, Point1 3]
-- [Point1 2.0,Point1 4.0,Point1 6.0]
transformAllBy :: (Functor c, IsTransformable g)
=> Transformation (Dimension g) (NumType g) -> c g -> c g
transformAllBy :: Transformation (Dimension g) (NumType g) -> c g -> c g
transformAllBy Transformation (Dimension g) (NumType g)
t = (g -> g) -> c g -> c g
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
fmap (Transformation (Dimension g) (NumType g) -> g -> g
forall g.
IsTransformable g =>
Transformation (Dimension g) (NumType g) -> g -> g
transformBy Transformation (Dimension g) (NumType g)
t)

-- | Apply transformation to a PointFunctor, ie something that contains
--   points. Polygons, triangles, line segments, etc, are all PointFunctors.
--
-- >>> transformPointFunctor (uniformScaling 2) \$ OpenLineSegment (Point1 1 :+ ()) (Point1 2 :+ ())
-- OpenLineSegment (Point1 2.0 :+ ()) (Point1 4.0 :+ ())
transformPointFunctor   :: ( PointFunctor g, Fractional r, d ~ Dimension (g r)
, Arity d, Arity (d + 1)
) => Transformation d r -> g r -> g r
transformPointFunctor :: Transformation d r -> g r -> g r
transformPointFunctor Transformation d r
t = (Point (Dimension (g r)) r -> Point (Dimension (g r)) r)
-> g r -> g r
forall (g :: * -> *) r s.
PointFunctor g =>
(Point (Dimension (g r)) r -> Point (Dimension (g s)) s)
-> g r -> g s
pmap (Transformation (Dimension (Point d r)) (NumType (Point d r))
-> Point d r -> Point d r
forall g.
IsTransformable g =>
Transformation (Dimension g) (NumType g) -> g -> g
transformBy Transformation d r
Transformation (Dimension (Point d r)) (NumType (Point d r))
t)

instance (Fractional r, Arity d, Arity (d + 1))
=> IsTransformable (Point d r) where
transformBy :: Transformation (Dimension (Point d r)) (NumType (Point d r))
-> Point d r -> Point d r
transformBy Transformation (Dimension (Point d r)) (NumType (Point d r))
t = Vector d r -> Point d r
forall (d :: Nat) r. Vector d r -> Point d r
Point (Vector d r -> Point d r)
-> (Point d r -> Vector d r) -> Point d r -> Point d r
forall b c a. (b -> c) -> (a -> b) -> a -> c
. Transformation (Dimension (Vector d r)) (NumType (Vector d r))
-> Vector d r -> Vector d r
forall g.
IsTransformable g =>
Transformation (Dimension g) (NumType g) -> g -> g
transformBy Transformation (Dimension (Vector d r)) (NumType (Vector d r))
Transformation (Dimension (Point d r)) (NumType (Point d r))
t (Vector d r -> Vector d r)
-> (Point d r -> Vector d r) -> Point d r -> Vector d r
forall b c a. (b -> c) -> (a -> b) -> a -> c
. Point d r -> Vector d r
forall (d :: Nat) r. Point d r -> Vector d r
toVec

instance (Fractional r, Arity d, Arity (d + 1))
=> IsTransformable (Vector d r) where
transformBy :: Transformation (Dimension (Vector d r)) (NumType (Vector d r))
-> Vector d r -> Vector d r
transformBy (Transformation Matrix
(Dimension (Vector d r) + 1)
(Dimension (Vector d r) + 1)
(NumType (Vector d r))
m) Vector d r
v = Vector (d + 1) r -> Vector d r
forall (d :: Nat) b.
(ImplicitPeano (Peano (d + 1)), ImplicitPeano (Peano d),
Fractional b, ArityPeano (Peano (FromPeano (Peano d))),
ArityPeano (Peano (FromPeano (Peano (d + 1)))), KnownNat d,
KnownNat (FromPeano (Peano d)),
KnownNat (FromPeano (Peano (d + 1))), KnownNat (d + 1),
Peano (FromPeano (Peano d) + 1) ~ 'S (Peano (FromPeano (Peano d))),
Peano (FromPeano (Peano (d + 1)) + 1)
~ 'S (Peano (FromPeano (Peano (d + 1))))) =>
Vector (d + 1) b -> Vector d b
f (Vector (d + 1) r -> Vector d r) -> Vector (d + 1) r -> Vector d r
forall a b. (a -> b) -> a -> b
\$ Matrix (d + 1) (d + 1) r
Matrix
(Dimension (Vector d r) + 1)
(Dimension (Vector d r) + 1)
(NumType (Vector d r))
m Matrix (d + 1) (d + 1) r -> Vector (d + 1) r -> Vector (d + 1) r
forall (m :: Nat) (n :: Nat) r.
(Arity m, Arity n, Num r) =>
Matrix n m r -> Vector m r -> Vector n r
`mult` Vector d r -> r -> Vector (d + 1) r
forall (d :: Nat) r.
(Arity (d + 1), Arity d) =>
Vector d r -> r -> Vector (d + 1) r
snoc Vector d r
v r
1
where
f :: Vector (d + 1) b -> Vector d b
f Vector (d + 1) b
u   = (b -> b -> b
forall a. Fractional a => a -> a -> a
/ Vector (d + 1) b -> b
forall (d :: Nat) r.
(KnownNat d, Arity (d + 1)) =>
Vector (d + 1) r -> r
V.last Vector (d + 1) b
u) (b -> b) -> Vector d b -> Vector d b
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<\$> Vector (d + 1) b -> Vector d b
forall (d :: Nat) r.
(Arity d, Arity (d + 1)) =>
Vector (d + 1) r -> Vector d r
V.init Vector (d + 1) b
u

--------------------------------------------------------------------------------
-- * Common transformations

-- | Create translation transformation from a vector.
--
-- >>> transformBy (translation \$ Vector2 1 2) \$ Point2 2 3
-- Point2 3.0 5.0
translation   :: (Num r, Arity d, Arity (d + 1))
=> Vector d r -> Transformation d r
translation :: Vector d r -> Transformation d r
translation Vector d r
v = Matrix (d + 1) (d + 1) r -> Transformation d r
forall (d :: Nat) r. Matrix (d + 1) (d + 1) r -> Transformation d r
Transformation (Matrix (d + 1) (d + 1) r -> Transformation d r)
-> (Vector (d + 1) (Vector (d + 1) r) -> Matrix (d + 1) (d + 1) r)
-> Vector (d + 1) (Vector (d + 1) r)
-> Transformation d r
forall b c a. (b -> c) -> (a -> b) -> a -> c
. Vector (d + 1) (Vector (d + 1) r) -> Matrix (d + 1) (d + 1) r
forall (n :: Nat) (m :: Nat) r.
Vector n (Vector m r) -> Matrix n m r
Matrix (Vector (d + 1) (Vector (d + 1) r) -> Transformation d r)
-> Vector (d + 1) (Vector (d + 1) r) -> Transformation d r
forall a b. (a -> b) -> a -> b
\$ (Int -> r -> Vector (d + 1) r)
-> Vector (d + 1) r -> Vector (d + 1) (Vector (d + 1) r)
forall i (f :: * -> *) a b.
FunctorWithIndex i f =>
(i -> a -> b) -> f a -> f b
imap Int -> r -> Vector (d + 1) r
forall (n :: Nat) r.
(Arity n, Arity (n + 1), Num r) =>
Int -> r -> Vector (n + 1) r
transRow (Vector d r -> r -> Vector (d + 1) r
forall (d :: Nat) r.
(Arity (d + 1), Arity d) =>
Vector d r -> r -> Vector (d + 1) r
snoc Vector d r
v r
1)

-- | Create scaling transformation from a vector.
--
-- >>> transformBy (scaling \$ Vector2 2 (-1)) \$ Point2 2 3
-- Point2 4.0 (-3.0)
scaling   :: (Num r, Arity d, Arity (d + 1))
=> Vector d r -> Transformation d r
scaling :: Vector d r -> Transformation d r
scaling Vector d r
v = Matrix (d + 1) (d + 1) r -> Transformation d r
forall (d :: Nat) r. Matrix (d + 1) (d + 1) r -> Transformation d r
Transformation (Matrix (d + 1) (d + 1) r -> Transformation d r)
-> (Vector (d + 1) (Vector (d + 1) r) -> Matrix (d + 1) (d + 1) r)
-> Vector (d + 1) (Vector (d + 1) r)
-> Transformation d r
forall b c a. (b -> c) -> (a -> b) -> a -> c
. Vector (d + 1) (Vector (d + 1) r) -> Matrix (d + 1) (d + 1) r
forall (n :: Nat) (m :: Nat) r.
Vector n (Vector m r) -> Matrix n m r
Matrix (Vector (d + 1) (Vector (d + 1) r) -> Transformation d r)
-> Vector (d + 1) (Vector (d + 1) r) -> Transformation d r
forall a b. (a -> b) -> a -> b
\$ (Int -> r -> Vector (d + 1) r)
-> Vector (d + 1) r -> Vector (d + 1) (Vector (d + 1) r)
forall i (f :: * -> *) a b.
FunctorWithIndex i f =>
(i -> a -> b) -> f a -> f b
imap Int -> r -> Vector (d + 1) r
forall (d :: Nat) r. (Arity d, Num r) => Int -> r -> Vector d r
mkRow (Vector d r -> r -> Vector (d + 1) r
forall (d :: Nat) r.
(Arity (d + 1), Arity d) =>
Vector d r -> r -> Vector (d + 1) r
snoc Vector d r
v r
1)

-- | Create scaling transformation from a scalar that is applied
--   to all dimensions.
--
-- >>> transformBy (uniformScaling 5) \$ Point2 2 3
-- Point2 10.0 15.0
-- >>> uniformScaling 5 == scaling (Vector2 5 5)
-- True
-- >>> uniformScaling 5 == scaling (Vector3 5 5 5)
-- True
uniformScaling :: (Num r, Arity d, Arity (d + 1)) => r -> Transformation d r
uniformScaling :: r -> Transformation d r
uniformScaling = Vector d r -> Transformation d r
forall r (d :: Nat).
(Num r, Arity d, Arity (d + 1)) =>
Vector d r -> Transformation d r
scaling (Vector d r -> Transformation d r)
-> (r -> Vector d r) -> r -> Transformation d r
forall b c a. (b -> c) -> (a -> b) -> a -> c
. r -> Vector d r
forall (f :: * -> *) a. Applicative f => a -> f a
pure

--------------------------------------------------------------------------------
-- * Functions that execute transformations

-- | Translate a given point.
--
-- >>> translateBy (Vector2 1 2) \$ Point2 2 3
-- Point2 3.0 5.0
translateBy :: ( IsTransformable g, Num (NumType g)
, Arity (Dimension g), Arity (Dimension g + 1)
) => Vector (Dimension g) (NumType g) -> g -> g
translateBy :: Vector (Dimension g) (NumType g) -> g -> g
translateBy = Transformation (Dimension g) (NumType g) -> g -> g
forall g.
IsTransformable g =>
Transformation (Dimension g) (NumType g) -> g -> g
transformBy (Transformation (Dimension g) (NumType g) -> g -> g)
-> (Vector (Dimension g) (NumType g)
-> Transformation (Dimension g) (NumType g))
-> Vector (Dimension g) (NumType g)
-> g
-> g
forall b c a. (b -> c) -> (a -> b) -> a -> c
. Vector (Dimension g) (NumType g)
-> Transformation (Dimension g) (NumType g)
forall r (d :: Nat).
(Num r, Arity d, Arity (d + 1)) =>
Vector d r -> Transformation d r
translation

-- | Scale a given point.
--
-- >>> scaleBy (Vector2 2 (-1)) \$ Point2 2 3
-- Point2 4.0 (-3.0)
scaleBy :: ( IsTransformable g, Num (NumType g)
, Arity (Dimension g), Arity (Dimension g + 1)
) => Vector (Dimension g) (NumType g) -> g -> g
scaleBy :: Vector (Dimension g) (NumType g) -> g -> g
scaleBy = Transformation (Dimension g) (NumType g) -> g -> g
forall g.
IsTransformable g =>
Transformation (Dimension g) (NumType g) -> g -> g
transformBy (Transformation (Dimension g) (NumType g) -> g -> g)
-> (Vector (Dimension g) (NumType g)
-> Transformation (Dimension g) (NumType g))
-> Vector (Dimension g) (NumType g)
-> g
-> g
forall b c a. (b -> c) -> (a -> b) -> a -> c
. Vector (Dimension g) (NumType g)
-> Transformation (Dimension g) (NumType g)
forall r (d :: Nat).
(Num r, Arity d, Arity (d + 1)) =>
Vector d r -> Transformation d r
scaling

-- | Scale a given point uniformly in all dimensions.
--
-- >>> scaleUniformlyBy 5 \$ Point2 2 3
-- Point2 10.0 15.0
scaleUniformlyBy :: ( IsTransformable g, Num (NumType g)
, Arity (Dimension g), Arity (Dimension g + 1)
) => NumType g -> g -> g
scaleUniformlyBy :: NumType g -> g -> g
scaleUniformlyBy = Transformation (Dimension g) (NumType g) -> g -> g
forall g.
IsTransformable g =>
Transformation (Dimension g) (NumType g) -> g -> g
transformBy  (Transformation (Dimension g) (NumType g) -> g -> g)
-> (NumType g -> Transformation (Dimension g) (NumType g))
-> NumType g
-> g
-> g
forall b c a. (b -> c) -> (a -> b) -> a -> c
. NumType g -> Transformation (Dimension g) (NumType g)
forall r (d :: Nat).
(Num r, Arity d, Arity (d + 1)) =>
r -> Transformation d r
uniformScaling

-- | Row in a translation matrix
-- transRow     :: forall n r. ( Arity n, Arity (n- 1), ((n - 1) + 1) ~ n
--                             , Num r) => Int -> r -> Vector n r
-- transRow i x = set (V.element (Proxy :: Proxy (n-1))) x \$ mkRow i 1

transRow     :: forall n r. (Arity n, Arity (n + 1), Num r)
=> Int -> r -> Vector (n + 1) r
transRow :: Int -> r -> Vector (n + 1) r
transRow Int
i r
x = ASetter (Vector (n + 1) r) (Vector (n + 1) r) r r
-> r -> Vector (n + 1) r -> Vector (n + 1) r
forall s t a b. ASetter s t a b -> b -> s -> t
set (Proxy n -> Lens' (Vector (n + 1) r) r
forall (proxy :: Nat -> *) (i :: Nat) (d :: Nat) r.
(Arity d, KnownNat i, (i + 1) <= d) =>
proxy i -> Lens' (Vector d r) r
V.element (Proxy n
forall k (t :: k). Proxy t
Proxy :: Proxy n)) r
x (Vector (n + 1) r -> Vector (n + 1) r)
-> Vector (n + 1) r -> Vector (n + 1) r
forall a b. (a -> b) -> a -> b
\$ Int -> r -> Vector (n + 1) r
forall (d :: Nat) r. (Arity d, Num r) => Int -> r -> Vector d r
mkRow Int
i r
1

--------------------------------------------------------------------------------
-- * 3D Rotations

-- | Given three new unit-length basis vectors (u,v,w) that map to (x,y,z),
-- construct the appropriate rotation that does this.
--
--
rotateTo                 :: Num r => Vector 3 (Vector 3 r) -> Transformation 3 r
rotateTo :: Vector 3 (Vector 3 r) -> Transformation 3 r
rotateTo (Vector3 Vector 3 r
u Vector 3 r
v Vector 3 r
w) = Matrix 4 4 r -> Transformation 3 r
forall (d :: Nat) r. Matrix (d + 1) (d + 1) r -> Transformation d r
Transformation (Matrix 4 4 r -> Transformation 3 r)
-> (Vector 4 (Vector 4 r) -> Matrix 4 4 r)
-> Vector 4 (Vector 4 r)
-> Transformation 3 r
forall b c a. (b -> c) -> (a -> b) -> a -> c
. Vector 4 (Vector 4 r) -> Matrix 4 4 r
forall (n :: Nat) (m :: Nat) r.
Vector n (Vector m r) -> Matrix n m r
Matrix (Vector 4 (Vector 4 r) -> Transformation 3 r)
-> Vector 4 (Vector 4 r) -> Transformation 3 r
forall a b. (a -> b) -> a -> b
\$ Vector 4 r
-> Vector 4 r -> Vector 4 r -> Vector 4 r -> Vector 4 (Vector 4 r)
forall r. r -> r -> r -> r -> Vector 4 r
Vector4 (Vector 3 r -> r -> Vector (3 + 1) r
forall (d :: Nat) r.
(Arity (d + 1), Arity d) =>
Vector d r -> r -> Vector (d + 1) r
snoc Vector 3 r
u        r
0)
(Vector 3 r -> r -> Vector (3 + 1) r
forall (d :: Nat) r.
(Arity (d + 1), Arity d) =>
Vector d r -> r -> Vector (d + 1) r
snoc Vector 3 r
v        r
0)
(Vector 3 r -> r -> Vector (3 + 1) r
forall (d :: Nat) r.
(Arity (d + 1), Arity d) =>
Vector d r -> r -> Vector (d + 1) r
snoc Vector 3 r
w        r
0)
(r -> r -> r -> r -> Vector 4 r
forall r. r -> r -> r -> r -> Vector 4 r
Vector4 r
0 r
0 r
0 r
1)

--------------------------------------------------------------------------------
-- * 2D Transformations

-- | Skew transformation that keeps the y-coordinates fixed and shifts
-- the x coordinates.
skewX        :: Num r => r -> Transformation 2 r
skewX :: r -> Transformation 2 r
skewX  = Matrix 3 3 r -> Transformation 2 r
forall (d :: Nat) r. Matrix (d + 1) (d + 1) r -> Transformation d r
Transformation (Matrix 3 3 r -> Transformation 2 r)
-> (Vector 3 (Vector 3 r) -> Matrix 3 3 r)
-> Vector 3 (Vector 3 r)
-> Transformation 2 r
forall b c a. (b -> c) -> (a -> b) -> a -> c
. Vector 3 (Vector 3 r) -> Matrix 3 3 r
forall (n :: Nat) (m :: Nat) r.
Vector n (Vector m r) -> Matrix n m r
Matrix (Vector 3 (Vector 3 r) -> Transformation 2 r)
-> Vector 3 (Vector 3 r) -> Transformation 2 r
forall a b. (a -> b) -> a -> b
\$ Vector 3 r -> Vector 3 r -> Vector 3 r -> Vector 3 (Vector 3 r)
forall r. r -> r -> r -> Vector 3 r
Vector3 (r -> r -> r -> Vector 3 r
forall r. r -> r -> r -> Vector 3 r
Vector3 r
1  r
0)
(r -> r -> r -> Vector 3 r
forall r. r -> r -> r -> Vector 3 r
Vector3 r
0 r
1      r
0)
(r -> r -> r -> Vector 3 r
forall r. r -> r -> r -> Vector 3 r
Vector3 r
0 r
0      r
1)