1.1 --- /dev/null	Thu Jan 01 00:00:00 1970 +0000
     1.2 +++ b/src/matrix.cc	Sat Jan 11 17:22:36 2014 +0200
     1.3 @@ -0,0 +1,393 @@
     1.4 +#include <math.h>
     1.5 +#include <stdio.h>
     1.6 +#include <string.h>
     1.7 +#include "matrix.h"
     1.8 +
     1.9 +Matrix4x4 Matrix4x4::identity = Matrix4x4(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1);
    1.10 +
    1.11 +Matrix4x4::Matrix4x4()
    1.12 +{
    1.13 +	*this = identity;
    1.14 +}
    1.15 +
    1.16 +Matrix4x4::Matrix4x4(	float m11, float m12, float m13, float m14,
    1.17 +						float m21, float m22, float m23, float m24,
    1.18 +						float m31, float m32, float m33, float m34,
    1.19 +						float m41, float m42, float m43, float m44)
    1.20 +{
    1.21 +	m[0][0] = m11; m[0][1] = m12; m[0][2] = m13; m[0][3] = m14;
    1.22 +	m[1][0] = m21; m[1][1] = m22; m[1][2] = m23; m[1][3] = m24;
    1.23 +	m[2][0] = m31; m[2][1] = m32; m[2][2] = m33; m[2][3] = m34;
    1.24 +	m[3][0] = m41; m[3][1] = m42; m[3][2] = m43; m[3][3] = m44;
    1.25 +}
    1.26 +
    1.27 +Matrix4x4 operator +(const Matrix4x4 &m1, const Matrix4x4 &m2)
    1.28 +{
    1.29 +	Matrix4x4 res;
    1.30 +	const float *op1 = m1.m[0], *op2 = m2.m[0];
    1.31 +	float *dest = res.m[0];
    1.32 +
    1.33 +	for(int i=0; i<16; i++) {
    1.34 +		*dest++ = *op1++ + *op2++;
    1.35 +	}
    1.36 +	return res;
    1.37 +}
    1.38 +
    1.39 +Matrix4x4 operator -(const Matrix4x4 &m1, const Matrix4x4 &m2)
    1.40 +{
    1.41 +	Matrix4x4 res;
    1.42 +	const float *op1 = m1.m[0], *op2 = m2.m[0];
    1.43 +	float *dest = res.m[0];
    1.44 +
    1.45 +	for(int i=0; i<16; i++) {
    1.46 +		*dest++ = *op1++ - *op2++;
    1.47 +	}
    1.48 +	return res;
    1.49 +}
    1.50 +
    1.51 +Matrix4x4 operator *(const Matrix4x4 &m1, const Matrix4x4 &m2) {
    1.52 +	Matrix4x4 res;
    1.53 +
    1.54 +	for(int i=0; i<4; i++) {
    1.55 +		for(int j=0; j<4; j++) {
    1.56 +			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];
    1.57 +		}
    1.58 +	}
    1.59 +
    1.60 +	return res;
    1.61 +}
    1.62 +
    1.63 +void operator +=(Matrix4x4 &m1, const Matrix4x4 &m2)
    1.64 +{
    1.65 +	float *op1 = m1.m[0];
    1.66 +	const float *op2 = m2.m[0];
    1.67 +
    1.68 +	for(int i=0; i<16; i++) {
    1.69 +		*op1++ += *op2++;
    1.70 +	}
    1.71 +}
    1.72 +
    1.73 +void operator -=(Matrix4x4 &m1, const Matrix4x4 &m2)
    1.74 +{
    1.75 +	float *op1 = m1.m[0];
    1.76 +	const float *op2 = m2.m[0];
    1.77 +
    1.78 +	for(int i=0; i<16; i++) {
    1.79 +		*op1++ -= *op2++;
    1.80 +	}
    1.81 +}
    1.82 +
    1.83 +void operator *=(Matrix4x4 &m1, const Matrix4x4 &m2)
    1.84 +{
    1.85 +	Matrix4x4 res;
    1.86 +	for(int i=0; i<4; i++) {
    1.87 +		for(int j=0; j<4; j++) {
    1.88 +			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];
    1.89 +		}
    1.90 +	}
    1.91 +	memcpy(m1.m, res.m, 16 * sizeof(float));
    1.92 +}
    1.93 +
    1.94 +Matrix4x4 operator *(const Matrix4x4 &mat, float scalar)
    1.95 +{
    1.96 +	Matrix4x4 res;
    1.97 +	const float *mptr = mat.m[0];
    1.98 +	float *dptr = res.m[0];
    1.99 +
   1.100 +	for(int i=0; i<16; i++) {
   1.101 +		*dptr++ = *mptr++ * scalar;
   1.102 +	}
   1.103 +	return res;
   1.104 +}
   1.105 +
   1.106 +Matrix4x4 operator *(float scalar, const Matrix4x4 &mat)
   1.107 +{
   1.108 +	Matrix4x4 res;
   1.109 +	const float *mptr = mat.m[0];
   1.110 +	float *dptr = res.m[0];
   1.111 +
   1.112 +	for(int i=0; i<16; i++) {
   1.113 +		*dptr++ = *mptr++ * scalar;
   1.114 +	}
   1.115 +	return res;
   1.116 +}
   1.117 +
   1.118 +void operator *=(Matrix4x4 &mat, float scalar)
   1.119 +{
   1.120 +	float *mptr = mat.m[0];
   1.121 +
   1.122 +	for(int i=0; i<16; i++) {
   1.123 +		*mptr++ *= scalar;
   1.124 +	}
   1.125 +}
   1.126 +
   1.127 +void Matrix4x4::translate(const Vector3 &trans)
   1.128 +{
   1.129 +	Matrix4x4 tmat(1, 0, 0, trans.x, 0, 1, 0, trans.y, 0, 0, 1, trans.z, 0, 0, 0, 1);
   1.130 +	*this *= tmat;
   1.131 +}
   1.132 +
   1.133 +void Matrix4x4::set_translation(const Vector3 &trans)
   1.134 +{
   1.135 +	*this = Matrix4x4(1, 0, 0, trans.x, 0, 1, 0, trans.y, 0, 0, 1, trans.z, 0, 0, 0, 1);
   1.136 +}
   1.137 +
   1.138 +void Matrix4x4::rotate(const Vector3 &euler)
   1.139 +{
   1.140 +	float sinx = sin(euler.x);
   1.141 +	float cosx = cos(euler.x);
   1.142 +	float siny = sin(euler.y);
   1.143 +	float cosy = cos(euler.y);
   1.144 +	float sinz = sin(euler.z);
   1.145 +	float cosz = cos(euler.z);
   1.146 +
   1.147 +	Matrix4x4 xrot = Matrix4x4(
   1.148 +			1, 0, 0, 0,
   1.149 +			0, cosx, sinx, 0,
   1.150 +			0, -sinx, cosx, 0,
   1.151 +			0, 0, 0, 1);
   1.152 +
   1.153 +	Matrix4x4 yrot = Matrix4x4(
   1.154 +			cosy, 0, -siny, 0,
   1.155 +			0, 1, 0, 0,
   1.156 +			siny, 0, cosy, 0,
   1.157 +			0, 0, 0, 1);
   1.158 +
   1.159 +	Matrix4x4 zrot = Matrix4x4(
   1.160 +			cosz, sinz, 0, 0,
   1.161 +			-sinz, cosz, 0, 0,
   1.162 +			0, 0, 1, 0,
   1.163 +			0, 0, 0, 1);
   1.164 +
   1.165 +	*this *= (xrot * yrot * zrot);
   1.166 +}
   1.167 +
   1.168 +void Matrix4x4::rotate(const Vector3 &axis, float angle)
   1.169 +{
   1.170 +	float sina = (float)sin(angle);
   1.171 +	float cosa = (float)cos(angle);
   1.172 +	float invcosa = 1-cosa;
   1.173 +	float nxsq = axis.x * axis.x;
   1.174 +	float nysq = axis.y * axis.y;
   1.175 +	float nzsq = axis.z * axis.z;
   1.176 +
   1.177 +	Matrix4x4 xform;
   1.178 +	xform[0][0] = nxsq + (1-nxsq) * cosa;
   1.179 +	xform[0][1] = axis.x * axis.y * invcosa - axis.z * sina;
   1.180 +	xform[0][2] = axis.x * axis.z * invcosa + axis.y * sina;
   1.181 +	xform[1][0] = axis.x * axis.y * invcosa + axis.z * sina;
   1.182 +	xform[1][1] = nysq + (1-nysq) * cosa;
   1.183 +	xform[1][2] = axis.y * axis.z * invcosa - axis.x * sina;
   1.184 +	xform[2][0] = axis.x * axis.z * invcosa - axis.y * sina;
   1.185 +	xform[2][1] = axis.y * axis.z * invcosa + axis.x * sina;
   1.186 +	xform[2][2] = nzsq + (1-nzsq) * cosa;
   1.187 +
   1.188 +	*this *= Matrix4x4(xform);
   1.189 +}
   1.190 +
   1.191 +void Matrix4x4::set_rotation(const Vector3 &axis, float angle)
   1.192 +{
   1.193 +	float sina = (float)sin(angle);
   1.194 +	float cosa = (float)cos(angle);
   1.195 +	float invcosa = 1-cosa;
   1.196 +	float nxsq = axis.x * axis.x;
   1.197 +	float nysq = axis.y * axis.y;
   1.198 +	float nzsq = axis.z * axis.z;
   1.199 +
   1.200 +	reset_identity();
   1.201 +	m[0][0] = nxsq + (1-nxsq) * cosa;
   1.202 +	m[0][1] = axis.x * axis.y * invcosa - axis.z * sina;
   1.203 +	m[0][2] = axis.x * axis.z * invcosa + axis.y * sina;
   1.204 +	m[1][0] = axis.x * axis.y * invcosa + axis.z * sina;
   1.205 +	m[1][1] = nysq + (1-nysq) * cosa;
   1.206 +	m[1][2] = axis.y * axis.z * invcosa - axis.x * sina;
   1.207 +	m[2][0] = axis.x * axis.z * invcosa - axis.y * sina;
   1.208 +	m[2][1] = axis.y * axis.z * invcosa + axis.x * sina;
   1.209 +	m[2][2] = nzsq + (1-nzsq) * cosa;
   1.210 +}
   1.211 +
   1.212 +void Matrix4x4::scale(const Vector3 &scale_vec)
   1.213 +{
   1.214 +	Matrix4x4 smat(	scale_vec.x, 0, 0, 0,
   1.215 +					0, scale_vec.y, 0, 0,
   1.216 +					0, 0, scale_vec.z, 0,
   1.217 +					0, 0, 0, 1.0);
   1.218 +	*this *= smat;
   1.219 +}
   1.220 +
   1.221 +void Matrix4x4::set_scaling(const Vector3 &scale_vec)
   1.222 +{
   1.223 +	*this = Matrix4x4(	scale_vec.x, 0, 0, 0,
   1.224 +						0, scale_vec.y, 0, 0,
   1.225 +						0, 0, scale_vec.z, 0,
   1.226 +						0, 0, 0, 1);
   1.227 +}
   1.228 +
   1.229 +void Matrix4x4::set_column_vector(const Vector3 &vec, unsigned int col_index)
   1.230 +{
   1.231 +	m[0][col_index] = vec.x;
   1.232 +	m[1][col_index] = vec.y;
   1.233 +	m[2][col_index] = vec.z;
   1.234 +	m[3][col_index] = 1;
   1.235 +}
   1.236 +
   1.237 +void Matrix4x4::set_row_vector(const Vector3 &vec, unsigned int row_index)
   1.238 +{
   1.239 +	m[row_index][0] = vec.x;
   1.240 +	m[row_index][1] = vec.y;
   1.241 +	m[row_index][2] = vec.z;
   1.242 +	m[row_index][3] = 1;
   1.243 +}
   1.244 +
   1.245 +Vector3 Matrix4x4::get_column_vector(unsigned int col_index) const
   1.246 +{
   1.247 +	return Vector3(m[0][col_index], m[1][col_index], m[2][col_index]);
   1.248 +}
   1.249 +
   1.250 +Vector3 Matrix4x4::get_row_vector(unsigned int row_index) const
   1.251 +{
   1.252 +	return Vector3(m[row_index][0], m[row_index][1], m[row_index][2]);
   1.253 +}
   1.254 +
   1.255 +void Matrix4x4::transpose()
   1.256 +{
   1.257 +	Matrix4x4 tmp = *this;
   1.258 +	for(int i=0; i<4; i++) {
   1.259 +		for(int j=0; j<4; j++) {
   1.260 +			m[i][j] = tmp[j][i];
   1.261 +		}
   1.262 +	}
   1.263 +}
   1.264 +
   1.265 +Matrix4x4 Matrix4x4::transposed() const
   1.266 +{
   1.267 +	Matrix4x4 res;
   1.268 +	for(int i=0; i<4; i++) {
   1.269 +		for(int j=0; j<4; j++) {
   1.270 +			res[i][j] = m[j][i];
   1.271 +		}
   1.272 +	}
   1.273 +	return res;
   1.274 +}
   1.275 +
   1.276 +float Matrix4x4::determinant() const
   1.277 +{
   1.278 +	float det11 =	(m[1][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
   1.279 +						(m[1][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
   1.280 +						(m[1][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
   1.281 +
   1.282 +	float det12 =	(m[1][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
   1.283 +						(m[1][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
   1.284 +						(m[1][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
   1.285 +
   1.286 +	float det13 =	(m[1][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
   1.287 +						(m[1][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
   1.288 +						(m[1][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
   1.289 +
   1.290 +	float det14 =	(m[1][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
   1.291 +						(m[1][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
   1.292 +						(m[1][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
   1.293 +
   1.294 +	return m[0][0] * det11 - m[0][1] * det12 + m[0][2] * det13 - m[0][3] * det14;
   1.295 +}
   1.296 +
   1.297 +
   1.298 +Matrix4x4 Matrix4x4::adjoint() const
   1.299 +{
   1.300 +	Matrix4x4 coef;
   1.301 +
   1.302 +	coef.m[0][0] =	(m[1][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
   1.303 +					(m[1][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
   1.304 +					(m[1][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
   1.305 +	coef.m[0][1] =	(m[1][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
   1.306 +					(m[1][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
   1.307 +					(m[1][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
   1.308 +	coef.m[0][2] =	(m[1][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
   1.309 +					(m[1][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
   1.310 +					(m[1][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
   1.311 +	coef.m[0][3] =	(m[1][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
   1.312 +					(m[1][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
   1.313 +					(m[1][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
   1.314 +
   1.315 +	coef.m[1][0] =	(m[0][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
   1.316 +					(m[0][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
   1.317 +					(m[0][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
   1.318 +	coef.m[1][1] =	(m[0][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
   1.319 +					(m[0][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
   1.320 +					(m[0][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
   1.321 +	coef.m[1][2] =	(m[0][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
   1.322 +					(m[0][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
   1.323 +					(m[0][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
   1.324 +	coef.m[1][3] =	(m[0][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
   1.325 +					(m[0][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
   1.326 +					(m[0][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
   1.327 +
   1.328 +	coef.m[2][0] =	(m[0][1] * (m[1][2] * m[3][3] - m[3][2] * m[1][3])) -
   1.329 +					(m[0][2] * (m[1][1] * m[3][3] - m[3][1] * m[1][3])) +
   1.330 +					(m[0][3] * (m[1][1] * m[3][2] - m[3][1] * m[1][2]));
   1.331 +	coef.m[2][1] =	(m[0][0] * (m[1][2] * m[3][3] - m[3][2] * m[1][3])) -
   1.332 +					(m[0][2] * (m[1][0] * m[3][3] - m[3][0] * m[1][3])) +
   1.333 +					(m[0][3] * (m[1][0] * m[3][2] - m[3][0] * m[1][2]));
   1.334 +	coef.m[2][2] =	(m[0][0] * (m[1][1] * m[3][3] - m[3][1] * m[1][3])) -
   1.335 +					(m[0][1] * (m[1][0] * m[3][3] - m[3][0] * m[1][3])) +
   1.336 +					(m[0][3] * (m[1][0] * m[3][1] - m[3][0] * m[1][1]));
   1.337 +	coef.m[2][3] =	(m[0][0] * (m[1][1] * m[3][2] - m[3][1] * m[1][2])) -
   1.338 +					(m[0][1] * (m[1][0] * m[3][2] - m[3][0] * m[1][2])) +
   1.339 +					(m[0][2] * (m[1][0] * m[3][1] - m[3][0] * m[1][1]));
   1.340 +
   1.341 +	coef.m[3][0] =	(m[0][1] * (m[1][2] * m[2][3] - m[2][2] * m[1][3])) -
   1.342 +					(m[0][2] * (m[1][1] * m[2][3] - m[2][1] * m[1][3])) +
   1.343 +					(m[0][3] * (m[1][1] * m[2][2] - m[2][1] * m[1][2]));
   1.344 +	coef.m[3][1] =	(m[0][0] * (m[1][2] * m[2][3] - m[2][2] * m[1][3])) -
   1.345 +					(m[0][2] * (m[1][0] * m[2][3] - m[2][0] * m[1][3])) +
   1.346 +					(m[0][3] * (m[1][0] * m[2][2] - m[2][0] * m[1][2]));
   1.347 +	coef.m[3][2] =	(m[0][0] * (m[1][1] * m[2][3] - m[2][1] * m[1][3])) -
   1.348 +					(m[0][1] * (m[1][0] * m[2][3] - m[2][0] * m[1][3])) +
   1.349 +					(m[0][3] * (m[1][0] * m[2][1] - m[2][0] * m[1][1]));
   1.350 +	coef.m[3][3] =	(m[0][0] * (m[1][1] * m[2][2] - m[2][1] * m[1][2])) -
   1.351 +					(m[0][1] * (m[1][0] * m[2][2] - m[2][0] * m[1][2])) +
   1.352 +					(m[0][2] * (m[1][0] * m[2][1] - m[2][0] * m[1][1]));
   1.353 +
   1.354 +	coef.transpose();
   1.355 +
   1.356 +	for(int i=0; i<4; i++) {
   1.357 +		for(int j=0; j<4; j++) {
   1.358 +			coef.m[i][j] = j%2 ? -coef.m[i][j] : coef.m[i][j];
   1.359 +			if(i%2) coef.m[i][j] = -coef.m[i][j];
   1.360 +		}
   1.361 +	}
   1.362 +
   1.363 +	return coef;
   1.364 +}
   1.365 +
   1.366 +Matrix4x4 Matrix4x4::inverse() const
   1.367 +{
   1.368 +	Matrix4x4 adj = adjoint();
   1.369 +
   1.370 +	return adj * (1.0f / determinant());
   1.371 +}
   1.372 +
   1.373 +inline float *Matrix4x4::operator [](int index)
   1.374 +{
   1.375 +	return m[index];
   1.376 +}
   1.377 +
   1.378 +inline const float *Matrix4x4::operator [](int index) const
   1.379 +{
   1.380 +	return m[index];
   1.381 +}
   1.382 +
   1.383 +inline void Matrix4x4::reset_identity()
   1.384 +{
   1.385 +	*this = identity;
   1.386 +}
   1.387 +
   1.388 +std::ostream &operator <<(std::ostream &out, const Matrix4x4 &mat)
   1.389 +{
   1.390 +	for(int i=0; i<4; i++) {
   1.391 +		char str[100];
   1.392 +		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]);
   1.393 +		out << str;
   1.394 +	}
   1.395 +	return out;
   1.396 +}