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 }