0 | module Data.NumIdr.Homogeneous
  1 |
  2 | import Data.Vect
  3 | import Data.NumIdr.Interfaces
  4 | import Data.NumIdr.Vector
  5 | import Data.NumIdr.Matrix
  6 |
  7 | %default total
  8 |
  9 |
 10 |
 11 | ||| A homogeneous matrix type.
 12 | |||
 13 | ||| Homogeneous matrices are matrices that encode an affine transformation, as
 14 | ||| opposed to the more restriced linear transformations of normal matrices.
 15 | ||| Their multiplication is identical to regular matrix multiplication, but the
 16 | ||| extra dimension in each axis allows the homogeneous matrix to store a
 17 | ||| translation vector that is applied during multiplication.
 18 | |||
 19 | ||| ```hmatrix mat tr *. v == mat *. v + tr```
 20 | public export
 21 | HMatrix : Nat -> Nat -> Type -> Type
 22 | HMatrix m n = Matrix (S m) (S n)
 23 |
 24 | ||| A synonym for a square homogeneous matrix.
 25 | public export
 26 | HMatrix' : Nat -> Type -> Type
 27 | HMatrix' n = HMatrix n n
 28 |
 29 | ||| An homogeneous vector type.
 30 | |||
 31 | ||| Homogeneous vectors are vectors intended to be multiplied with homogeneous
 32 | ||| matrices (see `HMatrix`). They have an extra dimension compared to regular
 33 | ||| vectors.
 34 | public export
 35 | HVector : Nat -> Type -> Type
 36 | HVector n = Vector (S n)
 37 |
 38 |
 39 | --------------------------------------------------------------------------------
 40 | -- Conversion functions
 41 | --------------------------------------------------------------------------------
 42 |
 43 |
 44 | ||| Convert a vector to a homogeneous vector.
 45 | export
 46 | vectorToH : Num a => Vector n a -> HVector n a
 47 | vectorToH v = rewrite plusCommutative 1 n in v ++ vector [1]
 48 |
 49 | ||| Convert a vector to a homogeneous vector that ignores the translation
 50 | ||| component of homogeneous matrices.
 51 | export
 52 | vectorToHLinear : Num a => Vector n a -> HVector n a
 53 | vectorToHLinear v = rewrite plusCommutative 1 n in v ++ vector [0]
 54 |
 55 | ||| Construct a homogeneous vector given its coordinates.
 56 | export
 57 | hvector : Num a => Vect n a -> HVector n a
 58 | hvector v = rewrite plusCommutative 1 n in vector (v ++ [1])
 59 |
 60 | ||| Construct a homogeneous vector that ignores translations given its
 61 | ||| coordinates.
 62 | export
 63 | hvectorLinear : Num a => Vect n a -> HVector n a
 64 | hvectorLinear v = rewrite plusCommutative 1 n in vector (v ++ [0])
 65 |
 66 |
 67 | ||| Extract a normal vector from a homogeneous vector.
 68 | export
 69 | fromHomogeneous : HVector n a -> Vector n a
 70 | fromHomogeneous {n} v with (viewShape v)
 71 |   _ | Shape [S n] = resizeLTE [n] v {ok = [lteSuccRight reflexive]}
 72 |
 73 |
 74 | ||| Construct a homogeneous matrix given a matrix and a translation vector.
 75 | export
 76 | hmatrix : Num a => Matrix m n a -> Vector m a -> HMatrix m n a
 77 | hmatrix {m,n} mat tr with (viewShape mat)
 78 |   _ | Shape [m,n] = indexSet [last,last] 1 $
 79 |                     resize [S m, S n] 0 $
 80 |                     mat `hconcat` hstack [tr]
 81 |
 82 | ||| Convert a regular matrix to a homogeneous matrix.
 83 | export
 84 | matrixToH : Num a => Matrix m n a -> HMatrix m n a
 85 | matrixToH {m,n} mat with (viewShape mat)
 86 |   _ | Shape [m,n] = indexSet [last,last] 1 $ resize [S m, S n] 0 mat
 87 |
 88 |
 89 | ||| Determine if a matrix fits the pattern of a homogeneous matrix.
 90 | export
 91 | isHMatrix : Eq a => Num a => Matrix m n a -> Bool
 92 | isHMatrix {m,n} mat with (viewShape mat)
 93 |   isHMatrix {m=Z,n} mat | Shape [Z,n] = False
 94 |   isHMatrix {m=S m,n=Z} mat | Shape [S m,Z] = False
 95 |   isHMatrix {m=S m,n=S n} mat | Shape [S m,S n] =
 96 |     getRow last mat == resize _ 1 (zeros [n])
 97 |
 98 |
 99 | ||| Get the regular matrix component of a homogeneous matrix.
100 | export
101 | getMatrix : HMatrix m n a -> Matrix m n a
102 | getMatrix {m,n} mat with (viewShape mat)
103 |   _ | Shape [S m, S n] = resizeLTE [m,n] mat
104 |                           {ok = [lteSuccRight reflexive,lteSuccRight reflexive]}
105 |
106 | ||| Get the translation vector from a homogeneous matrix.
107 | export
108 | getTranslationVector : HMatrix m n a -> Vector m a
109 | getTranslationVector {m,n} mat with (viewShape mat)
110 |   _ | Shape [S m, S n] = resizeLTE [m] {ok = [lteSuccRight reflexive]} $
111 |                           getColumn last mat
112 |
113 |
114 | --------------------------------------------------------------------------------
115 | -- Constructors of homogeneous matrices
116 | --------------------------------------------------------------------------------
117 |
118 |
119 | ||| Construct a homogeneous matrix that scales a vector by the input.
120 | export
121 | scalingH : {default B rep : Rep} -> RepConstraint rep a =>
122 |             {n : _} -> Num a => a -> HMatrix' n a
123 | scalingH x = indexSet [last,last] 1 $ repeatDiag {rep} x 0
124 |
125 | ||| Construct a homogeneous matrix that translates by the given vector.
126 | export
127 | translationH : Num a => Vector n a -> HMatrix' n a
128 | translationH {n} v with (viewShape v)
129 |   _ | Shape [n] = hmatrix identity v
130 |
131 |
132 | ||| Construct a 2D homogeneous matrix that rotates by the given angle (in radians).
133 | export
134 | rotate2DH : {default B rep : Rep} -> RepConstraint rep Double =>
135 |             Double -> HMatrix' 2 Double
136 | rotate2DH = matrixToH . rotate2D {rep}
137 |
138 | ||| Construct a 3D homogeneous matrix that rotates around the x-axis.
139 | export
140 | rotate3DXH : {default B rep : Rep} -> RepConstraint rep Double =>
141 |               Double -> HMatrix' 3 Double
142 | rotate3DXH = matrixToH . rotate3DX {rep}
143 |
144 | ||| Construct a 3D homogeneous matrix that rotates around the y-axis.
145 | export
146 | rotate3DYH : {default B rep : Rep} -> RepConstraint rep Double =>
147 |               Double -> HMatrix' 3 Double
148 | rotate3DYH = matrixToH . rotate3DY {rep}
149 |
150 | ||| Construct a 3D homogeneous matrix that rotates around the z-axis.
151 | export
152 | rotate3DZH : {default B rep : Rep} -> RepConstraint rep Double =>
153 |               Double -> HMatrix' 3 Double
154 | rotate3DZH = matrixToH . rotate3DZ {rep}
155 |
156 |
157 | export
158 | reflectH : {n : _} -> Neg a => Fin n -> HMatrix' n a
159 | reflectH i = indexSet [weaken i,weaken i] (-1) identity
160 |
161 | export
162 | reflectXH : {n : _} -> Neg a => HMatrix' (1 + n) a
163 | reflectXH = reflectH 0
164 |
165 | export
166 | reflectYH : {n : _} -> Neg a => HMatrix' (2 + n) a
167 | reflectYH = reflectH 1
168 |
169 | export
170 | reflectZH : {n : _} -> Neg a => HMatrix' (3 + n) a
171 | reflectZH = reflectH 2
172 |