scummvm/engine/tinygl/zmath.cpp

// Some simple mathematical functions. Don't look for some logic in
// the function names :-)

#include <stdlib.h>
#include <string.h>
#include <math.h>
#include "tinygl/zmath.h"

void gl_M4_Id(M4 *a) {
	int i, j;
	for (i = 0; i < 4; i++) {
		for (j = 0; j < 4; j++) {
			if (i == j)
				a->m[i][j] = 1.0;
			else
				a->m[i][j] = 0.0;
		}
	}
}

int gl_M4_IsId(M4 *a) {
	int i, j;
	for (i = 0; i < 4; i++) {
		for (j = 0; j < 4; j++) {
		if (i == j) { 
			if (a->m[i][j] != 1.0)
				return 0;
			} else if (a->m[i][j] != 0.0)
				return 0;
		}
	}
	return 1;
}

void gl_M4_Mul(M4 *c, M4 *a, M4 *b) {
	int i, j ,k;
	float s;
	for (i = 0; i < 4; i++) {
		for (j = 0; j < 4; j++) {
			s = 0.0;
			for (k = 0; k < 4; k++)
				s += a->m[i][k] * b->m[k][j];
			c->m[i][j]=s;
		}
	}
}

// c=c*a
void gl_M4_MulLeft(M4 *c, M4 *b) {
	int i, j, k;
	float s;
	M4 a;

	//memcpy(&a, c, 16 * sizeof(float));
	a = *c;

	for (i = 0; i < 4; i++) {
		for (j = 0; j < 4; j++) {
			s = 0.0;
			for (k = 0; k < 4; k++)
				s += a.m[i][k] * b->m[k][j];
			c->m[i][j]=s;
		}
	}
}

void gl_M4_Move(M4 *a, M4 *b) {
	memcpy(a, b, sizeof(M4));
}

void gl_MoveV3(V3 *a, V3 *b) {
	memcpy(a, b, sizeof(V3));
}

void gl_MulM4V3(V3 *a, M4 *b, V3 *c) {
	a->X = b->m[0][0] * c->X + b->m[0][1] * c->Y + b->m[0][2] * c->Z + b->m[0][3];
	a->Y = b->m[1][0] * c->X + b->m[1][1] * c->Y + b->m[1][2] * c->Z + b->m[1][3];
	a->Z = b->m[2][0] * c->X + b->m[2][1] * c->Y + b->m[2][2] * c->Z + b->m[2][3];
}

void gl_MulM3V3(V3 *a, M4 *b, V3 *c) {
	a->X = b->m[0][0] * c->X + b->m[0][1] * c->Y + b->m[0][2] * c->Z;
	a->Y = b->m[1][0] * c->X + b->m[1][1] * c->Y + b->m[1][2] * c->Z;
	a->Z = b->m[2][0] * c->X + b->m[2][1] * c->Y + b->m[2][2] * c->Z;
}

void gl_M4_MulV4(V4 *a, M4 *b, V4 *c) {
	a->X = b->m[0][0] * c->X + b->m[0][1] * c->Y + b->m[0][2] * c->Z + b->m[0][3] * c->W;
	a->Y = b->m[1][0] * c->X + b->m[1][1] * c->Y + b->m[1][2] * c->Z + b->m[1][3] * c->W;
	a->Z = b->m[2][0] * c->X + b->m[2][1] * c->Y + b->m[2][2] * c->Z + b->m[2][3] * c->W;
	a->W = b->m[3][0] * c->X + b->m[3][1] * c->Y + b->m[3][2] * c->Z + b->m[3][3] * c->W;
}

// transposition of a 4x4 matrix
void gl_M4_Transpose(M4 *a, M4 *b) {
	a->m[0][0] = b->m[0][0];
	a->m[0][1] = b->m[1][0];
	a->m[0][2] = b->m[2][0];
	a->m[0][3] = b->m[3][0];

	a->m[1][0] = b->m[0][1];
	a->m[1][1] = b->m[1][1];
	a->m[1][2] = b->m[2][1];
	a->m[1][3] = b->m[3][1];

	a->m[2][0] = b->m[0][2];
	a->m[2][1] = b->m[1][2];
	a->m[2][2] = b->m[2][2];
	a->m[2][3] = b->m[3][2];

	a->m[3][0] = b->m[0][3];
	a->m[3][1] = b->m[1][3];
	a->m[3][2] = b->m[2][3];
	a->m[3][3] = b->m[3][3];
}

// inversion of an orthogonal matrix of type Y=M.X+P
void gl_M4_InvOrtho(M4 *a, M4 b) {
	int i, j;
	float s;
	for (i = 0; i < 3; i++) {
		for (j = 0; j < 3; j++)
			a->m[i][j] = b.m[j][i];
		a->m[3][0] = 0.0;
		a->m[3][1] = 0.0;
		a->m[3][2] = 0.0;
		a->m[3][3] = 1.0;
		for (i = 0; i < 3; i++) {
			s = 0;
			for (j = 0; j < 3; j++)
				s -= b.m[j][i] * b.m[j][3];
			a->m[i][3]=s;
		}
	}
}

// Inversion of a general nxn matrix.
// Note : m is destroyed

int Matrix_Inv(float *r, float *m, int n) {
	int i, j, k, l;
	float max, tmp, t;

	// identit<69>e dans r
	for (i = 0; i < n * n; i++)
		r[i]=0;
	for (i = 0; i < n; i++)
		r[i * n + i] = 1;
	 
	for (j = 0; j < n; j++) {
		max = m[j * n + j];
		k = j;
		for (i = j + 1; i < n; i++) {
			if (fabs(m[i * n + j]) > fabs(max)) {
				 k = i;
				 max = m[i * n + j];
			}
		}
		// non intersible matrix
		if (max == 0)
			return 1;
		
		if (k != j) {
			 for (i = 0; i < n; i++) {
					tmp = m[j * n + i];
					m[j * n + i] = m[k * n + i];
					m[k * n + i] = tmp;

					tmp = r[j * n + i];
					r[j * n + i] = r[k * n + i];
					r[k * n + i] = tmp;
			 }
		}

		max = 1 / max;
		for (i = 0; i < n; i++) {
			m[j * n + i] *= max;
			r[j * n + i] *= max;
		}
		
			
		for (l = 0; l < n; l++) {
			if (l != j) {
				t = m[l * n + j];
				for (i = 0; i < n; i++) {
					m[l * n + i] -= m[j * n + i] * t;
					r[l * n + i] -= r[j * n + i] * t;
				}
			}
		}
	}

	return 0;
}

// inversion of a 4x4 matrix

void gl_M4_Inv(M4 *a, M4 *b) {
	M4 tmp;
	memcpy(&tmp, b, 16 * sizeof(float));
	//tmp = *b
	Matrix_Inv(&a->m[0][0], &tmp.m[0][0], 4);
}

void gl_M4_Rotate(M4 *a, float t, int u) {
	float s,c;
	int v,w;

	if ((v = u + 1) > 2)
		v = 0;
	if ((w = v + 1) > 2)
		w=0;
	s = sin(t);
	c = cos(t);
	gl_M4_Id(a);
	a->m[v][v] = c;
	a->m[v][w] = -s;
	a->m[w][v] = s;
	a->m[w][w] = c;
}
	
// inverse of a 3x3 matrix
void gl_M3_Inv(M3 *a, M3 *m) {
	float det;

	det = m->m[0][0] * m->m[1][1] * m->m[2][2] - m->m[0][0] * m->m[1][2] * m->m[2][1] -
		m->m[1][0] * m->m[0][1] * m->m[2][2] + m->m[1][0] * m->m[0][2] * m->m[2][1] +
		m->m[2][0] * m->m[0][1] * m->m[1][2] - m->m[2][0] * m->m[0][2] * m->m[1][1];

	a->m[0][0] = (m->m[1][1] * m->m[2][2] - m->m[1][2] * m->m[2][1]) / det;
	a->m[0][1] = -(m->m[0][1] * m->m[2][2] - m->m[0][2] * m->m[2][1]) / det;
	a->m[0][2] = -(-m->m[0][1] * m->m[1][2] + m->m[0][2] * m->m[1][1]) / det;

	a->m[1][0] = -(m->m[1][0] * m->m[2][2] - m->m[1][2] * m->m[2][0]) / det;
	a->m[1][1] = (m->m[0][0] * m->m[2][2] - m->m[0][2] * m->m[2][0]) / det;
	a->m[1][2] = -(m->m[0][0] * m->m[1][2] - m->m[0][2] * m->m[1][0]) / det;

	a->m[2][0] = (m->m[1][0] * m->m[2][1] - m->m[1][1] * m->m[2][0]) / det;
	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

int gl_V3_Norm(V3 *a) {
	float n;
	n = sqrt(a->X * a->X + a->Y * a->Y + a->Z * a->Z);
	if (n == 0)
		return 1;
	a->X /= n;
	a->Y /= n;
	a->Z /= n;
	return 0;
}

V3 gl_V3_New(float x, float y, float z) {
	V3 a;
	a.X = x;
	a.Y = y;
	a.Z = z;
	return a;
}

V4 gl_V4_New(float x, float y, float z, float w) {
	V4 a;
	a.X = x;
	a.Y = y;
	a.Z = z;
	a.W = w;
	return a;
}