nuclear@27: #include <cstdio>
nuclear@27: #include <cmath>
nuclear@27: #include "matrix.h"
nuclear@27: #include "vector.h"
nuclear@27: #include "quat.h"
nuclear@27: 
nuclear@27: using namespace std;
nuclear@27: 
nuclear@27: // ----------- Matrix3x3 --------------
nuclear@27: 
nuclear@27: Matrix3x3 Matrix3x3::identity = Matrix3x3(1, 0, 0, 0, 1, 0, 0, 0, 1);
nuclear@27: 
nuclear@27: Matrix3x3::Matrix3x3()
nuclear@27: {
nuclear@27: 	*this = Matrix3x3(1, 0, 0, 0, 1, 0, 0, 0, 1);
nuclear@27: }
nuclear@27: 
nuclear@27: Matrix3x3::Matrix3x3(	scalar_t m11, scalar_t m12, scalar_t m13,
nuclear@27: 						scalar_t m21, scalar_t m22, scalar_t m23,
nuclear@27: 						scalar_t m31, scalar_t m32, scalar_t m33)
nuclear@27: {
nuclear@27: 	m[0][0] = m11; m[0][1] = m12; m[0][2] = m13;
nuclear@27: 	m[1][0] = m21; m[1][1] = m22; m[1][2] = m23;
nuclear@27: 	m[2][0] = m31; m[2][1] = m32; m[2][2] = m33;
nuclear@27: }
nuclear@27: 
nuclear@27: Matrix3x3::Matrix3x3(const Vector3 &ivec, const Vector3 &jvec, const Vector3 &kvec)
nuclear@27: {
nuclear@27: 	set_row_vector(ivec, 0);
nuclear@27: 	set_row_vector(jvec, 1);
nuclear@27: 	set_row_vector(kvec, 2);
nuclear@27: }
nuclear@27: 
nuclear@27: Matrix3x3::Matrix3x3(const mat3_t cmat)
nuclear@27: {
nuclear@27: 	memcpy(m, cmat, sizeof(mat3_t));
nuclear@27: }
nuclear@27: 
nuclear@27: Matrix3x3::Matrix3x3(const Matrix4x4 &mat4x4)
nuclear@27: {
nuclear@27: 	for(int i=0; i<3; i++) {
nuclear@27: 		for(int j=0; j<3; j++) {
nuclear@27: 			m[i][j] = mat4x4[i][j];
nuclear@27: 		}
nuclear@27: 	}
nuclear@27: }
nuclear@27: 
nuclear@27: Matrix3x3 operator +(const Matrix3x3 &m1, const Matrix3x3 &m2)
nuclear@27: {
nuclear@27: 	Matrix3x3 res;
nuclear@27: 	const scalar_t *op1 = m1.m[0], *op2 = m2.m[0];
nuclear@27: 	scalar_t *dest = res.m[0];
nuclear@27: 
nuclear@27: 	for(int i=0; i<9; i++) {
nuclear@27: 		*dest++ = *op1++ + *op2++;
nuclear@27: 	}
nuclear@27: 	return res;
nuclear@27: }
nuclear@27: 
nuclear@27: Matrix3x3 operator -(const Matrix3x3 &m1, const Matrix3x3 &m2)
nuclear@27: {
nuclear@27: 	Matrix3x3 res;
nuclear@27: 	const scalar_t *op1 = m1.m[0], *op2 = m2.m[0];
nuclear@27: 	scalar_t *dest = res.m[0];
nuclear@27: 
nuclear@27: 	for(int i=0; i<9; i++) {
nuclear@27: 		*dest++ = *op1++ - *op2++;
nuclear@27: 	}
nuclear@27: 	return res;
nuclear@27: }
nuclear@27: 
nuclear@27: Matrix3x3 operator *(const Matrix3x3 &m1, const Matrix3x3 &m2)
nuclear@27: {
nuclear@27: 	Matrix3x3 res;
nuclear@27: 	for(int i=0; i<3; i++) {
nuclear@27: 		for(int j=0; j<3; j++) {
nuclear@27: 			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];
nuclear@27: 		}
nuclear@27: 	}
nuclear@27: 	return res;
nuclear@27: }
nuclear@27: 
nuclear@27: void operator +=(Matrix3x3 &m1, const Matrix3x3 &m2)
nuclear@27: {
nuclear@27: 	scalar_t *op1 = m1.m[0];
nuclear@27: 	const scalar_t *op2 = m2.m[0];
nuclear@27: 
nuclear@27: 	for(int i=0; i<9; i++) {
nuclear@27: 		*op1++ += *op2++;
nuclear@27: 	}
nuclear@27: }
nuclear@27: 
nuclear@27: void operator -=(Matrix3x3 &m1, const Matrix3x3 &m2)
nuclear@27: {
nuclear@27: 	scalar_t *op1 = m1.m[0];
nuclear@27: 	const scalar_t *op2 = m2.m[0];
nuclear@27: 
nuclear@27: 	for(int i=0; i<9; i++) {
nuclear@27: 		*op1++ -= *op2++;
nuclear@27: 	}
nuclear@27: }
nuclear@27: 
nuclear@27: void operator *=(Matrix3x3 &m1, const Matrix3x3 &m2)
nuclear@27: {
nuclear@27: 	Matrix3x3 res;
nuclear@27: 	for(int i=0; i<3; i++) {
nuclear@27: 		for(int j=0; j<3; j++) {
nuclear@27: 			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];
nuclear@27: 		}
nuclear@27: 	}
nuclear@27: 	memcpy(m1.m, res.m, 9 * sizeof(scalar_t));
nuclear@27: }
nuclear@27: 
nuclear@27: Matrix3x3 operator *(const Matrix3x3 &mat, scalar_t scalar)
nuclear@27: {
nuclear@27: 	Matrix3x3 res;
nuclear@27: 	const scalar_t *mptr = mat.m[0];
nuclear@27: 	scalar_t *dptr = res.m[0];
nuclear@27: 
nuclear@27: 	for(int i=0; i<9; i++) {
nuclear@27: 		*dptr++ = *mptr++ * scalar;
nuclear@27: 	}
nuclear@27: 	return res;
nuclear@27: }
nuclear@27: 
nuclear@27: Matrix3x3 operator *(scalar_t scalar, const Matrix3x3 &mat)
nuclear@27: {
nuclear@27: 	Matrix3x3 res;
nuclear@27: 	const scalar_t *mptr = mat.m[0];
nuclear@27: 	scalar_t *dptr = res.m[0];
nuclear@27: 
nuclear@27: 	for(int i=0; i<9; i++) {
nuclear@27: 		*dptr++ = *mptr++ * scalar;
nuclear@27: 	}
nuclear@27: 	return res;
nuclear@27: }
nuclear@27: 
nuclear@27: void operator *=(Matrix3x3 &mat, scalar_t scalar)
nuclear@27: {
nuclear@27: 	scalar_t *mptr = mat.m[0];
nuclear@27: 
nuclear@27: 	for(int i=0; i<9; i++) {
nuclear@27: 		*mptr++ *= scalar;
nuclear@27: 	}
nuclear@27: }
nuclear@27: 
nuclear@27: void Matrix3x3::translate(const Vector2 &trans)
nuclear@27: {
nuclear@27: 	Matrix3x3 tmat(1, 0, trans.x, 0, 1, trans.y, 0, 0, 1);
nuclear@27: 	*this *= tmat;
nuclear@27: }
nuclear@27: 
nuclear@27: void Matrix3x3::set_translation(const Vector2 &trans)
nuclear@27: {
nuclear@27: 	*this = Matrix3x3(1, 0, trans.x, 0, 1, trans.y, 0, 0, 1);
nuclear@27: }
nuclear@27: 
nuclear@27: void Matrix3x3::rotate(scalar_t angle)
nuclear@27: {
nuclear@27: 	scalar_t cos_a = cos(angle);
nuclear@27: 	scalar_t sin_a = sin(angle);
nuclear@27: 	Matrix3x3 rmat(	cos_a,	-sin_a,		0,
nuclear@27: 					sin_a,	cos_a,		0,
nuclear@27: 					0,		0,			1);
nuclear@27: 	*this *= rmat;
nuclear@27: }
nuclear@27: 
nuclear@27: void Matrix3x3::set_rotation(scalar_t angle)
nuclear@27: {
nuclear@27: 	scalar_t cos_a = cos(angle);
nuclear@27: 	scalar_t sin_a = sin(angle);
nuclear@27: 	*this = Matrix3x3(cos_a, -sin_a, 0, sin_a, cos_a, 0, 0, 0, 1);
nuclear@27: }
nuclear@27: 
nuclear@27: void Matrix3x3::rotate(const Vector3 &euler_angles)
nuclear@27: {
nuclear@27: 	Matrix3x3 xrot, yrot, zrot;
nuclear@27: 
nuclear@27: 	xrot = Matrix3x3(	1,			0,					0,
nuclear@27: 						0,	cos(euler_angles.x),	-sin(euler_angles.x),
nuclear@27: 						0,	sin(euler_angles.x),	cos(euler_angles.x));
nuclear@27: 
nuclear@27: 	yrot = Matrix3x3(	cos(euler_angles.y),	0,	sin(euler_angles.y),
nuclear@27: 								0,				1,				0,
nuclear@27: 						-sin(euler_angles.y),	0,	cos(euler_angles.y));
nuclear@27: 
nuclear@27: 	zrot = Matrix3x3(	cos(euler_angles.z),	-sin(euler_angles.z),	0,
nuclear@27: 						sin(euler_angles.z),	cos(euler_angles.z),	0,
nuclear@27: 								0,						0,				1);
nuclear@27: 
nuclear@27: 	*this *= xrot * yrot * zrot;
nuclear@27: }
nuclear@27: 
nuclear@27: void Matrix3x3::set_rotation(const Vector3 &euler_angles)
nuclear@27: {
nuclear@27: 	Matrix3x3 xrot, yrot, zrot;
nuclear@27: 
nuclear@27: 	xrot = Matrix3x3(	1,			0,					0,
nuclear@27: 						0,	cos(euler_angles.x),	-sin(euler_angles.x),
nuclear@27: 						0,	sin(euler_angles.x),	cos(euler_angles.x));
nuclear@27: 
nuclear@27: 	yrot = Matrix3x3(	cos(euler_angles.y),	0,	sin(euler_angles.y),
nuclear@27: 								0,				1,				0,
nuclear@27: 						-sin(euler_angles.y),	0,	cos(euler_angles.y));
nuclear@27: 
nuclear@27: 	zrot = Matrix3x3(	cos(euler_angles.z),	-sin(euler_angles.z),	0,
nuclear@27: 						sin(euler_angles.z),	cos(euler_angles.z),	0,
nuclear@27: 								0,						0,				1);
nuclear@27: 
nuclear@27: 	*this = xrot * yrot * zrot;
nuclear@27: }
nuclear@27: 
nuclear@27: void Matrix3x3::rotate(const Vector3 &axis, scalar_t angle)
nuclear@27: {
nuclear@27: 	scalar_t sina = (scalar_t)sin(angle);
nuclear@27: 	scalar_t cosa = (scalar_t)cos(angle);
nuclear@27: 	scalar_t invcosa = 1-cosa;
nuclear@27: 	scalar_t nxsq = axis.x * axis.x;
nuclear@27: 	scalar_t nysq = axis.y * axis.y;
nuclear@27: 	scalar_t nzsq = axis.z * axis.z;
nuclear@27: 
nuclear@27: 	Matrix3x3 xform;
nuclear@27: 	xform.m[0][0] = nxsq + (1-nxsq) * cosa;
nuclear@27: 	xform.m[0][1] = axis.x * axis.y * invcosa - axis.z * sina;
nuclear@27: 	xform.m[0][2] = axis.x * axis.z * invcosa + axis.y * sina;
nuclear@27: 
nuclear@27: 	xform.m[1][0] = axis.x * axis.y * invcosa + axis.z * sina;
nuclear@27: 	xform.m[1][1] = nysq + (1-nysq) * cosa;
nuclear@27: 	xform.m[1][2] = axis.y * axis.z * invcosa - axis.x * sina;
nuclear@27: 
nuclear@27: 	xform.m[2][0] = axis.x * axis.z * invcosa - axis.y * sina;
nuclear@27: 	xform.m[2][1] = axis.y * axis.z * invcosa + axis.x * sina;
nuclear@27: 	xform.m[2][2] = nzsq + (1-nzsq) * cosa;
nuclear@27: 
nuclear@27: 	*this *= xform;
nuclear@27: }
nuclear@27: 
nuclear@27: void Matrix3x3::set_rotation(const Vector3 &axis, scalar_t angle)
nuclear@27: {
nuclear@27: 	scalar_t sina = (scalar_t)sin(angle);
nuclear@27: 	scalar_t cosa = (scalar_t)cos(angle);
nuclear@27: 	scalar_t invcosa = 1-cosa;
nuclear@27: 	scalar_t nxsq = axis.x * axis.x;
nuclear@27: 	scalar_t nysq = axis.y * axis.y;
nuclear@27: 	scalar_t nzsq = axis.z * axis.z;
nuclear@27: 
nuclear@27: 	reset_identity();
nuclear@27: 	m[0][0] = nxsq + (1-nxsq) * cosa;
nuclear@27: 	m[0][1] = axis.x * axis.y * invcosa - axis.z * sina;
nuclear@27: 	m[0][2] = axis.x * axis.z * invcosa + axis.y * sina;
nuclear@27: 	m[1][0] = axis.x * axis.y * invcosa + axis.z * sina;
nuclear@27: 	m[1][1] = nysq + (1-nysq) * cosa;
nuclear@27: 	m[1][2] = axis.y * axis.z * invcosa - axis.x * sina;
nuclear@27: 	m[2][0] = axis.x * axis.z * invcosa - axis.y * sina;
nuclear@27: 	m[2][1] = axis.y * axis.z * invcosa + axis.x * sina;
nuclear@27: 	m[2][2] = nzsq + (1-nzsq) * cosa;
nuclear@27: }
nuclear@27: 
nuclear@27: // Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
nuclear@27: // article "Quaternion Calculus and Fast Animation".
nuclear@27: // adapted from: http://www.geometrictools.com/LibMathematics/Algebra/Wm5Quaternion.inl
nuclear@27: Quaternion Matrix3x3::get_rotation_quat() const
nuclear@27: {
nuclear@27: 	static const int next[3] = {1, 2, 0};
nuclear@27: 
nuclear@27: 	float quat[4];
nuclear@27: 
nuclear@27: 	scalar_t trace = m[0][0] + m[1][1] + m[2][2];
nuclear@27: 	scalar_t root;
nuclear@27: 
nuclear@27: 	if(trace > 0.0f) {
nuclear@27: 		// |w| > 1/2
nuclear@27: 		root = sqrt(trace + 1.0f);	// 2w
nuclear@27: 		quat[0] = 0.5f * root;
nuclear@27: 		root = 0.5f / root;	// 1 / 4w
nuclear@27: 		quat[1] = (m[2][1] - m[1][2]) * root;
nuclear@27: 		quat[2] = (m[0][2] - m[2][0]) * root;
nuclear@27: 		quat[3] = (m[1][0] - m[0][1]) * root;
nuclear@27: 	} else {
nuclear@27: 		// |w| <= 1/2
nuclear@27: 		int i = 0;
nuclear@27: 		if(m[1][1] > m[0][0]) {
nuclear@27: 			i = 1;
nuclear@27: 		}
nuclear@27: 		if(m[2][2] > m[i][i]) {
nuclear@27: 			i = 2;
nuclear@27: 		}
nuclear@27: 		int j = next[i];
nuclear@27: 		int k = next[j];
nuclear@27: 
nuclear@27: 		root = sqrt(m[i][i] - m[j][j] - m[k][k] + 1.0f);
nuclear@27: 		quat[i + 1] = 0.5f * root;
nuclear@27: 		root = 0.5f / root;
nuclear@27: 		quat[0] = (m[k][j] - m[j][k]) * root;
nuclear@27: 		quat[j + 1] = (m[j][i] - m[i][j]) * root;
nuclear@27: 		quat[k + 1] = (m[k][i] - m[i][k]) * root;
nuclear@27: 	}
nuclear@27: 	return Quaternion(quat[0], quat[1], quat[2], quat[3]);
nuclear@27: }
nuclear@27: 
nuclear@27: void Matrix3x3::scale(const Vector3 &scale_vec)
nuclear@27: {
nuclear@27: 	Matrix3x3 smat(	scale_vec.x, 0, 0,
nuclear@27: 					0, scale_vec.y, 0,
nuclear@27: 					0, 0, scale_vec.z);
nuclear@27: 	*this *= smat;
nuclear@27: }
nuclear@27: 
nuclear@27: void Matrix3x3::set_scaling(const Vector3 &scale_vec)
nuclear@27: {
nuclear@27: 	*this = Matrix3x3(	scale_vec.x, 0, 0,
nuclear@27: 						0, scale_vec.y, 0,
nuclear@27: 						0, 0, scale_vec.z);
nuclear@27: }
nuclear@27: 
nuclear@27: void Matrix3x3::set_column_vector(const Vector3 &vec, unsigned int col_index)
nuclear@27: {
nuclear@27: 	m[0][col_index] = vec.x;
nuclear@27: 	m[1][col_index] = vec.y;
nuclear@27: 	m[2][col_index] = vec.z;
nuclear@27: }
nuclear@27: 
nuclear@27: void Matrix3x3::set_row_vector(const Vector3 &vec, unsigned int row_index)
nuclear@27: {
nuclear@27: 	m[row_index][0] = vec.x;
nuclear@27: 	m[row_index][1] = vec.y;
nuclear@27: 	m[row_index][2] = vec.z;
nuclear@27: }
nuclear@27: 
nuclear@27: Vector3 Matrix3x3::get_column_vector(unsigned int col_index) const
nuclear@27: {
nuclear@27: 	return Vector3(m[0][col_index], m[1][col_index], m[2][col_index]);
nuclear@27: }
nuclear@27: 
nuclear@27: Vector3 Matrix3x3::get_row_vector(unsigned int row_index) const
nuclear@27: {
nuclear@27: 	return Vector3(m[row_index][0], m[row_index][1], m[row_index][2]);
nuclear@27: }
nuclear@27: 
nuclear@27: void Matrix3x3::transpose()
nuclear@27: {
nuclear@27: 	Matrix3x3 tmp = *this;
nuclear@27: 	for(int i=0; i<3; i++) {
nuclear@27: 		for(int j=0; j<3; j++) {
nuclear@27: 			m[i][j] = tmp[j][i];
nuclear@27: 		}
nuclear@27: 	}
nuclear@27: }
nuclear@27: 
nuclear@27: Matrix3x3 Matrix3x3::transposed() const
nuclear@27: {
nuclear@27: 	Matrix3x3 res;
nuclear@27: 	for(int i=0; i<3; i++) {
nuclear@27: 		for(int j=0; j<3; j++) {
nuclear@27: 			res[i][j] = m[j][i];
nuclear@27: 		}
nuclear@27: 	}
nuclear@27: 	return res;
nuclear@27: }
nuclear@27: 
nuclear@27: scalar_t Matrix3x3::determinant() const
nuclear@27: {
nuclear@27: 	return	m[0][0] * (m[1][1]*m[2][2] - m[1][2]*m[2][1]) -
nuclear@27: 			m[0][1] * (m[1][0]*m[2][2] - m[1][2]*m[2][0]) +
nuclear@27: 			m[0][2] * (m[1][0]*m[2][1] - m[1][1]*m[2][0]);
nuclear@27: }
nuclear@27: 
nuclear@27: Matrix3x3 Matrix3x3::inverse() const
nuclear@27: {
nuclear@27: 	// TODO: implement 3x3 inverse
nuclear@27: 	return *this;
nuclear@27: }
nuclear@27: 
nuclear@27: ostream &operator <<(ostream &out, const Matrix3x3 &mat)
nuclear@27: {
nuclear@27: 	for(int i=0; i<3; i++) {
nuclear@27: 		char str[100];
nuclear@27: 		sprintf(str, "[ %12.5f %12.5f %12.5f ]\n", (float)mat.m[i][0], (float)mat.m[i][1], (float)mat.m[i][2]);
nuclear@27: 		out << str;
nuclear@27: 	}
nuclear@27: 	return out;
nuclear@27: }
nuclear@27: 
nuclear@27: 
nuclear@27: 
nuclear@27: /* ----------------- Matrix4x4 implementation --------------- */
nuclear@27: 
nuclear@27: Matrix4x4 Matrix4x4::identity = Matrix4x4(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1);
nuclear@27: 
nuclear@27: Matrix4x4::Matrix4x4()
nuclear@27: {
nuclear@27: 	*this = Matrix4x4(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1);
nuclear@27: }
nuclear@27: 
nuclear@27: Matrix4x4::Matrix4x4(	scalar_t m11, scalar_t m12, scalar_t m13, scalar_t m14,
nuclear@27: 						scalar_t m21, scalar_t m22, scalar_t m23, scalar_t m24,
nuclear@27: 						scalar_t m31, scalar_t m32, scalar_t m33, scalar_t m34,
nuclear@27: 						scalar_t m41, scalar_t m42, scalar_t m43, scalar_t m44)
nuclear@27: {
nuclear@27: 	m[0][0] = m11; m[0][1] = m12; m[0][2] = m13; m[0][3] = m14;
nuclear@27: 	m[1][0] = m21; m[1][1] = m22; m[1][2] = m23; m[1][3] = m24;
nuclear@27: 	m[2][0] = m31; m[2][1] = m32; m[2][2] = m33; m[2][3] = m34;
nuclear@27: 	m[3][0] = m41; m[3][1] = m42; m[3][2] = m43; m[3][3] = m44;
nuclear@27: }
nuclear@27: 
nuclear@27: Matrix4x4::Matrix4x4(const mat4_t cmat)
nuclear@27: {
nuclear@27: 	memcpy(m, cmat, sizeof(mat4_t));
nuclear@27: }
nuclear@27: 
nuclear@27: Matrix4x4::Matrix4x4(const Matrix3x3 &mat3x3)
nuclear@27: {
nuclear@27: 	reset_identity();
nuclear@27: 	for(int i=0; i<3; i++) {
nuclear@27: 		for(int j=0; j<3; j++) {
nuclear@27: 			m[i][j] = mat3x3[i][j];
nuclear@27: 		}
nuclear@27: 	}
nuclear@27: }
nuclear@27: 
nuclear@27: Matrix4x4 operator +(const Matrix4x4 &m1, const Matrix4x4 &m2)
nuclear@27: {
nuclear@27: 	Matrix4x4 res;
nuclear@27: 	const scalar_t *op1 = m1.m[0], *op2 = m2.m[0];
nuclear@27: 	scalar_t *dest = res.m[0];
nuclear@27: 
nuclear@27: 	for(int i=0; i<16; i++) {
nuclear@27: 		*dest++ = *op1++ + *op2++;
nuclear@27: 	}
nuclear@27: 	return res;
nuclear@27: }
nuclear@27: 
nuclear@27: Matrix4x4 operator -(const Matrix4x4 &m1, const Matrix4x4 &m2)
nuclear@27: {
nuclear@27: 	Matrix4x4 res;
nuclear@27: 	const scalar_t *op1 = m1.m[0], *op2 = m2.m[0];
nuclear@27: 	scalar_t *dest = res.m[0];
nuclear@27: 
nuclear@27: 	for(int i=0; i<16; i++) {
nuclear@27: 		*dest++ = *op1++ - *op2++;
nuclear@27: 	}
nuclear@27: 	return res;
nuclear@27: }
nuclear@27: 
nuclear@27: void operator +=(Matrix4x4 &m1, const Matrix4x4 &m2)
nuclear@27: {
nuclear@27: 	scalar_t *op1 = m1.m[0];
nuclear@27: 	const scalar_t *op2 = m2.m[0];
nuclear@27: 
nuclear@27: 	for(int i=0; i<16; i++) {
nuclear@27: 		*op1++ += *op2++;
nuclear@27: 	}
nuclear@27: }
nuclear@27: 
nuclear@27: void operator -=(Matrix4x4 &m1, const Matrix4x4 &m2)
nuclear@27: {
nuclear@27: 	scalar_t *op1 = m1.m[0];
nuclear@27: 	const scalar_t *op2 = m2.m[0];
nuclear@27: 
nuclear@27: 	for(int i=0; i<16; i++) {
nuclear@27: 		*op1++ -= *op2++;
nuclear@27: 	}
nuclear@27: }
nuclear@27: 
nuclear@27: Matrix4x4 operator *(const Matrix4x4 &mat, scalar_t scalar)
nuclear@27: {
nuclear@27: 	Matrix4x4 res;
nuclear@27: 	const scalar_t *mptr = mat.m[0];
nuclear@27: 	scalar_t *dptr = res.m[0];
nuclear@27: 
nuclear@27: 	for(int i=0; i<16; i++) {
nuclear@27: 		*dptr++ = *mptr++ * scalar;
nuclear@27: 	}
nuclear@27: 	return res;
nuclear@27: }
nuclear@27: 
nuclear@27: Matrix4x4 operator *(scalar_t scalar, const Matrix4x4 &mat)
nuclear@27: {
nuclear@27: 	Matrix4x4 res;
nuclear@27: 	const scalar_t *mptr = mat.m[0];
nuclear@27: 	scalar_t *dptr = res.m[0];
nuclear@27: 
nuclear@27: 	for(int i=0; i<16; i++) {
nuclear@27: 		*dptr++ = *mptr++ * scalar;
nuclear@27: 	}
nuclear@27: 	return res;
nuclear@27: }
nuclear@27: 
nuclear@27: void operator *=(Matrix4x4 &mat, scalar_t scalar)
nuclear@27: {
nuclear@27: 	scalar_t *mptr = mat.m[0];
nuclear@27: 
nuclear@27: 	for(int i=0; i<16; i++) {
nuclear@27: 		*mptr++ *= scalar;
nuclear@27: 	}
nuclear@27: }
nuclear@27: 
nuclear@27: void Matrix4x4::translate(const Vector3 &trans)
nuclear@27: {
nuclear@27: 	Matrix4x4 tmat(1, 0, 0, trans.x, 0, 1, 0, trans.y, 0, 0, 1, trans.z, 0, 0, 0, 1);
nuclear@27: 	*this *= tmat;
nuclear@27: }
nuclear@27: 
nuclear@27: void Matrix4x4::set_translation(const Vector3 &trans)
nuclear@27: {
nuclear@27: 	*this = Matrix4x4(1, 0, 0, trans.x, 0, 1, 0, trans.y, 0, 0, 1, trans.z, 0, 0, 0, 1);
nuclear@27: }
nuclear@27: 
nuclear@27: Vector3 Matrix4x4::get_translation() const
nuclear@27: {
nuclear@27: 	return Vector3(m[0][3], m[1][3], m[2][3]);
nuclear@27: }
nuclear@27: 
nuclear@27: void Matrix4x4::rotate(const Vector3 &euler_angles)
nuclear@27: {
nuclear@27: 	Matrix3x3 xrot, yrot, zrot;
nuclear@27: 
nuclear@27: 	xrot = Matrix3x3(	1,			0,					0,
nuclear@27: 						0,	cos(euler_angles.x),	-sin(euler_angles.x),
nuclear@27: 						0,	sin(euler_angles.x),	cos(euler_angles.x));
nuclear@27: 
nuclear@27: 	yrot = Matrix3x3(	cos(euler_angles.y),	0,	sin(euler_angles.y),
nuclear@27: 								0,				1,				0,
nuclear@27: 						-sin(euler_angles.y),	0,	cos(euler_angles.y));
nuclear@27: 
nuclear@27: 	zrot = Matrix3x3(	cos(euler_angles.z),	-sin(euler_angles.z),	0,
nuclear@27: 						sin(euler_angles.z),	cos(euler_angles.z),	0,
nuclear@27: 								0,						0,				1);
nuclear@27: 
nuclear@27: 	*this *= Matrix4x4(xrot * yrot * zrot);
nuclear@27: }
nuclear@27: 
nuclear@27: void Matrix4x4::set_rotation(const Vector3 &euler_angles)
nuclear@27: {
nuclear@27: 	Matrix3x3 xrot, yrot, zrot;
nuclear@27: 
nuclear@27: 	xrot = Matrix3x3(	1,			0,					0,
nuclear@27: 						0,	cos(euler_angles.x),	-sin(euler_angles.x),
nuclear@27: 						0,	sin(euler_angles.x),	cos(euler_angles.x));
nuclear@27: 
nuclear@27: 	yrot = Matrix3x3(	cos(euler_angles.y),	0,	sin(euler_angles.y),
nuclear@27: 								0,				1,				0,
nuclear@27: 						-sin(euler_angles.y),	0,	cos(euler_angles.y));
nuclear@27: 
nuclear@27: 	zrot = Matrix3x3(	cos(euler_angles.z),	-sin(euler_angles.z),	0,
nuclear@27: 						sin(euler_angles.z),	cos(euler_angles.z),	0,
nuclear@27: 								0,						0,				1);
nuclear@27: 
nuclear@27: 	*this = Matrix4x4(xrot * yrot * zrot);
nuclear@27: }
nuclear@27: 
nuclear@27: void Matrix4x4::rotate(const Vector3 &axis, scalar_t angle)
nuclear@27: {
nuclear@27: 	scalar_t sina = (scalar_t)sin(angle);
nuclear@27: 	scalar_t cosa = (scalar_t)cos(angle);
nuclear@27: 	scalar_t invcosa = 1-cosa;
nuclear@27: 	scalar_t nxsq = axis.x * axis.x;
nuclear@27: 	scalar_t nysq = axis.y * axis.y;
nuclear@27: 	scalar_t nzsq = axis.z * axis.z;
nuclear@27: 
nuclear@27: 	Matrix4x4 xform;
nuclear@27: 	xform[0][0] = nxsq + (1-nxsq) * cosa;
nuclear@27: 	xform[0][1] = axis.x * axis.y * invcosa - axis.z * sina;
nuclear@27: 	xform[0][2] = axis.x * axis.z * invcosa + axis.y * sina;
nuclear@27: 	xform[1][0] = axis.x * axis.y * invcosa + axis.z * sina;
nuclear@27: 	xform[1][1] = nysq + (1-nysq) * cosa;
nuclear@27: 	xform[1][2] = axis.y * axis.z * invcosa - axis.x * sina;
nuclear@27: 	xform[2][0] = axis.x * axis.z * invcosa - axis.y * sina;
nuclear@27: 	xform[2][1] = axis.y * axis.z * invcosa + axis.x * sina;
nuclear@27: 	xform[2][2] = nzsq + (1-nzsq) * cosa;
nuclear@27: 
nuclear@27: 	*this *= xform;
nuclear@27: }
nuclear@27: 
nuclear@27: void Matrix4x4::set_rotation(const Vector3 &axis, scalar_t angle)
nuclear@27: {
nuclear@27: 	scalar_t sina = (scalar_t)sin(angle);
nuclear@27: 	scalar_t cosa = (scalar_t)cos(angle);
nuclear@27: 	scalar_t invcosa = 1-cosa;
nuclear@27: 	scalar_t nxsq = axis.x * axis.x;
nuclear@27: 	scalar_t nysq = axis.y * axis.y;
nuclear@27: 	scalar_t nzsq = axis.z * axis.z;
nuclear@27: 
nuclear@27: 	reset_identity();
nuclear@27: 	m[0][0] = nxsq + (1-nxsq) * cosa;
nuclear@27: 	m[0][1] = axis.x * axis.y * invcosa - axis.z * sina;
nuclear@27: 	m[0][2] = axis.x * axis.z * invcosa + axis.y * sina;
nuclear@27: 	m[1][0] = axis.x * axis.y * invcosa + axis.z * sina;
nuclear@27: 	m[1][1] = nysq + (1-nysq) * cosa;
nuclear@27: 	m[1][2] = axis.y * axis.z * invcosa - axis.x * sina;
nuclear@27: 	m[2][0] = axis.x * axis.z * invcosa - axis.y * sina;
nuclear@27: 	m[2][1] = axis.y * axis.z * invcosa + axis.x * sina;
nuclear@27: 	m[2][2] = nzsq + (1-nzsq) * cosa;
nuclear@27: }
nuclear@27: 
nuclear@27: void Matrix4x4::rotate(const Quaternion &quat)
nuclear@27: {
nuclear@27: 	*this *= quat.get_rotation_matrix();
nuclear@27: }
nuclear@27: 
nuclear@27: void Matrix4x4::set_rotation(const Quaternion &quat)
nuclear@27: {
nuclear@27: 	*this = quat.get_rotation_matrix();
nuclear@27: }
nuclear@27: 
nuclear@27: Quaternion Matrix4x4::get_rotation_quat() const
nuclear@27: {
nuclear@27: 	Matrix3x3 mat3 = *this;
nuclear@27: 	return mat3.get_rotation_quat();
nuclear@27: }
nuclear@27: 
nuclear@27: void Matrix4x4::scale(const Vector4 &scale_vec)
nuclear@27: {
nuclear@27: 	Matrix4x4 smat(	scale_vec.x, 0, 0, 0,
nuclear@27: 					0, scale_vec.y, 0, 0,
nuclear@27: 					0, 0, scale_vec.z, 0,
nuclear@27: 					0, 0, 0, scale_vec.w);
nuclear@27: 	*this *= smat;
nuclear@27: }
nuclear@27: 
nuclear@27: void Matrix4x4::set_scaling(const Vector4 &scale_vec)
nuclear@27: {
nuclear@27: 	*this = Matrix4x4(	scale_vec.x, 0, 0, 0,
nuclear@27: 						0, scale_vec.y, 0, 0,
nuclear@27: 						0, 0, scale_vec.z, 0,
nuclear@27: 						0, 0, 0, scale_vec.w);
nuclear@27: }
nuclear@27: 
nuclear@27: Vector3 Matrix4x4::get_scaling() const
nuclear@27: {
nuclear@27: 	Vector3 vi = get_row_vector(0);
nuclear@27: 	Vector3 vj = get_row_vector(1);
nuclear@27: 	Vector3 vk = get_row_vector(2);
nuclear@27: 
nuclear@27: 	return Vector3(vi.length(), vj.length(), vk.length());
nuclear@27: }
nuclear@27: 
nuclear@27: void Matrix4x4::set_perspective(float vfov, float aspect, float znear, float zfar)
nuclear@27: {
nuclear@27: 	float f = 1.0f / tan(vfov * 0.5f);
nuclear@27:     float dz = znear - zfar;
nuclear@27: 
nuclear@27: 	reset_identity();
nuclear@27: 
nuclear@27: 	m[0][0] = f / aspect;
nuclear@27:     m[1][1] = f;
nuclear@27:     m[2][2] = (zfar + znear) / dz;
nuclear@27:     m[3][2] = -1.0f;
nuclear@27:     m[2][3] = 2.0f * zfar * znear / dz;
nuclear@27:     m[3][3] = 0.0f;
nuclear@27: }
nuclear@27: 
nuclear@27: void Matrix4x4::set_orthographic(float left, float right, float bottom, float top, float znear, float zfar)
nuclear@27: {
nuclear@27: 	float dx = right - left;
nuclear@27: 	float dy = top - bottom;
nuclear@27: 	float dz = zfar - znear;
nuclear@27: 
nuclear@27: 	reset_identity();
nuclear@27: 
nuclear@27: 	m[0][0] = 2.0 / dx;
nuclear@27: 	m[1][1] = 2.0 / dy;
nuclear@27: 	m[2][2] = -2.0 / dz;
nuclear@27: 	m[0][3] = -(right + left) / dx;
nuclear@27: 	m[1][3] = -(top + bottom) / dy;
nuclear@27: 	m[2][3] = -(zfar + znear) / dz;
nuclear@27: }
nuclear@27: 
nuclear@27: void Matrix4x4::set_column_vector(const Vector4 &vec, unsigned int col_index)
nuclear@27: {
nuclear@27: 	m[0][col_index] = vec.x;
nuclear@27: 	m[1][col_index] = vec.y;
nuclear@27: 	m[2][col_index] = vec.z;
nuclear@27: 	m[3][col_index] = vec.w;
nuclear@27: }
nuclear@27: 
nuclear@27: void Matrix4x4::set_row_vector(const Vector4 &vec, unsigned int row_index)
nuclear@27: {
nuclear@27: 	m[row_index][0] = vec.x;
nuclear@27: 	m[row_index][1] = vec.y;
nuclear@27: 	m[row_index][2] = vec.z;
nuclear@27: 	m[row_index][3] = vec.w;
nuclear@27: }
nuclear@27: 
nuclear@27: Vector4 Matrix4x4::get_column_vector(unsigned int col_index) const
nuclear@27: {
nuclear@27: 	return Vector4(m[0][col_index], m[1][col_index], m[2][col_index], m[3][col_index]);
nuclear@27: }
nuclear@27: 
nuclear@27: Vector4 Matrix4x4::get_row_vector(unsigned int row_index) const
nuclear@27: {
nuclear@27: 	return Vector4(m[row_index][0], m[row_index][1], m[row_index][2], m[row_index][3]);
nuclear@27: }
nuclear@27: 
nuclear@27: void Matrix4x4::transpose()
nuclear@27: {
nuclear@27: 	Matrix4x4 tmp = *this;
nuclear@27: 	for(int i=0; i<4; i++) {
nuclear@27: 		for(int j=0; j<4; j++) {
nuclear@27: 			m[i][j] = tmp[j][i];
nuclear@27: 		}
nuclear@27: 	}
nuclear@27: }
nuclear@27: 
nuclear@27: Matrix4x4 Matrix4x4::transposed() const
nuclear@27: {
nuclear@27: 	Matrix4x4 res;
nuclear@27: 	for(int i=0; i<4; i++) {
nuclear@27: 		for(int j=0; j<4; j++) {
nuclear@27: 			res[i][j] = m[j][i];
nuclear@27: 		}
nuclear@27: 	}
nuclear@27: 	return res;
nuclear@27: }
nuclear@27: 
nuclear@27: scalar_t Matrix4x4::determinant() const
nuclear@27: {
nuclear@27: 	scalar_t det11 =	(m[1][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
nuclear@27: 						(m[1][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
nuclear@27: 						(m[1][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
nuclear@27: 
nuclear@27: 	scalar_t det12 =	(m[1][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
nuclear@27: 						(m[1][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
nuclear@27: 						(m[1][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
nuclear@27: 
nuclear@27: 	scalar_t det13 =	(m[1][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
nuclear@27: 						(m[1][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
nuclear@27: 						(m[1][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
nuclear@27: 
nuclear@27: 	scalar_t det14 =	(m[1][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
nuclear@27: 						(m[1][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
nuclear@27: 						(m[1][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
nuclear@27: 
nuclear@27: 	return m[0][0] * det11 - m[0][1] * det12 + m[0][2] * det13 - m[0][3] * det14;
nuclear@27: }
nuclear@27: 
nuclear@27: 
nuclear@27: Matrix4x4 Matrix4x4::adjoint() const
nuclear@27: {
nuclear@27: 	Matrix4x4 coef;
nuclear@27: 
nuclear@27: 	coef.m[0][0] =	(m[1][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
nuclear@27: 					(m[1][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
nuclear@27: 					(m[1][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
nuclear@27: 	coef.m[0][1] =	(m[1][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
nuclear@27: 					(m[1][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
nuclear@27: 					(m[1][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
nuclear@27: 	coef.m[0][2] =	(m[1][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
nuclear@27: 					(m[1][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
nuclear@27: 					(m[1][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
nuclear@27: 	coef.m[0][3] =	(m[1][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
nuclear@27: 					(m[1][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
nuclear@27: 					(m[1][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
nuclear@27: 
nuclear@27: 	coef.m[1][0] =	(m[0][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
nuclear@27: 					(m[0][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
nuclear@27: 					(m[0][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
nuclear@27: 	coef.m[1][1] =	(m[0][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
nuclear@27: 					(m[0][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
nuclear@27: 					(m[0][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
nuclear@27: 	coef.m[1][2] =	(m[0][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
nuclear@27: 					(m[0][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
nuclear@27: 					(m[0][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
nuclear@27: 	coef.m[1][3] =	(m[0][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
nuclear@27: 					(m[0][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
nuclear@27: 					(m[0][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
nuclear@27: 
nuclear@27: 	coef.m[2][0] =	(m[0][1] * (m[1][2] * m[3][3] - m[3][2] * m[1][3])) -
nuclear@27: 					(m[0][2] * (m[1][1] * m[3][3] - m[3][1] * m[1][3])) +
nuclear@27: 					(m[0][3] * (m[1][1] * m[3][2] - m[3][1] * m[1][2]));
nuclear@27: 	coef.m[2][1] =	(m[0][0] * (m[1][2] * m[3][3] - m[3][2] * m[1][3])) -
nuclear@27: 					(m[0][2] * (m[1][0] * m[3][3] - m[3][0] * m[1][3])) +
nuclear@27: 					(m[0][3] * (m[1][0] * m[3][2] - m[3][0] * m[1][2]));
nuclear@27: 	coef.m[2][2] =	(m[0][0] * (m[1][1] * m[3][3] - m[3][1] * m[1][3])) -
nuclear@27: 					(m[0][1] * (m[1][0] * m[3][3] - m[3][0] * m[1][3])) +
nuclear@27: 					(m[0][3] * (m[1][0] * m[3][1] - m[3][0] * m[1][1]));
nuclear@27: 	coef.m[2][3] =	(m[0][0] * (m[1][1] * m[3][2] - m[3][1] * m[1][2])) -
nuclear@27: 					(m[0][1] * (m[1][0] * m[3][2] - m[3][0] * m[1][2])) +
nuclear@27: 					(m[0][2] * (m[1][0] * m[3][1] - m[3][0] * m[1][1]));
nuclear@27: 
nuclear@27: 	coef.m[3][0] =	(m[0][1] * (m[1][2] * m[2][3] - m[2][2] * m[1][3])) -
nuclear@27: 					(m[0][2] * (m[1][1] * m[2][3] - m[2][1] * m[1][3])) +
nuclear@27: 					(m[0][3] * (m[1][1] * m[2][2] - m[2][1] * m[1][2]));
nuclear@27: 	coef.m[3][1] =	(m[0][0] * (m[1][2] * m[2][3] - m[2][2] * m[1][3])) -
nuclear@27: 					(m[0][2] * (m[1][0] * m[2][3] - m[2][0] * m[1][3])) +
nuclear@27: 					(m[0][3] * (m[1][0] * m[2][2] - m[2][0] * m[1][2]));
nuclear@27: 	coef.m[3][2] =	(m[0][0] * (m[1][1] * m[2][3] - m[2][1] * m[1][3])) -
nuclear@27: 					(m[0][1] * (m[1][0] * m[2][3] - m[2][0] * m[1][3])) +
nuclear@27: 					(m[0][3] * (m[1][0] * m[2][1] - m[2][0] * m[1][1]));
nuclear@27: 	coef.m[3][3] =	(m[0][0] * (m[1][1] * m[2][2] - m[2][1] * m[1][2])) -
nuclear@27: 					(m[0][1] * (m[1][0] * m[2][2] - m[2][0] * m[1][2])) +
nuclear@27: 					(m[0][2] * (m[1][0] * m[2][1] - m[2][0] * m[1][1]));
nuclear@27: 
nuclear@27: 	coef.transpose();
nuclear@27: 
nuclear@27: 	for(int i=0; i<4; i++) {
nuclear@27: 		for(int j=0; j<4; j++) {
nuclear@27: 			coef.m[i][j] = j%2 ? -coef.m[i][j] : coef.m[i][j];
nuclear@27: 			if(i%2) coef.m[i][j] = -coef.m[i][j];
nuclear@27: 		}
nuclear@27: 	}
nuclear@27: 
nuclear@27: 	return coef;
nuclear@27: }
nuclear@27: 
nuclear@27: Matrix4x4 Matrix4x4::inverse() const
nuclear@27: {
nuclear@27: 	Matrix4x4 adj = adjoint();
nuclear@27: 
nuclear@27: 	return adj * (1.0f / determinant());
nuclear@27: }
nuclear@27: 
nuclear@27: ostream &operator <<(ostream &out, const Matrix4x4 &mat)
nuclear@27: {
nuclear@27: 	for(int i=0; i<4; i++) {
nuclear@27: 		char str[100];
nuclear@27: 		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]);
nuclear@27: 		out << str;
nuclear@27: 	}
nuclear@27: 	return out;
nuclear@27: }