From 13d5f7aa21bc787b5db3795846c82e039a0248c2 Mon Sep 17 00:00:00 2001
From: Kiana Sheibani <kiana.a.sheibani@gmail.com>
Date: Fri, 9 Dec 2022 15:29:40 -0500
Subject: [PATCH] Implmement HMatrix to Rigid conversion

---
 src/Data/NumIdr/Array/Coords.idr          |  2 +-
 src/Data/NumIdr/Transform/Orthonormal.idr | 12 ++++++++++++
 src/Data/NumIdr/Transform/Rigid.idr       |  4 ++++
 src/Data/NumIdr/Transform/Rotation.idr    |  1 +
 src/Data/NumIdr/Vector.idr                | 15 +++++++++++++++
 5 files changed, 33 insertions(+), 1 deletion(-)

diff --git a/src/Data/NumIdr/Array/Coords.idr b/src/Data/NumIdr/Array/Coords.idr
index d47f182..4ad1304 100644
--- a/src/Data/NumIdr/Array/Coords.idr
+++ b/src/Data/NumIdr/Array/Coords.idr
@@ -9,7 +9,7 @@ import Data.NP
 
 
 -- A Nat-based range function with better semantics
-export
+public export
 range : Nat -> Nat -> List Nat
 range x y = if x < y then assert_total $ takeBefore (>= y) (countFrom x S)
                      else []
diff --git a/src/Data/NumIdr/Transform/Orthonormal.idr b/src/Data/NumIdr/Transform/Orthonormal.idr
index 263e04b..f5b76a0 100644
--- a/src/Data/NumIdr/Transform/Orthonormal.idr
+++ b/src/Data/NumIdr/Transform/Orthonormal.idr
@@ -32,6 +32,18 @@ fromMatrix mat = if isOrthonormal' mat then Just (unsafeMkTrans (matrixToH mat))
                                        else Nothing
 
 
+||| Determine if a homogeneous matrix represents a rotation.
+export
+isOrthonormal : Eq a => Num a => HMatrix' n a -> Bool
+isOrthonormal {n} mat with (viewShape mat)
+  _ | Shape [S n, S n] = isHMatrix mat && all (==0) (mat !!.. [EndBound last, One last])
+                            && isOrthonormal' (getMatrix mat)
+
+export
+fromHMatrix : Eq a => Num a => HMatrix' n a -> Maybe (Orthonormal n a)
+fromHMatrix mat = if isOrthonormal mat then Just (unsafeMkTrans mat) else Nothing
+
+
 --------------------------------------------------------------------------------
 -- Reflections
 --------------------------------------------------------------------------------
diff --git a/src/Data/NumIdr/Transform/Rigid.idr b/src/Data/NumIdr/Transform/Rigid.idr
index 7a6de96..4984cb1 100644
--- a/src/Data/NumIdr/Transform/Rigid.idr
+++ b/src/Data/NumIdr/Transform/Rigid.idr
@@ -26,6 +26,10 @@ export
 isRigid : FieldCmp a => HMatrix' n a -> Bool
 isRigid mat = isHMatrix mat && isRotation' (getMatrix mat)
 
+export
+fromHMatrix : FieldCmp a => HMatrix' n a -> Maybe (Rigid n a)
+fromHMatrix mat = if isRigid mat then Just (unsafeMkTrans mat) else Nothing
+
 
 export
 rigid2D : Vector 2 Double -> Double -> Rigid 2 Double
diff --git a/src/Data/NumIdr/Transform/Rotation.idr b/src/Data/NumIdr/Transform/Rotation.idr
index 34b8fe2..ac818a6 100644
--- a/src/Data/NumIdr/Transform/Rotation.idr
+++ b/src/Data/NumIdr/Transform/Rotation.idr
@@ -35,6 +35,7 @@ export
 isRotation : FieldCmp a => HMatrix' n a -> Bool
 isRotation {n} mat with (viewShape mat)
   _ | Shape [S n, S n] = isHMatrix mat && all (==0) (mat !!.. [EndBound last, One last])
+                            && isRotation' (getMatrix mat)
 
 export
 fromHMatrix : FieldCmp a => HMatrix' n a -> Maybe (Rotation n a)
diff --git a/src/Data/NumIdr/Vector.idr b/src/Data/NumIdr/Vector.idr
index 84ebac1..0adad52 100644
--- a/src/Data/NumIdr/Vector.idr
+++ b/src/Data/NumIdr/Vector.idr
@@ -49,6 +49,21 @@ export
 basis : Num a => {n : _} -> (i : Fin n) -> Vector n a
 basis i = fromFunction _ (\[j] => if i == j then 1 else 0)
 
+||| The first basis vector.
+export
+basisX : {n : _} -> Num a => Vector (1 + n) a
+basisX = basis FZ
+
+||| The second basis vector.
+export
+basisY : {n : _} -> Num a => Vector (2 + n) a
+basisY = basis (FS FZ)
+
+||| The third basis vector.
+export
+basisZ : {n : _} -> Num a => Vector (3 + n) a
+basisZ = basis (FS (FS FZ))
+
 
 ||| Calculate the 2D unit vector with the given angle off the x-axis.
 export