Add new utility functions for homogeneous coords

src/Data/NumIdr/Array/Array.idr

@@ -326,6 +326,15 @@ export
 arr !!.. rs = indexRange rs arr
 
 
+export
+indexUnsafe : Vect rk Nat -> Array {rk} s a -> a
+indexUnsafe is arr = index (getLocation' (strides arr) is) (getPrim arr)
+
+export
+(!#) : Array {rk} s a -> Vect rk Nat -> a
+(!#) = flip indexUnsafe
+
+
 --------------------------------------------------------------------------------
 -- Operations on arrays
 --------------------------------------------------------------------------------
@@ -338,7 +347,7 @@ arr !!.. rs = indexRange rs arr
 ||| @ ord The order to reinterpret the array by
 export
 reshape' : (s' : Vect rk' Nat) -> (ord : Order) -> Array {rk} s a ->
-             product s = product s' => Array s' a
+             (0 ok : product s = product s') => Array s' a
 reshape' s' ord' arr = MkArray ord' (calcStrides ord' s') s' (getPrim arr)
 
 ||| Reshape the array into the given shape.
@@ -346,7 +355,7 @@ reshape' s' ord' arr = MkArray ord' (calcStrides ord' s') s' (getPrim arr)
 ||| @ s' The shape to convert the array to
 export
 reshape : (s' : Vect rk' Nat) -> Array {rk} s a ->
-            product s = product s' => Array s' a
+            (0 ok : product s = product s') => Array s' a
 reshape s' arr = reshape' s' (getOrder arr) arr
 
 

src/Data/NumIdr/Homogeneous.idr

@@ -37,31 +37,75 @@ HVector : Nat -> Type -> Type
 HVector n = Vector (S n)
 
 
-export
+--------------------------------------------------------------------------------
-fromVector : Num a => Vector n a -> HVector n a
+-- Conversion functions
-fromVector v = rewrite plusCommutative 1 n in v ++ vector [1]
+--------------------------------------------------------------------------------
+
 
 export
-fromVectorL : Num a => Vector n a -> HVector n a
+vectorToH : Num a => Vector n a -> HVector n a
-fromVectorL v = rewrite plusCommutative 1 n in v ++ vector [0]
+vectorToH v = rewrite plusCommutative 1 n in v ++ vector [1]
 
 export
-toVector : HVector n a -> Vector n a
+vectorToHLinear : Num a => Vector n a -> HVector n a
-toVector = vector . init . toVect
+vectorToHLinear v = rewrite plusCommutative 1 n in v ++ vector [0]
--- TODO: Find an implementation for `toVector` that doesn't suck
 
+export
+hvector : Num a => Vect n a -> HVector n a
+hvector v = rewrite plusCommutative 1 n in vector (v ++ [1])
+
+export
+hvectorLinear : Num a => Vect n a -> HVector n a
+hvectorLinear v = rewrite plusCommutative 1 n in vector (v ++ [0])
+
+
+export
+fromHomogeneous : HVector n a -> Vector n a
+fromHomogeneous = vector . init . toVect
+-- TODO: Find an implementation for `fromHomogeneous` that doesn't suck
+
+
+export
+hmatrix : Num a => Matrix m n a -> Vector m a -> HMatrix m n a
+hmatrix mat tr with (viewShape mat)
+  _ | Shape [m,n] = indexSet [last,last] 1 $
+                    resize [S m, S n] 0 $
+                    mat `hconcat` reshape
+                      {ok = sym $ multOneRightNeutral _} [m,1] tr
+
+export
+matrixToH : Num a => Matrix m n a -> HMatrix m n a
+matrixToH mat with (viewShape mat)
+  _ | Shape [m,n] = indexSet [last,last] 1 $ resize [S m, S n] 0 mat
 
 
 
 export
-toHomogeneous : Num a => Matrix m n a -> HMatrix m n a
+getMatrix : HMatrix m n a -> Matrix m n a
-toHomogeneous mat with (viewShape mat)
+getMatrix mat with (viewShape mat)
-  _ | Shape [m,n] = indexSet [last, last] 1 $ resize [S m, S n] 0 mat
-
-
-
-export
-toMatrix : HMatrix m n a -> Matrix m n a
-toMatrix mat with (viewShape mat)
   _ | Shape [S m, S n] = resizeLTE [m,n] mat
                           {ok = [lteSuccRight reflexive,lteSuccRight reflexive]}
+
+export
+getTranslationVector : HMatrix m n a -> Vector m a
+getTranslationVector mat with (viewShape mat)
+  _ | Shape [S m, S n] = resizeLTE [m] {ok = [lteSuccRight reflexive]} $
+                          getColumn last mat
+
+
+--------------------------------------------------------------------------------
+-- Constructors of homogeneous matrices
+--------------------------------------------------------------------------------
+
+
+export
+scalingH : Num a => {n : _} -> a -> HMatrix' n a
+scalingH x = indexSet [last,last] 1 $ repeatDiag x 0
+
+export
+translationH : Num a => {n : _} -> Vector n a -> HMatrix' n a
+translationH = hmatrix identity
+
+export
+rotation2DH : Double -> HMatrix' 2 Double
+rotation2DH = matrixToH . rotation2D

src/Data/NumIdr/Matrix.idr

@@ -65,7 +65,7 @@ identity : Num a => {n : _} -> Matrix' n a
 identity = repeatDiag 1 0
 
 
-||| Calculate the matrix that scales a vector by the given value.
+||| Construct the matrix that scales a vector by the given value.
 export
 scaling : Num a => {n : _} -> a -> Matrix' n a
 scaling x = repeatDiag x 0
@@ -104,6 +104,17 @@ getColumn : Fin n -> Matrix m n a -> Vector m a
 getColumn c mat = rewrite sym (minusZeroRight m) in indexRange [All, One c] mat
 
 
+export
+diagonal' : Matrix m n a -> Vector (minimum m n) a
+diagonal' mat with (viewShape mat)
+  _ | Shape [m,n] = fromFunctionNB _ (\[i] => indexUnsafe [i,i] mat)
+
+export
+diagonal : Matrix' n a -> Vector n a
+diagonal mat with (viewShape mat)
+  _ | Shape [n,n] = fromFunctionNB [n] (\[i] => indexUnsafe [i,i] mat)
+
+
 --------------------------------------------------------------------------------
 -- Operations
 --------------------------------------------------------------------------------