1 files changed, 48 insertions, 3 deletions

diff --git a/src/math/mat.c b/src/math/mat.c
index e3c6ce3..3bb571a 100644
--- a/src/math/mat.c
+++ b/src/math/mat.c
@@ -12,6 +12,9 @@ static Mat4 sharedMat2;
 static Vec3 sharedVec3;
 static Vec4 sharedVec4;
 
+static Mat3 sharedMat3;
+static Mat3 sharedMat32;
+
 Mat4 mat4identity = {
 	1,0,0,0,
 	0,1,0,0,
@@ -19,6 +22,17 @@ Mat4 mat4identity = {
 	0,0,0,1
 };
 
+#define shrmat3(p)\
+do{\
+sharedMat3 = *p;\
+p = &sharedMat3;\
+}while(0)
+#define shrmat32(p)\
+do{\
+sharedMat32 = *p;\
+p = &sharedMat32;\
+}while(0)
+
 #define shrmat(p) \
 do{\
 sharedMat = *p;\
@@ -121,6 +135,8 @@ void internal_mat4_multiply(Mat4* m1, Mat4* m2, Mat4* out) {
 	out->e30 = mul(3, 0); out->e31 = mul(3, 1); out->e32 = mul(3, 2); out->e33 = mul(3, 3);
 }
 
+#undef mul
+
 void internal_mat4_setscale(float kx, float ky, float kz, Mat4* out) {
 	ssr_assert(out);
 	internal_mat4_zero(out);
@@ -748,20 +764,49 @@ void internal_mat4_toquat(Mat4* in, Quat* out) {
 	internal_quat_normalize(out, out);
 }
 
-void mat3_multvec3(Mat3* m, Vec3* v, Vec3* out) {
+bool internal_mat3_isidentity(Mat3* m) {
+	ssr_assert(m);
+	return
+		compare(m->c1.x, 1) && compare(m->c2.x, 0) && compare(m->c3.x, 0) &&
+		compare(m->c1.y, 0) && compare(m->c2.y, 1) && compare(m->c3.y, 0) &&
+		compare(m->c1.z, 0) && compare(m->c2.z, 0) && compare(m->c3.z, 1);
+}
+
+void internal_mat3_multvec3(Mat3* m, Vec3* v, Vec3* out) {
 	ssr_assert(m && v && out);
 	out->x = m->e00 * v->x + m->e01 * v->y + m->e02 * v->z;
 	out->y = m->e10 * v->x + m->e11 * v->y + m->e12 * v->z;
 	out->z = m->e20 * v->x + m->e21 * v->y + m->e22 * v->z;
 }
 
-void mat23_applytovec3(Mat23* m, Vec3* v, Vec2* out) {
+#define MAT3(_m, r, c) (_m->m[c][r])
+
+static float _mul3(float* r, float* c) {
+	return c[0] * r[0] + c[1] * r[3] + c[2] * r[6];
+}
+
+#define mul(r, c) _mul3(&MAT3(m1,r,0), &MAT3(m2,0,c))
+
+void internal_mat3_multmat3(Mat3* m1, Mat3* m2, Mat3* out) {
+	ssr_assert(m1 && m2 && out);
+	if (internal_mat3_isidentity(m1)) { if (m2 != out)*out = *m2;  return; }
+	if (internal_mat3_isidentity(m2)) { if (m1 != out)*out = *m1;  return; }
+	if (out == m1) shrmat3(m1);
+	if (out == m2) shrmat32(m2);
+	out->e00 = mul(0, 0); out->e01 = mul(0, 1); out->e02 = mul(0, 2); 
+	out->e10 = mul(1, 0); out->e11 = mul(1, 1); out->e12 = mul(1, 2); 
+	out->e20 = mul(2, 0); out->e21 = mul(2, 1); out->e22 = mul(2, 2); 
+}
+
+#undef mul
+
+void internal_mat23_applytovec3(Mat23* m, Vec3* v, Vec2* out) {
 	ssr_assert(m && v && out);
 	out->x = m->e00 * v->x + m->e01 * v->y + m->e02 * v->z;
 	out->y = m->e10 * v->x + m->e11 * v->y + m->e12 * v->z;
 }
 
-void mat43_applytovec3(Mat43* m, Vec3* v, Vec4* out) {
+void internal_mat43_applytovec3(Mat43* m, Vec3* v, Vec4* out) {
 	ssr_assert(m && v && out);
 	out->x = m->e00 * v->x + m->e01 * v->y + m->e02 * v->z;
 	out->y = m->e10 * v->x + m->e11 * v->y + m->e12 * v->z;