nuclear@13: #include <string.h>
nuclear@13: #include "matrix.h"
nuclear@13: 
nuclear@13: #define M(x, y)	((y) * 4 + (x))
nuclear@13: 
nuclear@13: Matrix4x4::Matrix4x4()
nuclear@13: {
nuclear@13: 	memset(m, 0, sizeof m);
nuclear@13: 	m[0] = m[5] = m[10] = m[15] = 1.0;
nuclear@13: }
nuclear@13: 
nuclear@13: Matrix4x4::Matrix4x4(const float *mat)
nuclear@13: {
nuclear@13: 	memcpy(m, mat, sizeof m);
nuclear@13: }
nuclear@13: 
nuclear@13: Matrix4x4::Matrix4x4(float m00, float m01, float m02, float m03,
nuclear@13: 		float m10, float m11, float m12, float m13,
nuclear@13: 		float m20, float m21, float m22, float m23,
nuclear@13: 		float m30, float m31, float m32, float m33)
nuclear@13: {
nuclear@13: 	m[M(0, 0)] = m00; m[M(0, 1)] = m01; m[M(0, 2)] = m02; m[M(0, 3)] = m03;
nuclear@13: 	m[M(1, 0)] = m10; m[M(1, 1)] = m11; m[M(1, 2)] = m12; m[M(1, 3)] = m13;
nuclear@13: 	m[M(2, 0)] = m20; m[M(2, 1)] = m21; m[M(2, 2)] = m22; m[M(2, 3)] = m23;
nuclear@13: 	m[M(3, 0)] = m30; m[M(3, 1)] = m31; m[M(3, 2)] = m32; m[M(3, 3)] = m33;
nuclear@13: }
nuclear@13: 
nuclear@13: Matrix4x4::Matrix4x4(const Matrix4x4 &mat)
nuclear@13: {
nuclear@13: 	memcpy(m, mat.m, sizeof m);
nuclear@13: }
nuclear@13: 
nuclear@13: Matrix4x4 &Matrix4x4::operator =(const Matrix4x4 &mat)
nuclear@13: {
nuclear@13: 	memcpy(m, mat.m, sizeof m);
nuclear@13: 	return *this;
nuclear@13: }
nuclear@13: 
nuclear@13: void Matrix4x4::identity()
nuclear@13: {
nuclear@13: 	memset(m, 0, sizeof m);
nuclear@13: 	m[0] = m[5] = m[10] = m[15] = 1.0;
nuclear@13: }
nuclear@13: 
nuclear@13: float Matrix4x4::determinant() const
nuclear@13: {
nuclear@13: 	float det11 =	(m[M(1, 1)] * (m[M(2, 2)] * m[M(3, 3)] - m[M(3, 2)] * m[M(2, 3)])) -
nuclear@13: 					(m[M(1, 2)] * (m[M(2, 1)] * m[M(3, 3)] - m[M(3, 1)] * m[M(2, 3)])) +
nuclear@13: 					(m[M(1, 3)] * (m[M(2, 1)] * m[M(3, 2)] - m[M(3, 1)] * m[M(2, 2)]));
nuclear@13: 
nuclear@13: 	float det12 =	(m[M(1, 0)] * (m[M(2, 2)] * m[M(3, 3)] - m[M(3, 2)] * m[M(2, 3)])) -
nuclear@13: 					(m[M(1, 2)] * (m[M(2, 0)] * m[M(3, 3)] - m[M(3, 0)] * m[M(2, 3)])) +
nuclear@13: 					(m[M(1, 3)] * (m[M(2, 0)] * m[M(3, 2)] - m[M(3, 0)] * m[M(2, 2)]));
nuclear@13: 
nuclear@13: 	float det13 =	(m[M(1, 0)] * (m[M(2, 1)] * m[M(3, 3)] - m[M(3, 1)] * m[M(2, 3)])) -
nuclear@13: 					(m[M(1, 1)] * (m[M(2, 0)] * m[M(3, 3)] - m[M(3, 0)] * m[M(2, 3)])) +
nuclear@13: 					(m[M(1, 3)] * (m[M(2, 0)] * m[M(3, 1)] - m[M(3, 0)] * m[M(2, 1)]));
nuclear@13: 
nuclear@13: 	float det14 =	(m[M(1, 0)] * (m[M(2, 1)] * m[M(3, 2)] - m[M(3, 1)] * m[M(2, 2)])) -
nuclear@13: 					(m[M(1, 1)] * (m[M(2, 0)] * m[M(3, 2)] - m[M(3, 0)] * m[M(2, 2)])) +
nuclear@13: 					(m[M(1, 2)] * (m[M(2, 0)] * m[M(3, 1)] - m[M(3, 0)] * m[M(2, 1)]));
nuclear@13: 
nuclear@13: 	return m[M(0, 0)] * det11 - m[M(0, 1)] * det12 + m[M(0, 2)] * det13 - m[M(0, 3)] * det14;
nuclear@13: }
nuclear@13: 
nuclear@13: Matrix4x4 Matrix4x4::adjoint() const
nuclear@13: {
nuclear@13: 	Matrix4x4 coef;
nuclear@13: 
nuclear@13: 	coef.m[M(0, 0)] =	(m[M(1, 1)] * (m[M(2, 2)] * m[M(3, 3)] - m[M(3, 2)] * m[M(2, 3)])) -
nuclear@13: 						(m[M(1, 2)] * (m[M(2, 1)] * m[M(3, 3)] - m[M(3, 1)] * m[M(2, 3)])) +
nuclear@13: 						(m[M(1, 3)] * (m[M(2, 1)] * m[M(3, 2)] - m[M(3, 1)] * m[M(2, 2)]));
nuclear@13: 	coef.m[M(0, 1)] =	(m[M(1, 0)] * (m[M(2, 2)] * m[M(3, 3)] - m[M(3, 2)] * m[M(2, 3)])) -
nuclear@13: 						(m[M(1, 2)] * (m[M(2, 0)] * m[M(3, 3)] - m[M(3, 0)] * m[M(2, 3)])) +
nuclear@13: 						(m[M(1, 3)] * (m[M(2, 0)] * m[M(3, 2)] - m[M(3, 0)] * m[M(2, 2)]));
nuclear@13: 	coef.m[M(0, 2)] =	(m[M(1, 0)] * (m[M(2, 1)] * m[M(3, 3)] - m[M(3, 1)] * m[M(2, 3)])) -
nuclear@13: 						(m[M(1, 1)] * (m[M(2, 0)] * m[M(3, 3)] - m[M(3, 0)] * m[M(2, 3)])) +
nuclear@13: 						(m[M(1, 3)] * (m[M(2, 0)] * m[M(3, 1)] - m[M(3, 0)] * m[M(2, 1)]));
nuclear@13: 	coef.m[M(0, 3)] =	(m[M(1, 0)] * (m[M(2, 1)] * m[M(3, 2)] - m[M(3, 1)] * m[M(2, 2)])) -
nuclear@13: 						(m[M(1, 1)] * (m[M(2, 0)] * m[M(3, 2)] - m[M(3, 0)] * m[M(2, 2)])) +
nuclear@13: 						(m[M(1, 2)] * (m[M(2, 0)] * m[M(3, 1)] - m[M(3, 0)] * m[M(2, 1)]));
nuclear@13: 
nuclear@13: 	coef.m[M(1, 0)] =	(m[M(0, 1)] * (m[M(2, 2)] * m[M(3, 3)] - m[M(3, 2)] * m[M(2, 3)])) -
nuclear@13: 						(m[M(0, 2)] * (m[M(2, 1)] * m[M(3, 3)] - m[M(3, 1)] * m[M(2, 3)])) +
nuclear@13: 						(m[M(0, 3)] * (m[M(2, 1)] * m[M(3, 2)] - m[M(3, 1)] * m[M(2, 2)]));
nuclear@13: 	coef.m[M(1, 1)] =	(m[M(0, 0)] * (m[M(2, 2)] * m[M(3, 3)] - m[M(3, 2)] * m[M(2, 3)])) -
nuclear@13: 						(m[M(0, 2)] * (m[M(2, 0)] * m[M(3, 3)] - m[M(3, 0)] * m[M(2, 3)])) +
nuclear@13: 						(m[M(0, 3)] * (m[M(2, 0)] * m[M(3, 2)] - m[M(3, 0)] * m[M(2, 2)]));
nuclear@13: 	coef.m[M(1, 2)] =	(m[M(0, 0)] * (m[M(2, 1)] * m[M(3, 3)] - m[M(3, 1)] * m[M(2, 3)])) -
nuclear@13: 						(m[M(0, 1)] * (m[M(2, 0)] * m[M(3, 3)] - m[M(3, 0)] * m[M(2, 3)])) +
nuclear@13: 						(m[M(0, 3)] * (m[M(2, 0)] * m[M(3, 1)] - m[M(3, 0)] * m[M(2, 1)]));
nuclear@13: 	coef.m[M(1, 3)] =	(m[M(0, 0)] * (m[M(2, 1)] * m[M(3, 2)] - m[M(3, 1)] * m[M(2, 2)])) -
nuclear@13: 						(m[M(0, 1)] * (m[M(2, 0)] * m[M(3, 2)] - m[M(3, 0)] * m[M(2, 2)])) +
nuclear@13: 						(m[M(0, 2)] * (m[M(2, 0)] * m[M(3, 1)] - m[M(3, 0)] * m[M(2, 1)]));
nuclear@13: 
nuclear@13: 	coef.m[M(2, 0)] =	(m[M(0, 1)] * (m[M(1, 2)] * m[M(3, 3)] - m[M(3, 2)] * m[M(1, 3)])) -
nuclear@13: 						(m[M(0, 2)] * (m[M(1, 1)] * m[M(3, 3)] - m[M(3, 1)] * m[M(1, 3)])) +
nuclear@13: 						(m[M(0, 3)] * (m[M(1, 1)] * m[M(3, 2)] - m[M(3, 1)] * m[M(1, 2)]));
nuclear@13: 	coef.m[M(2, 1)] =	(m[M(0, 0)] * (m[M(1, 2)] * m[M(3, 3)] - m[M(3, 2)] * m[M(1, 3)])) -
nuclear@13: 						(m[M(0, 2)] * (m[M(1, 0)] * m[M(3, 3)] - m[M(3, 0)] * m[M(1, 3)])) +
nuclear@13: 						(m[M(0, 3)] * (m[M(1, 0)] * m[M(3, 2)] - m[M(3, 0)] * m[M(1, 2)]));
nuclear@13: 	coef.m[M(2, 2)] =	(m[M(0, 0)] * (m[M(1, 1)] * m[M(3, 3)] - m[M(3, 1)] * m[M(1, 3)])) -
nuclear@13: 						(m[M(0, 1)] * (m[M(1, 0)] * m[M(3, 3)] - m[M(3, 0)] * m[M(1, 3)])) +
nuclear@13: 						(m[M(0, 3)] * (m[M(1, 0)] * m[M(3, 1)] - m[M(3, 0)] * m[M(1, 1)]));
nuclear@13: 	coef.m[M(2, 3)] =	(m[M(0, 0)] * (m[M(1, 1)] * m[M(3, 2)] - m[M(3, 1)] * m[M(1, 2)])) -
nuclear@13: 						(m[M(0, 1)] * (m[M(1, 0)] * m[M(3, 2)] - m[M(3, 0)] * m[M(1, 2)])) +
nuclear@13: 						(m[M(0, 2)] * (m[M(1, 0)] * m[M(3, 1)] - m[M(3, 0)] * m[M(1, 1)]));
nuclear@13: 
nuclear@13: 	coef.m[M(3, 0)] =	(m[M(0, 1)] * (m[M(1, 2)] * m[M(2, 3)] - m[M(2, 2)] * m[M(1, 3)])) -
nuclear@13: 						(m[M(0, 2)] * (m[M(1, 1)] * m[M(2, 3)] - m[M(2, 1)] * m[M(1, 3)])) +
nuclear@13: 						(m[M(0, 3)] * (m[M(1, 1)] * m[M(2, 2)] - m[M(2, 1)] * m[M(1, 2)]));
nuclear@13: 	coef.m[M(3, 1)] =	(m[M(0, 0)] * (m[M(1, 2)] * m[M(2, 3)] - m[M(2, 2)] * m[M(1, 3)])) -
nuclear@13: 						(m[M(0, 2)] * (m[M(1, 0)] * m[M(2, 3)] - m[M(2, 0)] * m[M(1, 3)])) +
nuclear@13: 						(m[M(0, 3)] * (m[M(1, 0)] * m[M(2, 2)] - m[M(2, 0)] * m[M(1, 2)]));
nuclear@13: 	coef.m[M(3, 2)] =	(m[M(0, 0)] * (m[M(1, 1)] * m[M(2, 3)] - m[M(2, 1)] * m[M(1, 3)])) -
nuclear@13: 						(m[M(0, 1)] * (m[M(1, 0)] * m[M(2, 3)] - m[M(2, 0)] * m[M(1, 3)])) +
nuclear@13: 						(m[M(0, 3)] * (m[M(1, 0)] * m[M(2, 1)] - m[M(2, 0)] * m[M(1, 1)]));
nuclear@13: 	coef.m[M(3, 3)] =	(m[M(0, 0)] * (m[M(1, 1)] * m[M(2, 2)] - m[M(2, 1)] * m[M(1, 2)])) -
nuclear@13: 						(m[M(0, 1)] * (m[M(1, 0)] * m[M(2, 2)] - m[M(2, 0)] * m[M(1, 2)])) +
nuclear@13: 						(m[M(0, 2)] * (m[M(1, 0)] * m[M(2, 1)] - m[M(2, 0)] * m[M(1, 1)]));
nuclear@13: 
nuclear@13: 	coef.transpose();
nuclear@13: 
nuclear@13: 	for(int i=0; i<4; i++) {
nuclear@13: 		for(int j=0; j<4; j++) {
nuclear@13: 			coef.m[M(i, j)] = j % 2 ? -coef.m[M(i, j)] : coef.m[M(i, j)];
nuclear@13: 			if(i % 2) coef.m[M(i, j)] = -coef.m[M(i, j)];
nuclear@13: 		}
nuclear@13: 	}
nuclear@13: 
nuclear@13: 	return coef;
nuclear@13: }
nuclear@13: 
nuclear@13: void Matrix4x4::invert()
nuclear@13: {
nuclear@13: 	Matrix4x4 adj = adjoint();
nuclear@13: 
nuclear@13: 	float det = determinant();
nuclear@13: 
nuclear@13: 	for(int i=0; i<16; i++) {
nuclear@13: 		m[i] = adj.m[i] / det;
nuclear@13: 	}
nuclear@13: }
nuclear@13: 
nuclear@13: void Matrix4x4::transpose()
nuclear@13: {
nuclear@13: 	float tmp[16];
nuclear@13: 
nuclear@13: 	memcpy(tmp, m, sizeof tmp);
nuclear@13: 	for(int i=0; i<4; i++) {
nuclear@13: 		for(int j=0; j<4; j++) {
nuclear@13: 			m[M(i, j)] = tmp[M(j, i)];
nuclear@13: 		}
nuclear@13: 	}
nuclear@13: }