1 module polyplex.math.matrix; 2 import polyplex.math; 3 import std.conv; 4 import std.stdio; 5 6 struct Matrix(int Dimensions) { 7 alias GMatrix = typeof(this); 8 float[Dimensions][Dimensions] data; 9 10 this(float[Dimensions][Dimensions] d) { 11 this.data = d; 12 } 13 14 private static float[Dimensions][Dimensions] clear(float val) { 15 float[Dimensions][Dimensions] dat; 16 foreach ( x; 0 .. Dimensions ) { 17 foreach ( y; 0 .. Dimensions ) { 18 dat[x][y] = val; 19 } 20 } 21 return dat; 22 } 23 24 public static GMatrix Identity() { 25 float[Dimensions][Dimensions] data = clear(0); 26 foreach ( i; 0 .. Dimensions ) { 27 data[i][i] = 1f; 28 } 29 return GMatrix(data); 30 } 31 32 static if (Dimensions == 4) 33 public static GMatrix Scaling(Vector3 scale_vec) { 34 GMatrix i = GMatrix.Identity; 35 i.data[0][0] = scale_vec.X; 36 i.data[1][1] = scale_vec.Y; 37 i.data[2][2] = scale_vec.Z; 38 return i; 39 } 40 41 static if(Dimensions == 3) 42 public static GMatrix Scaling(Vector!(float, Dimensions-1) scale_vec) { 43 float[Dimensions][Dimensions] dims = GMatrix.Identity.data; 44 foreach( x; 0 .. Dimensions-1 ) { 45 foreach( y; 0 .. Dimensions-1 ) { 46 if (x == y) dims[x][y] = scale_vec.data[x]; 47 else dims[x][y] = 0; 48 } 49 } 50 dims[Dimensions-1][Dimensions-1] = 1; 51 return GMatrix(dims); 52 } 53 54 static if(Dimensions >= 3) 55 public static GMatrix RotationX(float x_rot) { 56 float[Dimensions][Dimensions] dims = GMatrix.Identity.data; 57 dims[1][1] = Mathf.Cos(x_rot); 58 dims[1][2] = Mathf.Sin(x_rot); 59 dims[2][1] = -Mathf.Sin(x_rot); 60 dims[2][2] = Mathf.Cos(x_rot); 61 return GMatrix(dims); 62 } 63 64 static if(Dimensions >= 3) 65 public static GMatrix RotationY(float y_rot) { 66 float[Dimensions][Dimensions] dims = GMatrix.Identity.data; 67 dims[0][0] = Mathf.Cos(y_rot); 68 dims[0][2] = -Mathf.Sin(y_rot); 69 dims[2][0] = Mathf.Sin(y_rot); 70 dims[2][2] = Mathf.Cos(y_rot); 71 return GMatrix(dims); 72 } 73 74 static if(Dimensions >= 3) 75 public static GMatrix RotationZ(float z_rot) { 76 float[Dimensions][Dimensions] dims = GMatrix.Identity.data; 77 dims[0][0] = Mathf.Cos(z_rot); 78 dims[1][0] = Mathf.Sin(z_rot); 79 dims[0][1] = -Mathf.Sin(z_rot); 80 dims[1][1] = Mathf.Cos(z_rot); 81 return GMatrix(dims); 82 } 83 84 static if (Dimensions == 2) 85 public static GMatrix Rotation(float rot) { 86 return GMatrix([ 87 [Mathf.Cos(rot), -Mathf.Sin(rot)], 88 [Mathf.Sin(rot), Mathf.Cos(rot)] 89 ]); 90 } 91 92 static if (Dimensions == 4) 93 public static GMatrix FromQuaternion(Quaternion quat) { 94 float qx = quat.X; 95 float qy = quat.Y; 96 float qz = quat.Z; 97 float qw = quat.w; 98 float n = 2f/(qx*qx+qy*qy+qz*qz+qw*qw); 99 qx *= n; 100 qy *= n; 101 qz *= n; 102 qw *= n; 103 return GMatrix([ 104 [1.0f - n*qy*qy - n*qz*qz, n*qx*qy - n*qz*qw, n*qx*qz + n*qy*qw, 0f], 105 [n*qx*qy + n*qz*qw, 1f - n*qx*qx - n*qz*qz, n*qy*qz - n*qx*qw, 0f], 106 [n*qx*qz - n*qy*qw, n*qy*qz + n*qx*qw, 1f - n*qx*qx - n*qy*qy, 0f], 107 [0f, 0f, 0f, 1f] 108 ]); 109 } 110 111 static if(Dimensions == 2) 112 public static GMatrix Translation(Vector!(float, Dimensions) trans_vec) { 113 return GMatrix([ 114 [1f, trans_vec.X], 115 [0f, trans_vec.Y] 116 ]); 117 } 118 119 static if(Dimensions == 3) 120 public static GMatrix Translation(Vector!(float, Dimensions) trans_vec) { 121 return GMatrix([ 122 [1f, 0f, trans_vec.X], 123 [0f, 1f, trans_vec.Y], 124 [0f, 0f, trans_vec.Z] 125 ]); 126 } 127 128 static if(Dimensions == 4) 129 public static GMatrix Translation(Vector!(float, Dimensions-1) trans_vec) { 130 return GMatrix([ 131 [1f, 0f, 0f, trans_vec.X], 132 [0f, 1f, 0f, trans_vec.Y], 133 [0f, 0f, 1f, trans_vec.Z], 134 [0f, 0f, 0f, 1f] 135 ]); 136 } 137 138 static if(Dimensions == 4) 139 public static GMatrix Orthographic(float left, float right, float bottom, float top, float znear, float zfar) { 140 return GMatrix([ 141 [2/(right-left), 0f, 0f, -(right+left)/(right-left)], 142 [0f, 2/(top-bottom), 0f, -(top+bottom)/(top-bottom)], 143 [0f, 0f, -2/(zfar-znear), -(zfar+znear)/(zfar-znear)], 144 [0f, 0f, 0f, 1f] 145 ]); 146 } 147 148 static if(Dimensions == 4) 149 public static GMatrix OrthographicInverse(float left, float right, float bottom, float top, float znear, float zfar) { 150 return GMatrix([ 151 [(right-left)/2f, 0, 0, 0], 152 [0, (top-bottom)/2f, 0, 0], 153 [0, 0, (zfar-znear)/-2f, 0], 154 [(right+left)/2f, (top+bottom)/2f, (zfar+znear)/2f, 1f] 155 ]); 156 } 157 158 private static float[6] prepPerspective(float width, float height, float fov, float znear, float zfar) { 159 float aspect = width/height; 160 float top = znear * Mathf.Tan(Mathf.ToRadians(fov)); 161 float bottom = -top; 162 float right = top * aspect; 163 float left = -right; 164 return [left, right, bottom, top, znear, zfar]; 165 } 166 167 static if(Dimensions == 4) 168 public static GMatrix Perspective(float width, float height, float fov, float znear, float zfar) { 169 float[6] persp_data = prepPerspective(width, height, fov, znear, zfar); 170 return perspective(persp_data[0], persp_data[1], persp_data[2], persp_data[3], persp_data[4], persp_data[5]); 171 } 172 173 static if(Dimensions == 4) 174 private static GMatrix perspective(float left, float right, float bottom, float top, float near, float far) { 175 return GMatrix([ 176 [(2f*near)/(right-left), 0, (right+left)/(right-left), 0], 177 [0, (2f*near)/(top-bottom), -(2f*far*near)/(far-near), 0], 178 [0, (top+bottom)/(top-bottom), -(far+near)/(far-near), 0], 179 [0, 0, -1f, 0] 180 ]); 181 } 182 183 public GMatrix opBinary(string op: "*")(GMatrix other) { 184 float[Dimensions][Dimensions] dim = clear(0); 185 foreach ( row; 0 .. Dimensions ) { 186 foreach ( column; 0 .. Dimensions ) { 187 foreach ( i; 0 .. Dimensions ) { 188 dim[row][column] += this.data[row][i] * other.data[i][column]; 189 } 190 } 191 } 192 return GMatrix(dim); 193 } 194 195 public GMatrix Transpose() pure const nothrow { 196 GMatrix temp = this; 197 static foreach( x; 0 .. Dimensions ) { 198 static foreach ( y; 0 .. Dimensions ) { 199 temp.data[x][y] = this.data[y][x]; 200 } 201 } 202 return temp; 203 } 204 205 static if (Dimensions == 4) 206 public GMatrix Translate (Vector!(float, Dimensions-1) trans_vec) { 207 this = GMatrix.Translation(trans_vec) * this; 208 return this; 209 } 210 211 static if (Dimensions >= 3) 212 public GMatrix Scale(Vector!(float, Dimensions-1) scale_vec) { 213 this = GMatrix.Scaling(scale_vec) * this; 214 return this; 215 } 216 217 static if (Dimensions >= 3) 218 public GMatrix RotateX(float rot) { 219 this = GMatrix.RotationX(rot) * this; 220 return this; 221 } 222 223 static if (Dimensions >= 3) 224 public GMatrix RotateY(float rot) { 225 return GMatrix.RotationY(rot) * this; 226 } 227 228 static if (Dimensions >= 3) 229 public GMatrix RotateZ(float rot) { 230 return GMatrix.RotationZ(rot) * this; 231 } 232 233 static if (Dimensions == 2) 234 public GMatrix Rotate(float rot) { 235 return GMatrix.Rotation(rot) * this; 236 } 237 238 static if(Dimensions == 4) 239 public GMatrix Rotate(Quaternion rot) { 240 return GMatrix.FromQuaternion(rot) * this; 241 } 242 243 public string ToString() { 244 string dim = "["; 245 foreach( x; 0 .. Dimensions ) { 246 dim ~= "\n\t["; 247 foreach ( y; 0 .. Dimensions ) { 248 dim ~= this.data[x][y].text; 249 if (y != Dimensions-1) dim ~= ", "; 250 } 251 dim ~= "]"; 252 if (x != Dimensions-1) dim ~= ","; 253 } 254 return dim ~ "\n]"; 255 } 256 257 /// Returns a pointer to the data container of this vector 258 public inout(float)* ptr() pure inout nothrow { return data[0].ptr; } 259 } 260 261 alias Matrix4x4 = Matrix!4; 262 alias Matrix3x3 = Matrix!3; 263 alias Matrix2x2 = Matrix!2; 264 265 alias mat2x2 = Matrix2x2; 266 alias mat3x3 = Matrix3x3; 267 alias mat4x4 = Matrix4x4; 268 269 unittest { 270 Matrix4x4 mt4 = Matrix4x4.Identity; 271 Matrix4x4 mt4_o = Matrix4x4([ 272 [1, 0, 0, 2], 273 [0, 1, 0, 5], 274 [0, 0, 1, 3], 275 [0, 0, 0, 1] 276 ]); 277 278 writeln((mt4_o.Translate(Vector3(1, 1, 1))).ToString); 279 writeln((mt4_o.Translate(Vector3(1, 1, 1))).Transpose.ToString); 280 assert((mt4*mt4_o) == mt4_o, "Matrix multiplication error! should be " ~ mt4_o.ToString); 281 282 Matrix3x3 mt3 = Matrix3x3.Identity; 283 Matrix3x3 mt3_o = Matrix3x3([ 284 [1, 0, 3], 285 [0, 1, 2], 286 [0, 0, 1] 287 ]); 288 assert((mt3*mt3_o) == mt3_o, "Matrix multiplication error! should be " ~ mt3_o.ToString); 289 290 Matrix2x2 mt2 = Matrix2x2.Identity; 291 Matrix2x2 mt2_o = Matrix2x2([ 292 [1, 2], 293 [0, 1], 294 ]); 295 assert((mt2*mt2_o) == mt2_o, "Matrix multiplication error! should be " ~ mt3_o.ToString); 296 297 Matrix4x4 orth_mat = Matrix4x4.Orthographic(-1f, 1f, -1f, 1f, -1f, 1f); 298 Matrix4x4 comp_orth = Matrix4x4([ 299 [1f, 0f, 0f, -0f], 300 [0f, 1f, 0f, -0f], 301 [0f, 0f, -1f, -0f], 302 [0f, 0f, 0f, 1f] 303 ]); 304 assert(orth_mat == comp_orth, "orth_mat != comp_orth \n" ~ orth_mat.ToString ~ ",\n" ~ comp_orth.ToString); 305 }