diff --git a/graphics/tinygl/arrays.cpp b/graphics/tinygl/arrays.cpp
index d1bf0e589b7..dd74e67388c 100644
--- a/graphics/tinygl/arrays.cpp
+++ b/graphics/tinygl/arrays.cpp
@@ -28,7 +28,7 @@ void glopArrayElement(GLContext *c, GLParam *param) {
 		c->current_normal.X = c->normal_array[i];
 		c->current_normal.Y = c->normal_array[i + 1];
 		c->current_normal.Z = c->normal_array[i + 2];
-		c->current_normal.Z = 0.0f;
+		c->current_normal.W = 0.0f;
 	}
 	if (states & TEXCOORD_ARRAY) {
 		int size = c->texcoord_array_size;
diff --git a/graphics/tinygl/zmath.cpp b/graphics/tinygl/zmath.cpp
index b7b08cd5134..34a69be7c0a 100644
--- a/graphics/tinygl/zmath.cpp
+++ b/graphics/tinygl/zmath.cpp
@@ -214,6 +214,7 @@ void gl_M4_Rotate(M4 *a, float t, int u) {
 	a->m[w][w] = c;
 }
 
+/*
 // inverse of a 3x3 matrix
 void gl_M3_Inv(M3 *a, const M3 *m) {
 	float det;
@@ -234,6 +235,7 @@ void gl_M3_Inv(M3 *a, const M3 *m) {
 	a->m[2][1] = -(m->m[0][0] * m->m[2][1] - m->m[0][1] * m->m[2][0]) / det;
 	a->m[2][2] = (m->m[0][0] * m->m[1][1] - m->m[0][1] * m->m[1][0]) / det;
 }
+*/
 
 // vector arithmetic
 
diff --git a/graphics/tinygl/zmath.h b/graphics/tinygl/zmath.h
index 0988a551ce0..5a89d729255 100644
--- a/graphics/tinygl/zmath.h
+++ b/graphics/tinygl/zmath.h
@@ -5,10 +5,60 @@ namespace TinyGL {
 
 // Matrix & Vertex
 
+class Vector3
+{
+public:
+	Vector3();
+	Vector3(float x, float y, float z);
+
+	Vector3 operator*(float value);
+	Vector3 operator+(const Vector3& other);
+	Vector3 operator-(const Vector3& other);
+
+private:
+	float v[3];
+};
+
+class Vector4
+{
+public:
+	Vector4();
+	Vector4(float x, float y, float z, float w);
+
+	Vector4 operator*(float value);
+	Vector4 operator+(const Vector4& other);
+	Vector4 operator-(const Vector4& other);
+
+private:
+	float v[4];
+};
+
+class Matrix4
+{
+public:
+	Matrix4();
+	Matrix4(const Matrix4& other);
+
+	Matrix4 operator=(const Matrix4& other);
+	Matrix4 operator*(const Matrix4& b);
+	static Matrix4 identity();
+
+	Matrix4 transpose() const;
+	Matrix4 inverseOrtho() const;
+	Matrix4 inverse() const;
+	Matrix4 rotation() const;
+
+	Vector3 transform(const Vector3& vector);
+	Vector4 transform(const Vector4& vector);
+private:
+	float m[4][4];
+};
+
 struct M4 {
 	float m[4][4];
 };
 
+
 struct M3 {
 	float m[3][3];
 };