C:Matrix Operations

From GPWiki

Sometimes it can be userful to handle matrices direcly. After reading this page I created some routines to handle matrices:

void mtxMultMM(float m[16], float m2[16])
{
	float r[16];

	int i, j;

	/* Calculate the new matrix */
	for (i = 0; i < 4; i++)
		for (j = 0; j < 4; j++)
			r[j + i * 4] = m[i * 4 + 0] * m2[j] + m[i * 4 + 1] * m2[4 + j] + m[i * 4 + 2] * m2[8 + j] + m[i * 4 + 3] * m2[12 + j];

	/* Copy in the destination matrix */
	for (i = 0; i < 16; i++)
		m[i] = r[i];
}

void mtxMultVM(float v[4], float m[16])
{
	float r[4];
	int i, j;

	/* Calculate the new vector */
	for (i = 0; i < 4; i++)
		r[i] = v[0] * m[i] + v[1] * m[4 + i] + v[2] * m[8 + i] + v[3] * m[12 + i];

	/* Copy in the destination vector */
	v[0] = r[0];
	v[1] = r[1];
	v[2] = r[2];
	v[3] = r[3];
}

void mtxMultMS(float m[16], float scalar)
{
	int i;

	/* Multiply each component for the scalar */
	for (i = 0; i < 16; i++)
		m[i] *= scalar;
}

void mtxLoadIdentity(float m[16])
{
	int i;

	for (i = 0; i < 16; i++)
		m[i] = 0.0;

	/* Diagonal is 1 */
	m[0] = m[5] = m[10] = m[15] = 1.0;
}

void mtxTranslate(float m[16], float x, float y, float z)
{
	float m2[16];

	/* Create the translation matrix */
	mtxLoadIdentity(m2);
	m2[12] = x;
	m2[13] = y;
	m2[14] = z;

	/* Multiply the matrices */
	mtxMultMM(m, m2);
}

#define D2R (M_PI / 180.0) /* Degree to Radians factor: angles are in degrees */

void mtxRotatex(float m[16], float a)
{
	/* X axis rotation matrix
	 * 1 0    0     0
	 * 0 cosa -sina 0
	 * 0 sina cosa  0
	 * 0 0    0     1
	 */

	float m2[16];

	/* Create the translation matrix */
	mtxLoadIdentity(m2);
	m2[5] = cos(D2R * a);
	m2[6] = -sin(D2R * a);
	m2[9] = -m2[6];
	m2[10] = m2[5];

	/* Multiply the matrices */
	mtxMultMM(m, m2);
}

void mtxRotatey(float m[16], float a)
{
	/* Y axis rotation matrix
	 * cosa 0 -sina 0
	 * 0    1 0     0
	 * sina 0 cosa  0
	 * 0    0 0     1
	 */

	float m2[16];

	/* Create the translation matrix */
	mtxLoadIdentity(m2);
	m2[0] = cos(D2R * a);
	m2[2] = -sin(D2R * a);
	m2[8] = -m2[2];
	m2[10] = m2[0];

	/* Multiply the matrices */
	mtxMultMM(m, m2);
}

void mtxRotatez(float m[16], float a)
{
	/* Z axis rotation matrix
	 * cosa -sina 0 0
	 * sina  cosa 0 0
	 * 0     0    1 0
	 * 0     0    0 1
	 */

	float m2[16];

	/* Create the translation matrix */
	mtxLoadIdentity(m2);
	m2[0] = cos(D2R * a);
	m2[1] = -sin(D2R * a);
	m2[4] = -m2[1];
	m2[5] = m2[0];

	/* Multiply the matrices */
	mtxMultMM(m, m2);
}