eleni@0: #include <math.h>
eleni@0: #include <stdio.h>
eleni@0: #include <string.h>
eleni@0: #include "matrix.h"
eleni@0: 
eleni@0: Matrix4x4 Matrix4x4::identity = Matrix4x4(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1);
eleni@0: 
eleni@0: Matrix4x4::Matrix4x4()
eleni@0: {
eleni@0: 	*this = identity;
eleni@0: }
eleni@0: 
eleni@0: Matrix4x4::Matrix4x4(	float m11, float m12, float m13, float m14,
eleni@0: 						float m21, float m22, float m23, float m24,
eleni@0: 						float m31, float m32, float m33, float m34,
eleni@0: 						float m41, float m42, float m43, float m44)
eleni@0: {
eleni@0: 	m[0][0] = m11; m[0][1] = m12; m[0][2] = m13; m[0][3] = m14;
eleni@0: 	m[1][0] = m21; m[1][1] = m22; m[1][2] = m23; m[1][3] = m24;
eleni@0: 	m[2][0] = m31; m[2][1] = m32; m[2][2] = m33; m[2][3] = m34;
eleni@0: 	m[3][0] = m41; m[3][1] = m42; m[3][2] = m43; m[3][3] = m44;
eleni@0: }
eleni@0: 
eleni@0: Matrix4x4 operator +(const Matrix4x4 &m1, const Matrix4x4 &m2)
eleni@0: {
eleni@0: 	Matrix4x4 res;
eleni@0: 	const float *op1 = m1.m[0], *op2 = m2.m[0];
eleni@0: 	float *dest = res.m[0];
eleni@0: 
eleni@0: 	for(int i=0; i<16; i++) {
eleni@0: 		*dest++ = *op1++ + *op2++;
eleni@0: 	}
eleni@0: 	return res;
eleni@0: }
eleni@0: 
eleni@0: Matrix4x4 operator -(const Matrix4x4 &m1, const Matrix4x4 &m2)
eleni@0: {
eleni@0: 	Matrix4x4 res;
eleni@0: 	const float *op1 = m1.m[0], *op2 = m2.m[0];
eleni@0: 	float *dest = res.m[0];
eleni@0: 
eleni@0: 	for(int i=0; i<16; i++) {
eleni@0: 		*dest++ = *op1++ - *op2++;
eleni@0: 	}
eleni@0: 	return res;
eleni@0: }
eleni@0: 
eleni@0: Matrix4x4 operator *(const Matrix4x4 &m1, const Matrix4x4 &m2) {
eleni@0: 	Matrix4x4 res;
eleni@0: 
eleni@0: 	for(int i=0; i<4; i++) {
eleni@0: 		for(int j=0; j<4; j++) {
eleni@0: 			res.m[i][j] = m1.m[i][0] * m2.m[0][j] + m1.m[i][1] * m2.m[1][j] + m1.m[i][2] * m2.m[2][j] + m1.m[i][3] * m2.m[3][j];
eleni@0: 		}
eleni@0: 	}
eleni@0: 
eleni@0: 	return res;
eleni@0: }
eleni@0: 
eleni@0: void operator +=(Matrix4x4 &m1, const Matrix4x4 &m2)
eleni@0: {
eleni@0: 	float *op1 = m1.m[0];
eleni@0: 	const float *op2 = m2.m[0];
eleni@0: 
eleni@0: 	for(int i=0; i<16; i++) {
eleni@0: 		*op1++ += *op2++;
eleni@0: 	}
eleni@0: }
eleni@0: 
eleni@0: void operator -=(Matrix4x4 &m1, const Matrix4x4 &m2)
eleni@0: {
eleni@0: 	float *op1 = m1.m[0];
eleni@0: 	const float *op2 = m2.m[0];
eleni@0: 
eleni@0: 	for(int i=0; i<16; i++) {
eleni@0: 		*op1++ -= *op2++;
eleni@0: 	}
eleni@0: }
eleni@0: 
eleni@0: void operator *=(Matrix4x4 &m1, const Matrix4x4 &m2)
eleni@0: {
eleni@0: 	Matrix4x4 res;
eleni@0: 	for(int i=0; i<4; i++) {
eleni@0: 		for(int j=0; j<4; j++) {
eleni@0: 			res.m[i][j] = m1.m[i][0] * m2.m[0][j] + m1.m[i][1] * m2.m[1][j] + m1.m[i][2] * m2.m[2][j] + m1.m[i][3] * m2.m[3][j];
eleni@0: 		}
eleni@0: 	}
eleni@0: 	memcpy(m1.m, res.m, 16 * sizeof(float));
eleni@0: }
eleni@0: 
eleni@0: Matrix4x4 operator *(const Matrix4x4 &mat, float scalar)
eleni@0: {
eleni@0: 	Matrix4x4 res;
eleni@0: 	const float *mptr = mat.m[0];
eleni@0: 	float *dptr = res.m[0];
eleni@0: 
eleni@0: 	for(int i=0; i<16; i++) {
eleni@0: 		*dptr++ = *mptr++ * scalar;
eleni@0: 	}
eleni@0: 	return res;
eleni@0: }
eleni@0: 
eleni@0: Matrix4x4 operator *(float scalar, const Matrix4x4 &mat)
eleni@0: {
eleni@0: 	Matrix4x4 res;
eleni@0: 	const float *mptr = mat.m[0];
eleni@0: 	float *dptr = res.m[0];
eleni@0: 
eleni@0: 	for(int i=0; i<16; i++) {
eleni@0: 		*dptr++ = *mptr++ * scalar;
eleni@0: 	}
eleni@0: 	return res;
eleni@0: }
eleni@0: 
eleni@0: void operator *=(Matrix4x4 &mat, float scalar)
eleni@0: {
eleni@0: 	float *mptr = mat.m[0];
eleni@0: 
eleni@0: 	for(int i=0; i<16; i++) {
eleni@0: 		*mptr++ *= scalar;
eleni@0: 	}
eleni@0: }
eleni@0: 
eleni@0: void Matrix4x4::translate(const Vector3 &trans)
eleni@0: {
eleni@0: 	Matrix4x4 tmat(1, 0, 0, trans.x, 0, 1, 0, trans.y, 0, 0, 1, trans.z, 0, 0, 0, 1);
eleni@0: 	*this *= tmat;
eleni@0: }
eleni@0: 
eleni@0: void Matrix4x4::set_translation(const Vector3 &trans)
eleni@0: {
eleni@0: 	*this = Matrix4x4(1, 0, 0, trans.x, 0, 1, 0, trans.y, 0, 0, 1, trans.z, 0, 0, 0, 1);
eleni@0: }
eleni@0: 
eleni@0: void Matrix4x4::rotate(const Vector3 &euler)
eleni@0: {
eleni@0: 	float sinx = sin(euler.x);
eleni@0: 	float cosx = cos(euler.x);
eleni@0: 	float siny = sin(euler.y);
eleni@0: 	float cosy = cos(euler.y);
eleni@0: 	float sinz = sin(euler.z);
eleni@0: 	float cosz = cos(euler.z);
eleni@0: 
eleni@0: 	Matrix4x4 xrot = Matrix4x4(
eleni@0: 			1, 0, 0, 0,
eleni@0: 			0, cosx, sinx, 0,
eleni@0: 			0, -sinx, cosx, 0,
eleni@0: 			0, 0, 0, 1);
eleni@0: 
eleni@0: 	Matrix4x4 yrot = Matrix4x4(
eleni@0: 			cosy, 0, -siny, 0,
eleni@0: 			0, 1, 0, 0,
eleni@0: 			siny, 0, cosy, 0,
eleni@0: 			0, 0, 0, 1);
eleni@0: 
eleni@0: 	Matrix4x4 zrot = Matrix4x4(
eleni@0: 			cosz, sinz, 0, 0,
eleni@0: 			-sinz, cosz, 0, 0,
eleni@0: 			0, 0, 1, 0,
eleni@0: 			0, 0, 0, 1);
eleni@0: 
eleni@0: 	*this *= (xrot * yrot * zrot);
eleni@0: }
eleni@0: 
eleni@0: void Matrix4x4::rotate(const Vector3 &axis, float angle)
eleni@0: {
eleni@0: 	float sina = (float)sin(angle);
eleni@0: 	float cosa = (float)cos(angle);
eleni@0: 	float invcosa = 1-cosa;
eleni@0: 	float nxsq = axis.x * axis.x;
eleni@0: 	float nysq = axis.y * axis.y;
eleni@0: 	float nzsq = axis.z * axis.z;
eleni@0: 
eleni@0: 	Matrix4x4 xform;
eleni@0: 	xform[0][0] = nxsq + (1-nxsq) * cosa;
eleni@0: 	xform[0][1] = axis.x * axis.y * invcosa - axis.z * sina;
eleni@0: 	xform[0][2] = axis.x * axis.z * invcosa + axis.y * sina;
eleni@0: 	xform[1][0] = axis.x * axis.y * invcosa + axis.z * sina;
eleni@0: 	xform[1][1] = nysq + (1-nysq) * cosa;
eleni@0: 	xform[1][2] = axis.y * axis.z * invcosa - axis.x * sina;
eleni@0: 	xform[2][0] = axis.x * axis.z * invcosa - axis.y * sina;
eleni@0: 	xform[2][1] = axis.y * axis.z * invcosa + axis.x * sina;
eleni@0: 	xform[2][2] = nzsq + (1-nzsq) * cosa;
eleni@0: 
eleni@0: 	*this *= Matrix4x4(xform);
eleni@0: }
eleni@0: 
eleni@0: void Matrix4x4::set_rotation(const Vector3 &axis, float angle)
eleni@0: {
eleni@0: 	float sina = (float)sin(angle);
eleni@0: 	float cosa = (float)cos(angle);
eleni@0: 	float invcosa = 1-cosa;
eleni@0: 	float nxsq = axis.x * axis.x;
eleni@0: 	float nysq = axis.y * axis.y;
eleni@0: 	float nzsq = axis.z * axis.z;
eleni@0: 
eleni@0: 	reset_identity();
eleni@0: 	m[0][0] = nxsq + (1-nxsq) * cosa;
eleni@0: 	m[0][1] = axis.x * axis.y * invcosa - axis.z * sina;
eleni@0: 	m[0][2] = axis.x * axis.z * invcosa + axis.y * sina;
eleni@0: 	m[1][0] = axis.x * axis.y * invcosa + axis.z * sina;
eleni@0: 	m[1][1] = nysq + (1-nysq) * cosa;
eleni@0: 	m[1][2] = axis.y * axis.z * invcosa - axis.x * sina;
eleni@0: 	m[2][0] = axis.x * axis.z * invcosa - axis.y * sina;
eleni@0: 	m[2][1] = axis.y * axis.z * invcosa + axis.x * sina;
eleni@0: 	m[2][2] = nzsq + (1-nzsq) * cosa;
eleni@0: }
eleni@0: 
eleni@0: void Matrix4x4::scale(const Vector3 &scale_vec)
eleni@0: {
eleni@0: 	Matrix4x4 smat(	scale_vec.x, 0, 0, 0,
eleni@0: 					0, scale_vec.y, 0, 0,
eleni@0: 					0, 0, scale_vec.z, 0,
eleni@0: 					0, 0, 0, 1.0);
eleni@0: 	*this *= smat;
eleni@0: }
eleni@0: 
eleni@0: void Matrix4x4::set_scaling(const Vector3 &scale_vec)
eleni@0: {
eleni@0: 	*this = Matrix4x4(	scale_vec.x, 0, 0, 0,
eleni@0: 						0, scale_vec.y, 0, 0,
eleni@0: 						0, 0, scale_vec.z, 0,
eleni@0: 						0, 0, 0, 1);
eleni@0: }
eleni@0: 
eleni@0: void Matrix4x4::set_column_vector(const Vector3 &vec, unsigned int col_index)
eleni@0: {
eleni@0: 	m[0][col_index] = vec.x;
eleni@0: 	m[1][col_index] = vec.y;
eleni@0: 	m[2][col_index] = vec.z;
eleni@0: 	m[3][col_index] = 1;
eleni@0: }
eleni@0: 
eleni@0: void Matrix4x4::set_row_vector(const Vector3 &vec, unsigned int row_index)
eleni@0: {
eleni@0: 	m[row_index][0] = vec.x;
eleni@0: 	m[row_index][1] = vec.y;
eleni@0: 	m[row_index][2] = vec.z;
eleni@0: 	m[row_index][3] = 1;
eleni@0: }
eleni@0: 
eleni@0: Vector3 Matrix4x4::get_column_vector(unsigned int col_index) const
eleni@0: {
eleni@0: 	return Vector3(m[0][col_index], m[1][col_index], m[2][col_index]);
eleni@0: }
eleni@0: 
eleni@0: Vector3 Matrix4x4::get_row_vector(unsigned int row_index) const
eleni@0: {
eleni@0: 	return Vector3(m[row_index][0], m[row_index][1], m[row_index][2]);
eleni@0: }
eleni@0: 
eleni@0: void Matrix4x4::transpose()
eleni@0: {
eleni@0: 	Matrix4x4 tmp = *this;
eleni@0: 	for(int i=0; i<4; i++) {
eleni@0: 		for(int j=0; j<4; j++) {
eleni@0: 			m[i][j] = tmp[j][i];
eleni@0: 		}
eleni@0: 	}
eleni@0: }
eleni@0: 
eleni@0: Matrix4x4 Matrix4x4::transposed() const
eleni@0: {
eleni@0: 	Matrix4x4 res;
eleni@0: 	for(int i=0; i<4; i++) {
eleni@0: 		for(int j=0; j<4; j++) {
eleni@0: 			res[i][j] = m[j][i];
eleni@0: 		}
eleni@0: 	}
eleni@0: 	return res;
eleni@0: }
eleni@0: 
eleni@0: float Matrix4x4::determinant() const
eleni@0: {
eleni@0: 	float det11 =	(m[1][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
eleni@0: 						(m[1][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
eleni@0: 						(m[1][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
eleni@0: 
eleni@0: 	float det12 =	(m[1][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
eleni@0: 						(m[1][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
eleni@0: 						(m[1][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
eleni@0: 
eleni@0: 	float det13 =	(m[1][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
eleni@0: 						(m[1][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
eleni@0: 						(m[1][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
eleni@0: 
eleni@0: 	float det14 =	(m[1][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
eleni@0: 						(m[1][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
eleni@0: 						(m[1][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
eleni@0: 
eleni@0: 	return m[0][0] * det11 - m[0][1] * det12 + m[0][2] * det13 - m[0][3] * det14;
eleni@0: }
eleni@0: 
eleni@0: 
eleni@0: Matrix4x4 Matrix4x4::adjoint() const
eleni@0: {
eleni@0: 	Matrix4x4 coef;
eleni@0: 
eleni@0: 	coef.m[0][0] =	(m[1][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
eleni@0: 					(m[1][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
eleni@0: 					(m[1][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
eleni@0: 	coef.m[0][1] =	(m[1][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
eleni@0: 					(m[1][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
eleni@0: 					(m[1][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
eleni@0: 	coef.m[0][2] =	(m[1][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
eleni@0: 					(m[1][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
eleni@0: 					(m[1][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
eleni@0: 	coef.m[0][3] =	(m[1][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
eleni@0: 					(m[1][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
eleni@0: 					(m[1][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
eleni@0: 
eleni@0: 	coef.m[1][0] =	(m[0][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
eleni@0: 					(m[0][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
eleni@0: 					(m[0][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
eleni@0: 	coef.m[1][1] =	(m[0][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
eleni@0: 					(m[0][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
eleni@0: 					(m[0][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
eleni@0: 	coef.m[1][2] =	(m[0][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
eleni@0: 					(m[0][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
eleni@0: 					(m[0][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
eleni@0: 	coef.m[1][3] =	(m[0][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
eleni@0: 					(m[0][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
eleni@0: 					(m[0][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
eleni@0: 
eleni@0: 	coef.m[2][0] =	(m[0][1] * (m[1][2] * m[3][3] - m[3][2] * m[1][3])) -
eleni@0: 					(m[0][2] * (m[1][1] * m[3][3] - m[3][1] * m[1][3])) +
eleni@0: 					(m[0][3] * (m[1][1] * m[3][2] - m[3][1] * m[1][2]));
eleni@0: 	coef.m[2][1] =	(m[0][0] * (m[1][2] * m[3][3] - m[3][2] * m[1][3])) -
eleni@0: 					(m[0][2] * (m[1][0] * m[3][3] - m[3][0] * m[1][3])) +
eleni@0: 					(m[0][3] * (m[1][0] * m[3][2] - m[3][0] * m[1][2]));
eleni@0: 	coef.m[2][2] =	(m[0][0] * (m[1][1] * m[3][3] - m[3][1] * m[1][3])) -
eleni@0: 					(m[0][1] * (m[1][0] * m[3][3] - m[3][0] * m[1][3])) +
eleni@0: 					(m[0][3] * (m[1][0] * m[3][1] - m[3][0] * m[1][1]));
eleni@0: 	coef.m[2][3] =	(m[0][0] * (m[1][1] * m[3][2] - m[3][1] * m[1][2])) -
eleni@0: 					(m[0][1] * (m[1][0] * m[3][2] - m[3][0] * m[1][2])) +
eleni@0: 					(m[0][2] * (m[1][0] * m[3][1] - m[3][0] * m[1][1]));
eleni@0: 
eleni@0: 	coef.m[3][0] =	(m[0][1] * (m[1][2] * m[2][3] - m[2][2] * m[1][3])) -
eleni@0: 					(m[0][2] * (m[1][1] * m[2][3] - m[2][1] * m[1][3])) +
eleni@0: 					(m[0][3] * (m[1][1] * m[2][2] - m[2][1] * m[1][2]));
eleni@0: 	coef.m[3][1] =	(m[0][0] * (m[1][2] * m[2][3] - m[2][2] * m[1][3])) -
eleni@0: 					(m[0][2] * (m[1][0] * m[2][3] - m[2][0] * m[1][3])) +
eleni@0: 					(m[0][3] * (m[1][0] * m[2][2] - m[2][0] * m[1][2]));
eleni@0: 	coef.m[3][2] =	(m[0][0] * (m[1][1] * m[2][3] - m[2][1] * m[1][3])) -
eleni@0: 					(m[0][1] * (m[1][0] * m[2][3] - m[2][0] * m[1][3])) +
eleni@0: 					(m[0][3] * (m[1][0] * m[2][1] - m[2][0] * m[1][1]));
eleni@0: 	coef.m[3][3] =	(m[0][0] * (m[1][1] * m[2][2] - m[2][1] * m[1][2])) -
eleni@0: 					(m[0][1] * (m[1][0] * m[2][2] - m[2][0] * m[1][2])) +
eleni@0: 					(m[0][2] * (m[1][0] * m[2][1] - m[2][0] * m[1][1]));
eleni@0: 
eleni@0: 	coef.transpose();
eleni@0: 
eleni@0: 	for(int i=0; i<4; i++) {
eleni@0: 		for(int j=0; j<4; j++) {
eleni@0: 			coef.m[i][j] = j%2 ? -coef.m[i][j] : coef.m[i][j];
eleni@0: 			if(i%2) coef.m[i][j] = -coef.m[i][j];
eleni@0: 		}
eleni@0: 	}
eleni@0: 
eleni@0: 	return coef;
eleni@0: }
eleni@0: 
eleni@0: Matrix4x4 Matrix4x4::inverse() const
eleni@0: {
eleni@0: 	Matrix4x4 adj = adjoint();
eleni@0: 
eleni@0: 	return adj * (1.0f / determinant());
eleni@0: }
eleni@0: 
eleni@0: inline float *Matrix4x4::operator [](int index)
eleni@0: {
eleni@0: 	return m[index];
eleni@0: }
eleni@0: 
eleni@0: inline const float *Matrix4x4::operator [](int index) const
eleni@0: {
eleni@0: 	return m[index];
eleni@0: }
eleni@0: 
eleni@0: inline void Matrix4x4::reset_identity()
eleni@0: {
eleni@0: 	*this = identity;
eleni@0: }
eleni@0: 
eleni@0: std::ostream &operator <<(std::ostream &out, const Matrix4x4 &mat)
eleni@0: {
eleni@0: 	for(int i=0; i<4; i++) {
eleni@0: 		char str[100];
eleni@0: 		sprintf(str, "[ %12.5f %12.5f %12.5f %12.5f ]\n", (float)mat.m[i][0], (float)mat.m[i][1], (float)mat.m[i][2], (float)mat.m[i][3]);
eleni@0: 		out << str;
eleni@0: 	}
eleni@0: 	return out;
eleni@0: }