1.1 --- /dev/null	Thu Jan 01 00:00:00 1970 +0000
     1.2 +++ b/src/matrix.cc	Wed Aug 04 04:51:06 2010 +0100
     1.3 @@ -0,0 +1,155 @@
     1.4 +#include <string.h>
     1.5 +#include "matrix.h"
     1.6 +
     1.7 +#define M(x, y)	((y) * 4 + (x))
     1.8 +
     1.9 +Matrix4x4::Matrix4x4()
    1.10 +{
    1.11 +	memset(m, 0, sizeof m);
    1.12 +	m[0] = m[5] = m[10] = m[15] = 1.0;
    1.13 +}
    1.14 +
    1.15 +Matrix4x4::Matrix4x4(const float *mat)
    1.16 +{
    1.17 +	memcpy(m, mat, sizeof m);
    1.18 +}
    1.19 +
    1.20 +Matrix4x4::Matrix4x4(float m00, float m01, float m02, float m03,
    1.21 +		float m10, float m11, float m12, float m13,
    1.22 +		float m20, float m21, float m22, float m23,
    1.23 +		float m30, float m31, float m32, float m33)
    1.24 +{
    1.25 +	m[M(0, 0)] = m00; m[M(0, 1)] = m01; m[M(0, 2)] = m02; m[M(0, 3)] = m03;
    1.26 +	m[M(1, 0)] = m10; m[M(1, 1)] = m11; m[M(1, 2)] = m12; m[M(1, 3)] = m13;
    1.27 +	m[M(2, 0)] = m20; m[M(2, 1)] = m21; m[M(2, 2)] = m22; m[M(2, 3)] = m23;
    1.28 +	m[M(3, 0)] = m30; m[M(3, 1)] = m31; m[M(3, 2)] = m32; m[M(3, 3)] = m33;
    1.29 +}
    1.30 +
    1.31 +Matrix4x4::Matrix4x4(const Matrix4x4 &mat)
    1.32 +{
    1.33 +	memcpy(m, mat.m, sizeof m);
    1.34 +}
    1.35 +
    1.36 +Matrix4x4 &Matrix4x4::operator =(const Matrix4x4 &mat)
    1.37 +{
    1.38 +	memcpy(m, mat.m, sizeof m);
    1.39 +	return *this;
    1.40 +}
    1.41 +
    1.42 +void Matrix4x4::identity()
    1.43 +{
    1.44 +	memset(m, 0, sizeof m);
    1.45 +	m[0] = m[5] = m[10] = m[15] = 1.0;
    1.46 +}
    1.47 +
    1.48 +float Matrix4x4::determinant() const
    1.49 +{
    1.50 +	float det11 =	(m[M(1, 1)] * (m[M(2, 2)] * m[M(3, 3)] - m[M(3, 2)] * m[M(2, 3)])) -
    1.51 +					(m[M(1, 2)] * (m[M(2, 1)] * m[M(3, 3)] - m[M(3, 1)] * m[M(2, 3)])) +
    1.52 +					(m[M(1, 3)] * (m[M(2, 1)] * m[M(3, 2)] - m[M(3, 1)] * m[M(2, 2)]));
    1.53 +
    1.54 +	float det12 =	(m[M(1, 0)] * (m[M(2, 2)] * m[M(3, 3)] - m[M(3, 2)] * m[M(2, 3)])) -
    1.55 +					(m[M(1, 2)] * (m[M(2, 0)] * m[M(3, 3)] - m[M(3, 0)] * m[M(2, 3)])) +
    1.56 +					(m[M(1, 3)] * (m[M(2, 0)] * m[M(3, 2)] - m[M(3, 0)] * m[M(2, 2)]));
    1.57 +
    1.58 +	float det13 =	(m[M(1, 0)] * (m[M(2, 1)] * m[M(3, 3)] - m[M(3, 1)] * m[M(2, 3)])) -
    1.59 +					(m[M(1, 1)] * (m[M(2, 0)] * m[M(3, 3)] - m[M(3, 0)] * m[M(2, 3)])) +
    1.60 +					(m[M(1, 3)] * (m[M(2, 0)] * m[M(3, 1)] - m[M(3, 0)] * m[M(2, 1)]));
    1.61 +
    1.62 +	float det14 =	(m[M(1, 0)] * (m[M(2, 1)] * m[M(3, 2)] - m[M(3, 1)] * m[M(2, 2)])) -
    1.63 +					(m[M(1, 1)] * (m[M(2, 0)] * m[M(3, 2)] - m[M(3, 0)] * m[M(2, 2)])) +
    1.64 +					(m[M(1, 2)] * (m[M(2, 0)] * m[M(3, 1)] - m[M(3, 0)] * m[M(2, 1)]));
    1.65 +
    1.66 +	return m[M(0, 0)] * det11 - m[M(0, 1)] * det12 + m[M(0, 2)] * det13 - m[M(0, 3)] * det14;
    1.67 +}
    1.68 +
    1.69 +Matrix4x4 Matrix4x4::adjoint() const
    1.70 +{
    1.71 +	Matrix4x4 coef;
    1.72 +
    1.73 +	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)])) -
    1.74 +						(m[M(1, 2)] * (m[M(2, 1)] * m[M(3, 3)] - m[M(3, 1)] * m[M(2, 3)])) +
    1.75 +						(m[M(1, 3)] * (m[M(2, 1)] * m[M(3, 2)] - m[M(3, 1)] * m[M(2, 2)]));
    1.76 +	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)])) -
    1.77 +						(m[M(1, 2)] * (m[M(2, 0)] * m[M(3, 3)] - m[M(3, 0)] * m[M(2, 3)])) +
    1.78 +						(m[M(1, 3)] * (m[M(2, 0)] * m[M(3, 2)] - m[M(3, 0)] * m[M(2, 2)]));
    1.79 +	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)])) -
    1.80 +						(m[M(1, 1)] * (m[M(2, 0)] * m[M(3, 3)] - m[M(3, 0)] * m[M(2, 3)])) +
    1.81 +						(m[M(1, 3)] * (m[M(2, 0)] * m[M(3, 1)] - m[M(3, 0)] * m[M(2, 1)]));
    1.82 +	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)])) -
    1.83 +						(m[M(1, 1)] * (m[M(2, 0)] * m[M(3, 2)] - m[M(3, 0)] * m[M(2, 2)])) +
    1.84 +						(m[M(1, 2)] * (m[M(2, 0)] * m[M(3, 1)] - m[M(3, 0)] * m[M(2, 1)]));
    1.85 +
    1.86 +	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)])) -
    1.87 +						(m[M(0, 2)] * (m[M(2, 1)] * m[M(3, 3)] - m[M(3, 1)] * m[M(2, 3)])) +
    1.88 +						(m[M(0, 3)] * (m[M(2, 1)] * m[M(3, 2)] - m[M(3, 1)] * m[M(2, 2)]));
    1.89 +	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)])) -
    1.90 +						(m[M(0, 2)] * (m[M(2, 0)] * m[M(3, 3)] - m[M(3, 0)] * m[M(2, 3)])) +
    1.91 +						(m[M(0, 3)] * (m[M(2, 0)] * m[M(3, 2)] - m[M(3, 0)] * m[M(2, 2)]));
    1.92 +	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)])) -
    1.93 +						(m[M(0, 1)] * (m[M(2, 0)] * m[M(3, 3)] - m[M(3, 0)] * m[M(2, 3)])) +
    1.94 +						(m[M(0, 3)] * (m[M(2, 0)] * m[M(3, 1)] - m[M(3, 0)] * m[M(2, 1)]));
    1.95 +	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)])) -
    1.96 +						(m[M(0, 1)] * (m[M(2, 0)] * m[M(3, 2)] - m[M(3, 0)] * m[M(2, 2)])) +
    1.97 +						(m[M(0, 2)] * (m[M(2, 0)] * m[M(3, 1)] - m[M(3, 0)] * m[M(2, 1)]));
    1.98 +
    1.99 +	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)])) -
   1.100 +						(m[M(0, 2)] * (m[M(1, 1)] * m[M(3, 3)] - m[M(3, 1)] * m[M(1, 3)])) +
   1.101 +						(m[M(0, 3)] * (m[M(1, 1)] * m[M(3, 2)] - m[M(3, 1)] * m[M(1, 2)]));
   1.102 +	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)])) -
   1.103 +						(m[M(0, 2)] * (m[M(1, 0)] * m[M(3, 3)] - m[M(3, 0)] * m[M(1, 3)])) +
   1.104 +						(m[M(0, 3)] * (m[M(1, 0)] * m[M(3, 2)] - m[M(3, 0)] * m[M(1, 2)]));
   1.105 +	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)])) -
   1.106 +						(m[M(0, 1)] * (m[M(1, 0)] * m[M(3, 3)] - m[M(3, 0)] * m[M(1, 3)])) +
   1.107 +						(m[M(0, 3)] * (m[M(1, 0)] * m[M(3, 1)] - m[M(3, 0)] * m[M(1, 1)]));
   1.108 +	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)])) -
   1.109 +						(m[M(0, 1)] * (m[M(1, 0)] * m[M(3, 2)] - m[M(3, 0)] * m[M(1, 2)])) +
   1.110 +						(m[M(0, 2)] * (m[M(1, 0)] * m[M(3, 1)] - m[M(3, 0)] * m[M(1, 1)]));
   1.111 +
   1.112 +	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)])) -
   1.113 +						(m[M(0, 2)] * (m[M(1, 1)] * m[M(2, 3)] - m[M(2, 1)] * m[M(1, 3)])) +
   1.114 +						(m[M(0, 3)] * (m[M(1, 1)] * m[M(2, 2)] - m[M(2, 1)] * m[M(1, 2)]));
   1.115 +	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)])) -
   1.116 +						(m[M(0, 2)] * (m[M(1, 0)] * m[M(2, 3)] - m[M(2, 0)] * m[M(1, 3)])) +
   1.117 +						(m[M(0, 3)] * (m[M(1, 0)] * m[M(2, 2)] - m[M(2, 0)] * m[M(1, 2)]));
   1.118 +	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)])) -
   1.119 +						(m[M(0, 1)] * (m[M(1, 0)] * m[M(2, 3)] - m[M(2, 0)] * m[M(1, 3)])) +
   1.120 +						(m[M(0, 3)] * (m[M(1, 0)] * m[M(2, 1)] - m[M(2, 0)] * m[M(1, 1)]));
   1.121 +	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)])) -
   1.122 +						(m[M(0, 1)] * (m[M(1, 0)] * m[M(2, 2)] - m[M(2, 0)] * m[M(1, 2)])) +
   1.123 +						(m[M(0, 2)] * (m[M(1, 0)] * m[M(2, 1)] - m[M(2, 0)] * m[M(1, 1)]));
   1.124 +
   1.125 +	coef.transpose();
   1.126 +
   1.127 +	for(int i=0; i<4; i++) {
   1.128 +		for(int j=0; j<4; j++) {
   1.129 +			coef.m[M(i, j)] = j % 2 ? -coef.m[M(i, j)] : coef.m[M(i, j)];
   1.130 +			if(i % 2) coef.m[M(i, j)] = -coef.m[M(i, j)];
   1.131 +		}
   1.132 +	}
   1.133 +
   1.134 +	return coef;
   1.135 +}
   1.136 +
   1.137 +void Matrix4x4::invert()
   1.138 +{
   1.139 +	Matrix4x4 adj = adjoint();
   1.140 +
   1.141 +	float det = determinant();
   1.142 +
   1.143 +	for(int i=0; i<16; i++) {
   1.144 +		m[i] = adj.m[i] / det;
   1.145 +	}
   1.146 +}
   1.147 +
   1.148 +void Matrix4x4::transpose()
   1.149 +{
   1.150 +	float tmp[16];
   1.151 +
   1.152 +	memcpy(tmp, m, sizeof tmp);
   1.153 +	for(int i=0; i<4; i++) {
   1.154 +		for(int j=0; j<4; j++) {
   1.155 +			m[M(i, j)] = tmp[M(j, i)];
   1.156 +		}
   1.157 +	}
   1.158 +}