1.1 --- a/src/vmath.cc	Thu Apr 10 08:42:33 2014 +0300
     1.2 +++ b/src/vmath.cc	Sat Apr 12 23:28:24 2014 +0300
     1.3 @@ -16,3 +16,109 @@
     1.4  	translate(-pos.x, -pos.y, -pos.z);
     1.5  	*this = *this * m;
     1.6  }
     1.7 +
     1.8 +void Matrix4x4::transpose()
     1.9 +{
    1.10 +	Matrix4x4 tmp = *this;
    1.11 +	for(int i=0; i<4; i++) {
    1.12 +		for(int j=0; j<4; j++) {
    1.13 +			m[i][j] = tmp[j][i];
    1.14 +		}
    1.15 +	}
    1.16 +}
    1.17 +
    1.18 +float Matrix4x4::determinant() const
    1.19 +{
    1.20 +	float det11 =	(m[1][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
    1.21 +					(m[1][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
    1.22 +					(m[1][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
    1.23 +
    1.24 +	float det12 =	(m[1][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
    1.25 +					(m[1][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
    1.26 +					(m[1][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
    1.27 +
    1.28 +	float det13 =	(m[1][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
    1.29 +					(m[1][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
    1.30 +					(m[1][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
    1.31 +
    1.32 +	float det14 =	(m[1][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
    1.33 +					(m[1][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
    1.34 +					(m[1][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
    1.35 +
    1.36 +	return m[0][0] * det11 - m[0][1] * det12 + m[0][2] * det13 - m[0][3] * det14;
    1.37 +}
    1.38 +
    1.39 +
    1.40 +Matrix4x4 Matrix4x4::adjoint() const
    1.41 +{
    1.42 +	Matrix4x4 coef;
    1.43 +
    1.44 +	coef.m[0][0] =	(m[1][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
    1.45 +					(m[1][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
    1.46 +					(m[1][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
    1.47 +	coef.m[0][1] =	(m[1][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
    1.48 +					(m[1][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
    1.49 +					(m[1][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
    1.50 +	coef.m[0][2] =	(m[1][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
    1.51 +					(m[1][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
    1.52 +					(m[1][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
    1.53 +	coef.m[0][3] =	(m[1][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
    1.54 +					(m[1][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
    1.55 +					(m[1][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
    1.56 +
    1.57 +	coef.m[1][0] =	(m[0][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
    1.58 +					(m[0][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
    1.59 +					(m[0][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
    1.60 +	coef.m[1][1] =	(m[0][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
    1.61 +					(m[0][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
    1.62 +					(m[0][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
    1.63 +	coef.m[1][2] =	(m[0][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
    1.64 +					(m[0][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
    1.65 +					(m[0][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
    1.66 +	coef.m[1][3] =	(m[0][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
    1.67 +					(m[0][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
    1.68 +					(m[0][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
    1.69 +
    1.70 +	coef.m[2][0] =	(m[0][1] * (m[1][2] * m[3][3] - m[3][2] * m[1][3])) -
    1.71 +					(m[0][2] * (m[1][1] * m[3][3] - m[3][1] * m[1][3])) +
    1.72 +					(m[0][3] * (m[1][1] * m[3][2] - m[3][1] * m[1][2]));
    1.73 +	coef.m[2][1] =	(m[0][0] * (m[1][2] * m[3][3] - m[3][2] * m[1][3])) -
    1.74 +					(m[0][2] * (m[1][0] * m[3][3] - m[3][0] * m[1][3])) +
    1.75 +					(m[0][3] * (m[1][0] * m[3][2] - m[3][0] * m[1][2]));
    1.76 +	coef.m[2][2] =	(m[0][0] * (m[1][1] * m[3][3] - m[3][1] * m[1][3])) -
    1.77 +					(m[0][1] * (m[1][0] * m[3][3] - m[3][0] * m[1][3])) +
    1.78 +					(m[0][3] * (m[1][0] * m[3][1] - m[3][0] * m[1][1]));
    1.79 +	coef.m[2][3] =	(m[0][0] * (m[1][1] * m[3][2] - m[3][1] * m[1][2])) -
    1.80 +					(m[0][1] * (m[1][0] * m[3][2] - m[3][0] * m[1][2])) +
    1.81 +					(m[0][2] * (m[1][0] * m[3][1] - m[3][0] * m[1][1]));
    1.82 +
    1.83 +	coef.m[3][0] =	(m[0][1] * (m[1][2] * m[2][3] - m[2][2] * m[1][3])) -
    1.84 +					(m[0][2] * (m[1][1] * m[2][3] - m[2][1] * m[1][3])) +
    1.85 +					(m[0][3] * (m[1][1] * m[2][2] - m[2][1] * m[1][2]));
    1.86 +	coef.m[3][1] =	(m[0][0] * (m[1][2] * m[2][3] - m[2][2] * m[1][3])) -
    1.87 +					(m[0][2] * (m[1][0] * m[2][3] - m[2][0] * m[1][3])) +
    1.88 +					(m[0][3] * (m[1][0] * m[2][2] - m[2][0] * m[1][2]));
    1.89 +	coef.m[3][2] =	(m[0][0] * (m[1][1] * m[2][3] - m[2][1] * m[1][3])) -
    1.90 +					(m[0][1] * (m[1][0] * m[2][3] - m[2][0] * m[1][3])) +
    1.91 +					(m[0][3] * (m[1][0] * m[2][1] - m[2][0] * m[1][1]));
    1.92 +	coef.m[3][3] =	(m[0][0] * (m[1][1] * m[2][2] - m[2][1] * m[1][2])) -
    1.93 +					(m[0][1] * (m[1][0] * m[2][2] - m[2][0] * m[1][2])) +
    1.94 +					(m[0][2] * (m[1][0] * m[2][1] - m[2][0] * m[1][1]));
    1.95 +
    1.96 +	coef.transpose();
    1.97 +
    1.98 +	for(int i=0; i<4; i++) {
    1.99 +		for(int j=0; j<4; j++) {
   1.100 +			coef.m[i][j] = j % 2 ? -coef.m[i][j] : coef.m[i][j];
   1.101 +			if(i % 2) coef.m[i][j] = -coef.m[i][j];
   1.102 +		}
   1.103 +	}
   1.104 +
   1.105 +	return coef;
   1.106 +}
   1.107 +
   1.108 +Matrix4x4 Matrix4x4::inverse() const
   1.109 +{
   1.110 +	Matrix4x4 adj = adjoint();
   1.111 +	return adj * (1.0f / determinant());
   1.112 +}