From 342cff97ddc96fdfde2c4271e978650e4bb73c4f Mon Sep 17 00:00:00 2001
From: Kiana Sheibani <kiana.a.sheibani@gmail.com>
Date: Sat, 15 Oct 2022 16:09:32 -0400
Subject: [PATCH] Add utility functions for Transform types

---
 src/Data/NumIdr/Homogeneous.idr           | 20 +++++
 src/Data/NumIdr/Matrix.idr                | 18 +++++
 src/Data/NumIdr/Transform.idr             | 13 ++++
 src/Data/NumIdr/Transform/Affine.idr      | 23 ++++++
 src/Data/NumIdr/Transform/Isometry.idr    | 17 +++++
 src/Data/NumIdr/Transform/Linear.idr      | 32 ++++++++
 src/Data/NumIdr/Transform/Orthonormal.idr | 51 +++++++++++++
 src/Data/NumIdr/Transform/Point.idr       | 23 +++---
 src/Data/NumIdr/Transform/Rigid.idr       | 17 +++++
 src/Data/NumIdr/Transform/Rotation.idr    | 22 ++++++
 src/Data/NumIdr/Transform/Transform.idr   | 91 ++++++++++++++++++++---
 src/Data/NumIdr/Transform/Translation.idr | 31 ++++----
 src/Data/NumIdr/Transform/Trivial.idr     | 23 ++++++
 13 files changed, 341 insertions(+), 40 deletions(-)
 create mode 100644 src/Data/NumIdr/Transform.idr
 create mode 100644 src/Data/NumIdr/Transform/Affine.idr
 create mode 100644 src/Data/NumIdr/Transform/Isometry.idr
 create mode 100644 src/Data/NumIdr/Transform/Linear.idr
 create mode 100644 src/Data/NumIdr/Transform/Orthonormal.idr
 create mode 100644 src/Data/NumIdr/Transform/Rigid.idr
 create mode 100644 src/Data/NumIdr/Transform/Trivial.idr

diff --git a/src/Data/NumIdr/Homogeneous.idr b/src/Data/NumIdr/Homogeneous.idr
index 16b4708..ca78360 100644
--- a/src/Data/NumIdr/Homogeneous.idr
+++ b/src/Data/NumIdr/Homogeneous.idr
@@ -79,6 +79,14 @@ matrixToH {m,n} mat with (viewShape mat)
   _ | Shape [m,n] = indexSet [last,last] 1 $ resize [S m, S n] 0 mat
 
 
+export
+isHMatrix : (Eq a, Num a) => Matrix m n a -> Bool
+isHMatrix {m,n} mat with (viewShape mat)
+  isHMatrix {m=Z,n} mat | Shape [Z,n] = False
+  isHMatrix {m=S m,n=Z} mat | Shape [S m,Z] = False
+  isHMatrix {m=S m,n=S n} mat | Shape [S m,S n] =
+    getRow last mat == resize _ 1 (zeros [n])
+
 
 export
 getMatrix : HMatrix m n a -> Matrix m n a
@@ -110,3 +118,15 @@ translationH {n} v with (viewShape v)
 export
 rotation2DH : Double -> HMatrix' 2 Double
 rotation2DH = matrixToH . rotation2D
+
+export
+rotation3DXH : Double -> HMatrix' 3 Double
+rotation3DXH = matrixToH . rotation3DX
+
+export
+rotation3DYH : Double -> HMatrix' 3 Double
+rotation3DYH = matrixToH . rotation3DY
+
+export
+rotation3DZH : Double -> HMatrix' 3 Double
+rotation3DZH = matrixToH . rotation3DZ
diff --git a/src/Data/NumIdr/Matrix.idr b/src/Data/NumIdr/Matrix.idr
index 277d7a7..e336eca 100644
--- a/src/Data/NumIdr/Matrix.idr
+++ b/src/Data/NumIdr/Matrix.idr
@@ -80,6 +80,19 @@ rotation2D : Double -> Matrix' 2 Double
 rotation2D a = matrix [[cos a, - sin a], [sin a, cos a]]
 
 
+export
+rotation3DX : Double -> Matrix' 3 Double
+rotation3DX a = matrix [[1,0,0], [0, cos a, - sin a], [0, sin a, cos a]]
+
+export
+rotation3DY : Double -> Matrix' 3 Double
+rotation3DY a = matrix [[cos a, 0, sin a], [0,1,0], [- sin a, 0, cos a]]
+
+export
+rotation3DZ : Double -> Matrix' 3 Double
+rotation3DZ a = matrix [[cos a, - sin a, 0], [sin a, cos a, 0], [0,0,1]]
+
+
 --------------------------------------------------------------------------------
 -- Indexing
 --------------------------------------------------------------------------------
@@ -573,6 +586,11 @@ solve mat = solveWithLUP mat (decompLUP mat)
 --------------------------------------------------------------------------------
 
 
+export
+invertible : FieldCmp a => Matrix' n a -> Bool
+invertible {n} mat with (viewShape mat)
+  _ | Shape [n,n] = let lup = decompLUP mat in all (/=0) (diagonal lup.lu)
+
 ||| Try to invert a square matrix, returning `Nothing` if an inverse
 ||| does not exist.
 export
diff --git a/src/Data/NumIdr/Transform.idr b/src/Data/NumIdr/Transform.idr
new file mode 100644
index 0000000..48309f2
--- /dev/null
+++ b/src/Data/NumIdr/Transform.idr
@@ -0,0 +1,13 @@
+module Data.NumIdr.Transform
+
+import public Data.NumIdr.Transform.Point
+import public Data.NumIdr.Transform.Transform
+
+import public Data.NumIdr.Transform.Affine
+import public Data.NumIdr.Transform.Isometry
+import public Data.NumIdr.Transform.Linear
+import public Data.NumIdr.Transform.Orthonormal
+import public Data.NumIdr.Transform.Rigid
+import public Data.NumIdr.Transform.Rotation
+import public Data.NumIdr.Transform.Translation
+import public Data.NumIdr.Transform.Trivial
diff --git a/src/Data/NumIdr/Transform/Affine.idr b/src/Data/NumIdr/Transform/Affine.idr
new file mode 100644
index 0000000..7aaab74
--- /dev/null
+++ b/src/Data/NumIdr/Transform/Affine.idr
@@ -0,0 +1,23 @@
+module Data.NumIdr.Transform.Affine
+
+import Data.Vect
+import Data.NumIdr.Interfaces
+import Data.NumIdr.Array
+import Data.NumIdr.Vector
+import Data.NumIdr.Matrix
+import Data.NumIdr.Homogeneous
+import Data.NumIdr.Transform.Point
+import Data.NumIdr.Transform.Transform
+
+%default total
+
+
+public export
+Affine : Nat -> Type -> Type
+Affine = Transform TAffine
+
+export
+fromHMatrix : FieldCmp a => HMatrix' n a -> Maybe (Affine n a)
+fromHMatrix mat = if invertible (getMatrix mat)
+                  then Just (unsafeMkTrans mat)
+                  else Nothing
diff --git a/src/Data/NumIdr/Transform/Isometry.idr b/src/Data/NumIdr/Transform/Isometry.idr
new file mode 100644
index 0000000..726fb07
--- /dev/null
+++ b/src/Data/NumIdr/Transform/Isometry.idr
@@ -0,0 +1,17 @@
+module Data.NumIdr.Transform.Isometry
+
+import Data.Vect
+import Data.NumIdr.Interfaces
+import Data.NumIdr.Array
+import Data.NumIdr.Vector
+import Data.NumIdr.Matrix
+import Data.NumIdr.Homogeneous
+import Data.NumIdr.Transform.Point
+import Data.NumIdr.Transform.Transform
+
+%default total
+
+
+public export
+Isometry : Nat -> Type -> Type
+Isometry = Transform TIsometry
diff --git a/src/Data/NumIdr/Transform/Linear.idr b/src/Data/NumIdr/Transform/Linear.idr
new file mode 100644
index 0000000..d58142c
--- /dev/null
+++ b/src/Data/NumIdr/Transform/Linear.idr
@@ -0,0 +1,32 @@
+module Data.NumIdr.Transform.Linear
+
+import Data.Vect
+import Data.NumIdr.Interfaces
+import Data.NumIdr.Array
+import Data.NumIdr.Vector
+import Data.NumIdr.Matrix
+import Data.NumIdr.Homogeneous
+import Data.NumIdr.Transform.Point
+import Data.NumIdr.Transform.Transform
+
+%default total
+
+
+public export
+Linear : Nat -> Type -> Type
+Linear = Transform TLinear
+
+
+export
+isLinear : FieldCmp a => HMatrix' n a -> Bool
+isLinear mat = isHMatrix mat && invertible (getMatrix mat)
+                    && all (==0) (getTranslationVector mat)
+
+export
+fromHMatrix : FieldCmp a => HMatrix' n a -> Maybe (Linear n a)
+fromHMatrix mat = if isLinear mat then Just (unsafeMkTrans mat) else Nothing
+
+export
+fromMatrix : FieldCmp a => Matrix' n a -> Maybe (Linear n a)
+fromMatrix mat = if invertible mat then Just (unsafeMkTrans (matrixToH mat))
+                                   else Nothing
diff --git a/src/Data/NumIdr/Transform/Orthonormal.idr b/src/Data/NumIdr/Transform/Orthonormal.idr
new file mode 100644
index 0000000..e63e21d
--- /dev/null
+++ b/src/Data/NumIdr/Transform/Orthonormal.idr
@@ -0,0 +1,51 @@
+module Data.NumIdr.Transform.Orthonormal
+
+import Data.Vect
+import Data.NumIdr.Interfaces
+import Data.NumIdr.Array
+import Data.NumIdr.Vector
+import Data.NumIdr.Matrix
+import Data.NumIdr.Homogeneous
+import Data.NumIdr.Transform.Point
+import Data.NumIdr.Transform.Transform
+
+%default total
+
+
+public export
+Orthonormal : Nat -> Type -> Type
+Orthonormal = Transform TOrthonormal
+
+
+export
+isOrthonormal : Eq a => Num a => Matrix' n a -> Bool
+isOrthonormal {n} mat with (viewShape mat)
+  _ | Shape [n,n] = identity == fromFunction [n,n] (\[i,j] => getColumn i mat `dot` getColumn j mat)
+
+export
+fromMatrix : Eq a => Num a => Matrix' n a -> Maybe (Orthonormal n a)
+fromMatrix mat = if isOrthonormal mat then Just (unsafeMkTrans (matrixToH mat))
+                                     else Nothing
+
+
+export
+reflect : {n : _} -> Neg a => Fin n -> Orthonormal n a
+reflect i = unsafeMkTrans $ indexSet [weaken i,weaken i] (-1) identity
+
+export
+reflectX : {n : _} -> Neg a => Orthonormal (1 + n) a
+reflectX = reflect 0
+
+export
+reflectY : {n : _} -> Neg a => Orthonormal (2 + n) a
+reflectY = reflect 1
+
+export
+reflectZ : {n : _} -> Neg a => Orthonormal (3 + n) a
+reflectZ = reflect 2
+
+
+export
+reflectNormal : (Neg a, Fractional a) => Vector n a -> Orthonormal n a
+reflectNormal {n} v with (viewShape v)
+  _ | Shape [n] = unsafeMkTrans $ matrixToH $ (identity - (2 / normSq v) *. outer v v)
diff --git a/src/Data/NumIdr/Transform/Point.idr b/src/Data/NumIdr/Transform/Point.idr
index b7cdf95..7979f5b 100644
--- a/src/Data/NumIdr/Transform/Point.idr
+++ b/src/Data/NumIdr/Transform/Point.idr
@@ -92,21 +92,22 @@ export
 
 
 -- Affine space operations
+-- These seem to cause issues with interface resolution
 
-namespace Left
-  export
-  (+) : Num a => Vector n a -> Point n a -> Point n a
-  a + MkPoint b = MkPoint (zipWith (+) a b)
+-- namespace Left
+--   export
+--   (+) : Num a => Vector n a -> Point n a -> Point n a
+--   a + MkPoint b = MkPoint (zipWith (+) a b)
 
-namespace Right
-  export
-  (+) : Num a => Point n a -> Vector n a -> Point n a
-  MkPoint a + b = MkPoint (zipWith (+) a b)
+-- namespace Right
+--   export
+--   (+) : Num a => Point n a -> Vector n a -> Point n a
+--   MkPoint a + b = MkPoint (zipWith (+) a b)
 
 
-export
-(-) : Neg a => Point n a -> Point n a -> Vector n a
-MkPoint a - MkPoint b = zipWith (-) a b
+-- export
+-- (-) : Neg a => Point n a -> Point n a -> Vector n a
+-- MkPoint a - MkPoint b = zipWith (-) a b
 
 
 --------------------------------------------------------------------------------
diff --git a/src/Data/NumIdr/Transform/Rigid.idr b/src/Data/NumIdr/Transform/Rigid.idr
new file mode 100644
index 0000000..d89b2b1
--- /dev/null
+++ b/src/Data/NumIdr/Transform/Rigid.idr
@@ -0,0 +1,17 @@
+module Data.NumIdr.Transform.Rigid
+
+import Data.Vect
+import Data.NumIdr.Interfaces
+import Data.NumIdr.Array
+import Data.NumIdr.Vector
+import Data.NumIdr.Matrix
+import Data.NumIdr.Homogeneous
+import Data.NumIdr.Transform.Point
+import Data.NumIdr.Transform.Transform
+
+%default total
+
+
+public export
+Rigid : Nat -> Type -> Type
+Rigid = Transform TRigid
diff --git a/src/Data/NumIdr/Transform/Rotation.idr b/src/Data/NumIdr/Transform/Rotation.idr
index e69de29..1a1812b 100644
--- a/src/Data/NumIdr/Transform/Rotation.idr
+++ b/src/Data/NumIdr/Transform/Rotation.idr
@@ -0,0 +1,22 @@
+module Data.NumIdr.Transform.Rotation
+
+import Data.Vect
+import Data.NumIdr.Interfaces
+import Data.NumIdr.Array
+import Data.NumIdr.Vector
+import Data.NumIdr.Matrix
+import Data.NumIdr.Homogeneous
+import Data.NumIdr.Transform.Point
+import Data.NumIdr.Transform.Transform
+
+%default total
+
+
+public export
+Rotation : Nat -> Type -> Type
+Rotation = Transform TRotation
+
+
+export
+isRotation' : Matrix' n a -> Bool
+isRotation' mat =
diff --git a/src/Data/NumIdr/Transform/Transform.idr b/src/Data/NumIdr/Transform/Transform.idr
index 0a0d594..9e22769 100644
--- a/src/Data/NumIdr/Transform/Transform.idr
+++ b/src/Data/NumIdr/Transform/Transform.idr
@@ -8,10 +8,32 @@ import Data.NumIdr.Matrix
 import Data.NumIdr.Homogeneous
 import Data.NumIdr.Transform.Point
 
+%default total
+
+
+--------------------------------------------------------------------------------
+-- Transformation Types
+--------------------------------------------------------------------------------
+
+
 public export
 data TransType = TAffine | TIsometry | TRigid | TTranslation
                | TLinear | TOrthonormal | TRotation | TTrivial
 
+%name TransType ty
+
+
+public export
+Show TransType where
+  show TAffine = "TAffine"
+  show TIsometry = "TIsometry"
+  show TRigid = "TRigid"
+  show TTranslation = "TTranslation"
+  show TLinear = "TLinear"
+  show TOrthonormal = "TOrthonormal"
+  show TRotation = "TRotation"
+  show TTrivial = "TTrivial"
+
 
 -- Lower numbers can be coerced to higher numbers
 toSignature : TransType -> (Fin 4, Bool)
@@ -35,6 +57,12 @@ fromSignature (2, False) = TOrthonormal
 fromSignature (1, False) = TRotation
 fromSignature (0, False) = TTrivial
 
+
+--------------------------------------------------------------------------------
+-- Transformation type operations
+--------------------------------------------------------------------------------
+
+
 public export
 (:<) : TransType -> TransType -> Bool
 x :< y with (toSignature x, toSignature y)
@@ -46,33 +74,74 @@ transMult x y with (toSignature x, toSignature y)
   _ | ((xn, xb), (yn, yb)) = fromSignature (max xn yn, xb && yb)
 
 public export
-linearize : TransType -> TransType
-linearize = fromSignature . mapSnd (const False) . toSignature
+linearizeType : TransType -> TransType
+linearizeType = fromSignature . mapSnd (const False) . toSignature
+
 
 export
 data Transform : TransType -> Nat -> Type -> Type where
   MkTrans : (type : TransType) -> HMatrix' n a -> Transform type n a
 
+%name Transform t
+
+
+export
+unsafeMkTrans : {ty : _} -> HMatrix' n a -> Transform ty n a
+unsafeMkTrans = MkTrans _
+
+export
+toHMatrix : Transform ty n a -> HMatrix' n a
+toHMatrix (MkTrans _ mat) = mat
+
+export
+linearize : Num a => Transform ty n a -> Transform (linearizeType ty) n a
+linearize {n} (MkTrans _ mat) with (viewShape mat)
+  _ | Shape [S n,S n] = MkTrans _ (hmatrix (getMatrix mat) (zeros _))
+
+
+--------------------------------------------------------------------------------
+-- Interface implementations
+--------------------------------------------------------------------------------
+
 
 mulPoint : Num a => HMatrix' n a -> Point n a -> Point n a
-mulPoint m p = fromVector $ zipWith (+) (getMatrix m *. toVector p)
-                                        (getTranslationVector m)
+mulPoint mat p = fromVector $ zipWith (+) (getMatrix mat *. toVector p)
+                                          (getTranslationVector mat)
 
 mulVector : Num a => HMatrix' n a -> Vector n a -> Vector n a
-mulVector m v = getMatrix m *. v
+mulVector mat v = getMatrix mat *. v
 
 export
-Num a => Mult (Transform type n a) (Point n a) (Point n a) where
-  MkTrans _ m *. p = mulPoint m p
+Num a => Mult (Transform ty n a) (Point n a) (Point n a) where
+  MkTrans _ mat *. p = mulPoint mat p
 
 export
-Num a => Mult (Transform type n a) (Vector n a) (Vector n a) where
-  MkTrans _ m *. v = mulVector m v
+Num a => Mult (Transform ty n a) (Vector n a) (Vector n a) where
+  MkTrans _ mat *. v = mulVector mat v
+
 
 export
 Num a => Mult (Transform t1 n a) (Transform t2 n a) (Transform (transMult t1 t2) n a) where
   MkTrans _ m1 *. MkTrans _ m2 = MkTrans _ (m1 *. m2)
 
 export
-{t2 : _} -> So (t1 :< t2) => Cast (Transform t1 n a) (Transform t2 n a) where
-  cast (MkTrans t1 m) = MkTrans t2 m
+[TransformMult'] Num a => Mult (Transform ty n a) (Transform ty n a) (Transform ty n a) where
+  MkTrans _ m1 *. MkTrans _ m2 = MkTrans _ (m1 *. m2)
+
+
+export
+{n,ty : _} -> Num a => MultMonoid (Transform ty n a) using TransformMult' where
+  identity = MkTrans ty identity
+
+export
+{n,ty : _} -> FieldCmp a => MultGroup (Transform ty n a) where
+  inverse (MkTrans _ mat) = MkTrans _ (inverse mat)
+
+
+export
+{t2 : _} -> So (t1 :< t2) => Cast a b => Cast (Transform t1 n a) (Transform t2 n b) where
+  cast (MkTrans t1 mat) = MkTrans t2 (cast mat)
+
+export
+Show a => Show (Transform ty n a) where
+  showPrec p (MkTrans ty mat) = showCon p "MkTrans" $ showArg ty ++ showArg mat
diff --git a/src/Data/NumIdr/Transform/Translation.idr b/src/Data/NumIdr/Transform/Translation.idr
index 3cb8404..3175989 100644
--- a/src/Data/NumIdr/Transform/Translation.idr
+++ b/src/Data/NumIdr/Transform/Translation.idr
@@ -4,33 +4,28 @@ import Data.Vect
 import Data.NumIdr.Interfaces
 import Data.NumIdr.Array
 import Data.NumIdr.Vector
+import Data.NumIdr.Matrix
+import Data.NumIdr.Homogeneous
 import Data.NumIdr.Transform.Point
+import Data.NumIdr.Transform.Transform
 
 %default total
 
 
-export
-record Translation n a where
-  constructor MkTrl
-  offset : Vector n a
-
-export
-fromVector : Vector n a -> Translation n a
-fromVector = MkTrl
+public export
+Translation : Nat -> Type -> Type
+Translation = Transform TTranslation
 
 
 export
-Cast a b => Cast (Translation n a) (Translation n b) where
-  cast (MkTrl v) = MkTrl (cast v)
+isTranslation : (Eq a, Num a) => HMatrix' n a -> Bool
+isTranslation {n} mat with (viewShape mat)
+  _ | Shape [S n,S n] = isHMatrix mat && getMatrix mat == identity
 
 export
-Num a => Mult (Translation n a) (Translation n a) (Translation n a) where
-  MkTrl a *. MkTrl b = MkTrl (zipWith (+) a b)
+fromVector : Num a => Vector n a -> Translation n a
+fromVector v = unsafeMkTrans (translationH v)
 
 export
-{n : _} -> Num a => MultMonoid (Translation n a) where
-  identity = MkTrl $ zeros [n]
-
-export
-{n : _} -> Neg a => MultGroup (Translation n a) where
-  inverse (MkTrl v) = MkTrl (-v)
+fromHMatrix : (Eq a, Num a) => HMatrix' n a -> Maybe (Translation n a)
+fromHMatrix mat = if isTranslation mat then Just (unsafeMkTrans mat) else Nothing
diff --git a/src/Data/NumIdr/Transform/Trivial.idr b/src/Data/NumIdr/Transform/Trivial.idr
new file mode 100644
index 0000000..30ee812
--- /dev/null
+++ b/src/Data/NumIdr/Transform/Trivial.idr
@@ -0,0 +1,23 @@
+module Data.NumIdr.Transform.Trivial
+
+import Data.Vect
+import Data.NumIdr.Interfaces
+import Data.NumIdr.Array
+import Data.NumIdr.Vector
+import Data.NumIdr.Matrix
+import Data.NumIdr.Homogeneous
+import Data.NumIdr.Transform.Point
+import Data.NumIdr.Transform.Transform
+
+%default total
+
+
+public export
+Trivial : Nat -> Type -> Type
+Trivial = Transform TTrivial
+
+
+export
+isTrivial : (Eq a, Num a) => HMatrix' n a -> Bool
+isTrivial {n} mat with (viewShape mat)
+  _ | Shape [S n,S n] = mat == identity