1.1 --- /dev/null	Thu Jan 01 00:00:00 1970 +0000
     1.2 +++ b/prototype/vmath/matrix.cc	Thu Jun 28 06:05:50 2012 +0300
     1.3 @@ -0,0 +1,743 @@
     1.4 +#include <cstdio>
     1.5 +#include <cmath>
     1.6 +#include "matrix.h"
     1.7 +#include "vector.h"
     1.8 +#include "quat.h"
     1.9 +
    1.10 +using namespace std;
    1.11 +
    1.12 +// ----------- Matrix3x3 --------------
    1.13 +
    1.14 +Matrix3x3 Matrix3x3::identity = Matrix3x3(1, 0, 0, 0, 1, 0, 0, 0, 1);
    1.15 +
    1.16 +Matrix3x3::Matrix3x3()
    1.17 +{
    1.18 +	*this = identity;
    1.19 +}
    1.20 +
    1.21 +Matrix3x3::Matrix3x3(	scalar_t m11, scalar_t m12, scalar_t m13,
    1.22 +						scalar_t m21, scalar_t m22, scalar_t m23,
    1.23 +						scalar_t m31, scalar_t m32, scalar_t m33)
    1.24 +{
    1.25 +	m[0][0] = m11; m[0][1] = m12; m[0][2] = m13;
    1.26 +	m[1][0] = m21; m[1][1] = m22; m[1][2] = m23;
    1.27 +	m[2][0] = m31; m[2][1] = m32; m[2][2] = m33;
    1.28 +}
    1.29 +
    1.30 +Matrix3x3::Matrix3x3(const Vector3 &ivec, const Vector3 &jvec, const Vector3 &kvec)
    1.31 +{
    1.32 +	set_row_vector(ivec, 0);
    1.33 +	set_row_vector(jvec, 1);
    1.34 +	set_row_vector(kvec, 2);
    1.35 +}
    1.36 +
    1.37 +Matrix3x3::Matrix3x3(const mat3_t cmat)
    1.38 +{
    1.39 +	memcpy(m, cmat, sizeof(mat3_t));
    1.40 +}
    1.41 +
    1.42 +Matrix3x3::Matrix3x3(const Matrix4x4 &mat4x4)
    1.43 +{
    1.44 +	for(int i=0; i<3; i++) {
    1.45 +		for(int j=0; j<3; j++) {
    1.46 +			m[i][j] = mat4x4[i][j];
    1.47 +		}
    1.48 +	}
    1.49 +}
    1.50 +
    1.51 +Matrix3x3 operator +(const Matrix3x3 &m1, const Matrix3x3 &m2)
    1.52 +{
    1.53 +	Matrix3x3 res;
    1.54 +	const scalar_t *op1 = m1.m[0], *op2 = m2.m[0];
    1.55 +	scalar_t *dest = res.m[0];
    1.56 +
    1.57 +	for(int i=0; i<9; i++) {
    1.58 +		*dest++ = *op1++ + *op2++;
    1.59 +	}
    1.60 +	return res;
    1.61 +}
    1.62 +
    1.63 +Matrix3x3 operator -(const Matrix3x3 &m1, const Matrix3x3 &m2)
    1.64 +{
    1.65 +	Matrix3x3 res;
    1.66 +	const scalar_t *op1 = m1.m[0], *op2 = m2.m[0];
    1.67 +	scalar_t *dest = res.m[0];
    1.68 +
    1.69 +	for(int i=0; i<9; i++) {
    1.70 +		*dest++ = *op1++ - *op2++;
    1.71 +	}
    1.72 +	return res;
    1.73 +}
    1.74 +
    1.75 +Matrix3x3 operator *(const Matrix3x3 &m1, const Matrix3x3 &m2)
    1.76 +{
    1.77 +	Matrix3x3 res;
    1.78 +	for(int i=0; i<3; i++) {
    1.79 +		for(int j=0; j<3; j++) {
    1.80 +			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];
    1.81 +		}
    1.82 +	}
    1.83 +	return res;
    1.84 +}
    1.85 +
    1.86 +void operator +=(Matrix3x3 &m1, const Matrix3x3 &m2)
    1.87 +{
    1.88 +	scalar_t *op1 = m1.m[0];
    1.89 +	const scalar_t *op2 = m2.m[0];
    1.90 +
    1.91 +	for(int i=0; i<9; i++) {
    1.92 +		*op1++ += *op2++;
    1.93 +	}
    1.94 +}
    1.95 +
    1.96 +void operator -=(Matrix3x3 &m1, const Matrix3x3 &m2)
    1.97 +{
    1.98 +	scalar_t *op1 = m1.m[0];
    1.99 +	const scalar_t *op2 = m2.m[0];
   1.100 +
   1.101 +	for(int i=0; i<9; i++) {
   1.102 +		*op1++ -= *op2++;
   1.103 +	}
   1.104 +}
   1.105 +
   1.106 +void operator *=(Matrix3x3 &m1, const Matrix3x3 &m2)
   1.107 +{
   1.108 +	Matrix3x3 res;
   1.109 +	for(int i=0; i<3; i++) {
   1.110 +		for(int j=0; j<3; j++) {
   1.111 +			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];
   1.112 +		}
   1.113 +	}
   1.114 +	memcpy(m1.m, res.m, 9 * sizeof(scalar_t));
   1.115 +}
   1.116 +
   1.117 +Matrix3x3 operator *(const Matrix3x3 &mat, scalar_t scalar)
   1.118 +{
   1.119 +	Matrix3x3 res;
   1.120 +	const scalar_t *mptr = mat.m[0];
   1.121 +	scalar_t *dptr = res.m[0];
   1.122 +
   1.123 +	for(int i=0; i<9; i++) {
   1.124 +		*dptr++ = *mptr++ * scalar;
   1.125 +	}
   1.126 +	return res;
   1.127 +}
   1.128 +
   1.129 +Matrix3x3 operator *(scalar_t scalar, const Matrix3x3 &mat)
   1.130 +{
   1.131 +	Matrix3x3 res;
   1.132 +	const scalar_t *mptr = mat.m[0];
   1.133 +	scalar_t *dptr = res.m[0];
   1.134 +
   1.135 +	for(int i=0; i<9; i++) {
   1.136 +		*dptr++ = *mptr++ * scalar;
   1.137 +	}
   1.138 +	return res;
   1.139 +}
   1.140 +
   1.141 +void operator *=(Matrix3x3 &mat, scalar_t scalar)
   1.142 +{
   1.143 +	scalar_t *mptr = mat.m[0];
   1.144 +
   1.145 +	for(int i=0; i<9; i++) {
   1.146 +		*mptr++ *= scalar;
   1.147 +	}
   1.148 +}
   1.149 +
   1.150 +void Matrix3x3::translate(const Vector2 &trans)
   1.151 +{
   1.152 +	Matrix3x3 tmat(1, 0, trans.x, 0, 1, trans.y, 0, 0, 1);
   1.153 +	*this *= tmat;
   1.154 +}
   1.155 +
   1.156 +void Matrix3x3::set_translation(const Vector2 &trans)
   1.157 +{
   1.158 +	*this = Matrix3x3(1, 0, trans.x, 0, 1, trans.y, 0, 0, 1);
   1.159 +}
   1.160 +
   1.161 +void Matrix3x3::rotate(scalar_t angle)
   1.162 +{
   1.163 +	scalar_t cos_a = cos(angle);
   1.164 +	scalar_t sin_a = sin(angle);
   1.165 +	Matrix3x3 rmat(	cos_a,	-sin_a,		0,
   1.166 +					sin_a,	cos_a,		0,
   1.167 +					0,		0,			1);
   1.168 +	*this *= rmat;
   1.169 +}
   1.170 +
   1.171 +void Matrix3x3::set_rotation(scalar_t angle)
   1.172 +{
   1.173 +	scalar_t cos_a = cos(angle);
   1.174 +	scalar_t sin_a = sin(angle);
   1.175 +	*this = Matrix3x3(cos_a, -sin_a, 0, sin_a, cos_a, 0, 0, 0, 1);
   1.176 +}
   1.177 +
   1.178 +void Matrix3x3::rotate(const Vector3 &euler_angles)
   1.179 +{
   1.180 +	Matrix3x3 xrot, yrot, zrot;
   1.181 +
   1.182 +	xrot = Matrix3x3(	1,			0,					0,
   1.183 +						0,	cos(euler_angles.x),	-sin(euler_angles.x),
   1.184 +						0,	sin(euler_angles.x),	cos(euler_angles.x));
   1.185 +
   1.186 +	yrot = Matrix3x3(	cos(euler_angles.y),	0,	sin(euler_angles.y),
   1.187 +								0,				1,				0,
   1.188 +						-sin(euler_angles.y),	0,	cos(euler_angles.y));
   1.189 +
   1.190 +	zrot = Matrix3x3(	cos(euler_angles.z),	-sin(euler_angles.z),	0,
   1.191 +						sin(euler_angles.z),	cos(euler_angles.z),	0,
   1.192 +								0,						0,				1);
   1.193 +
   1.194 +	*this *= xrot * yrot * zrot;
   1.195 +}
   1.196 +
   1.197 +void Matrix3x3::set_rotation(const Vector3 &euler_angles)
   1.198 +{
   1.199 +	Matrix3x3 xrot, yrot, zrot;
   1.200 +
   1.201 +	xrot = Matrix3x3(	1,			0,					0,
   1.202 +						0,	cos(euler_angles.x),	-sin(euler_angles.x),
   1.203 +						0,	sin(euler_angles.x),	cos(euler_angles.x));
   1.204 +
   1.205 +	yrot = Matrix3x3(	cos(euler_angles.y),	0,	sin(euler_angles.y),
   1.206 +								0,				1,				0,
   1.207 +						-sin(euler_angles.y),	0,	cos(euler_angles.y));
   1.208 +
   1.209 +	zrot = Matrix3x3(	cos(euler_angles.z),	-sin(euler_angles.z),	0,
   1.210 +						sin(euler_angles.z),	cos(euler_angles.z),	0,
   1.211 +								0,						0,				1);
   1.212 +
   1.213 +	*this = xrot * yrot * zrot;
   1.214 +}
   1.215 +
   1.216 +void Matrix3x3::rotate(const Vector3 &axis, scalar_t angle)
   1.217 +{
   1.218 +	scalar_t sina = (scalar_t)sin(angle);
   1.219 +	scalar_t cosa = (scalar_t)cos(angle);
   1.220 +	scalar_t invcosa = 1-cosa;
   1.221 +	scalar_t nxsq = axis.x * axis.x;
   1.222 +	scalar_t nysq = axis.y * axis.y;
   1.223 +	scalar_t nzsq = axis.z * axis.z;
   1.224 +
   1.225 +	Matrix3x3 xform;
   1.226 +	xform.m[0][0] = nxsq + (1-nxsq) * cosa;
   1.227 +	xform.m[0][1] = axis.x * axis.y * invcosa - axis.z * sina;
   1.228 +	xform.m[0][2] = axis.x * axis.z * invcosa + axis.y * sina;
   1.229 +
   1.230 +	xform.m[1][0] = axis.x * axis.y * invcosa + axis.z * sina;
   1.231 +	xform.m[1][1] = nysq + (1-nysq) * cosa;
   1.232 +	xform.m[1][2] = axis.y * axis.z * invcosa - axis.x * sina;
   1.233 +
   1.234 +	xform.m[2][0] = axis.x * axis.z * invcosa - axis.y * sina;
   1.235 +	xform.m[2][1] = axis.y * axis.z * invcosa + axis.x * sina;
   1.236 +	xform.m[2][2] = nzsq + (1-nzsq) * cosa;
   1.237 +
   1.238 +	*this *= xform;
   1.239 +}
   1.240 +
   1.241 +void Matrix3x3::set_rotation(const Vector3 &axis, scalar_t angle)
   1.242 +{
   1.243 +	scalar_t sina = (scalar_t)sin(angle);
   1.244 +	scalar_t cosa = (scalar_t)cos(angle);
   1.245 +	scalar_t invcosa = 1-cosa;
   1.246 +	scalar_t nxsq = axis.x * axis.x;
   1.247 +	scalar_t nysq = axis.y * axis.y;
   1.248 +	scalar_t nzsq = axis.z * axis.z;
   1.249 +
   1.250 +	reset_identity();
   1.251 +	m[0][0] = nxsq + (1-nxsq) * cosa;
   1.252 +	m[0][1] = axis.x * axis.y * invcosa - axis.z * sina;
   1.253 +	m[0][2] = axis.x * axis.z * invcosa + axis.y * sina;
   1.254 +	m[1][0] = axis.x * axis.y * invcosa + axis.z * sina;
   1.255 +	m[1][1] = nysq + (1-nysq) * cosa;
   1.256 +	m[1][2] = axis.y * axis.z * invcosa - axis.x * sina;
   1.257 +	m[2][0] = axis.x * axis.z * invcosa - axis.y * sina;
   1.258 +	m[2][1] = axis.y * axis.z * invcosa + axis.x * sina;
   1.259 +	m[2][2] = nzsq + (1-nzsq) * cosa;
   1.260 +}
   1.261 +
   1.262 +void Matrix3x3::scale(const Vector3 &scale_vec)
   1.263 +{
   1.264 +	Matrix3x3 smat(	scale_vec.x, 0, 0,
   1.265 +					0, scale_vec.y, 0,
   1.266 +					0, 0, scale_vec.z);
   1.267 +	*this *= smat;
   1.268 +}
   1.269 +
   1.270 +void Matrix3x3::set_scaling(const Vector3 &scale_vec)
   1.271 +{
   1.272 +	*this = Matrix3x3(	scale_vec.x, 0, 0,
   1.273 +						0, scale_vec.y, 0,
   1.274 +						0, 0, scale_vec.z);
   1.275 +}
   1.276 +
   1.277 +void Matrix3x3::set_column_vector(const Vector3 &vec, unsigned int col_index)
   1.278 +{
   1.279 +	m[0][col_index] = vec.x;
   1.280 +	m[1][col_index] = vec.y;
   1.281 +	m[2][col_index] = vec.z;
   1.282 +}
   1.283 +
   1.284 +void Matrix3x3::set_row_vector(const Vector3 &vec, unsigned int row_index)
   1.285 +{
   1.286 +	m[row_index][0] = vec.x;
   1.287 +	m[row_index][1] = vec.y;
   1.288 +	m[row_index][2] = vec.z;
   1.289 +}
   1.290 +
   1.291 +Vector3 Matrix3x3::get_column_vector(unsigned int col_index) const
   1.292 +{
   1.293 +	return Vector3(m[0][col_index], m[1][col_index], m[2][col_index]);
   1.294 +}
   1.295 +
   1.296 +Vector3 Matrix3x3::get_row_vector(unsigned int row_index) const
   1.297 +{
   1.298 +	return Vector3(m[row_index][0], m[row_index][1], m[row_index][2]);
   1.299 +}
   1.300 +
   1.301 +void Matrix3x3::transpose()
   1.302 +{
   1.303 +	Matrix3x3 tmp = *this;
   1.304 +	for(int i=0; i<3; i++) {
   1.305 +		for(int j=0; j<3; j++) {
   1.306 +			m[i][j] = tmp[j][i];
   1.307 +		}
   1.308 +	}
   1.309 +}
   1.310 +
   1.311 +Matrix3x3 Matrix3x3::transposed() const
   1.312 +{
   1.313 +	Matrix3x3 res;
   1.314 +	for(int i=0; i<3; i++) {
   1.315 +		for(int j=0; j<3; j++) {
   1.316 +			res[i][j] = m[j][i];
   1.317 +		}
   1.318 +	}
   1.319 +	return res;
   1.320 +}
   1.321 +
   1.322 +scalar_t Matrix3x3::determinant() const
   1.323 +{
   1.324 +	return	m[0][0] * (m[1][1]*m[2][2] - m[1][2]*m[2][1]) -
   1.325 +			m[0][1] * (m[1][0]*m[2][2] - m[1][2]*m[2][0]) +
   1.326 +			m[0][2] * (m[1][0]*m[2][1] - m[1][1]*m[2][0]);
   1.327 +}
   1.328 +
   1.329 +Matrix3x3 Matrix3x3::inverse() const
   1.330 +{
   1.331 +	// TODO: implement 3x3 inverse
   1.332 +	return *this;
   1.333 +}
   1.334 +
   1.335 +ostream &operator <<(ostream &out, const Matrix3x3 &mat)
   1.336 +{
   1.337 +	for(int i=0; i<3; i++) {
   1.338 +		char str[100];
   1.339 +		sprintf(str, "[ %12.5f %12.5f %12.5f ]\n", (float)mat.m[i][0], (float)mat.m[i][1], (float)mat.m[i][2]);
   1.340 +		out << str;
   1.341 +	}
   1.342 +	return out;
   1.343 +}
   1.344 +
   1.345 +
   1.346 +
   1.347 +/* ----------------- Matrix4x4 implementation --------------- */
   1.348 +
   1.349 +Matrix4x4 Matrix4x4::identity = Matrix4x4(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1);
   1.350 +
   1.351 +Matrix4x4::Matrix4x4()
   1.352 +{
   1.353 +	*this = identity;
   1.354 +}
   1.355 +
   1.356 +Matrix4x4::Matrix4x4(	scalar_t m11, scalar_t m12, scalar_t m13, scalar_t m14,
   1.357 +						scalar_t m21, scalar_t m22, scalar_t m23, scalar_t m24,
   1.358 +						scalar_t m31, scalar_t m32, scalar_t m33, scalar_t m34,
   1.359 +						scalar_t m41, scalar_t m42, scalar_t m43, scalar_t m44)
   1.360 +{
   1.361 +	m[0][0] = m11; m[0][1] = m12; m[0][2] = m13; m[0][3] = m14;
   1.362 +	m[1][0] = m21; m[1][1] = m22; m[1][2] = m23; m[1][3] = m24;
   1.363 +	m[2][0] = m31; m[2][1] = m32; m[2][2] = m33; m[2][3] = m34;
   1.364 +	m[3][0] = m41; m[3][1] = m42; m[3][2] = m43; m[3][3] = m44;
   1.365 +}
   1.366 +
   1.367 +Matrix4x4::Matrix4x4(const mat4_t cmat)
   1.368 +{
   1.369 +	memcpy(m, cmat, sizeof(mat4_t));
   1.370 +}
   1.371 +
   1.372 +Matrix4x4::Matrix4x4(const Matrix3x3 &mat3x3)
   1.373 +{
   1.374 +	reset_identity();
   1.375 +	for(int i=0; i<3; i++) {
   1.376 +		for(int j=0; j<3; j++) {
   1.377 +			m[i][j] = mat3x3[i][j];
   1.378 +		}
   1.379 +	}
   1.380 +}
   1.381 +
   1.382 +Matrix4x4 operator +(const Matrix4x4 &m1, const Matrix4x4 &m2)
   1.383 +{
   1.384 +	Matrix4x4 res;
   1.385 +	const scalar_t *op1 = m1.m[0], *op2 = m2.m[0];
   1.386 +	scalar_t *dest = res.m[0];
   1.387 +
   1.388 +	for(int i=0; i<16; i++) {
   1.389 +		*dest++ = *op1++ + *op2++;
   1.390 +	}
   1.391 +	return res;
   1.392 +}
   1.393 +
   1.394 +Matrix4x4 operator -(const Matrix4x4 &m1, const Matrix4x4 &m2)
   1.395 +{
   1.396 +	Matrix4x4 res;
   1.397 +	const scalar_t *op1 = m1.m[0], *op2 = m2.m[0];
   1.398 +	scalar_t *dest = res.m[0];
   1.399 +
   1.400 +	for(int i=0; i<16; i++) {
   1.401 +		*dest++ = *op1++ - *op2++;
   1.402 +	}
   1.403 +	return res;
   1.404 +}
   1.405 +
   1.406 +/*
   1.407 +Matrix4x4 operator *(const Matrix4x4 &m1, const Matrix4x4 &m2) {
   1.408 +	Matrix4x4 res;
   1.409 +
   1.410 +	for(int i=0; i<4; i++) {
   1.411 +		for(int j=0; j<4; j++) {
   1.412 +			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.413 +		}
   1.414 +	}
   1.415 +
   1.416 +	return res;
   1.417 +}
   1.418 +*/
   1.419 +
   1.420 +void operator +=(Matrix4x4 &m1, const Matrix4x4 &m2)
   1.421 +{
   1.422 +	scalar_t *op1 = m1.m[0];
   1.423 +	const scalar_t *op2 = m2.m[0];
   1.424 +
   1.425 +	for(int i=0; i<16; i++) {
   1.426 +		*op1++ += *op2++;
   1.427 +	}
   1.428 +}
   1.429 +
   1.430 +void operator -=(Matrix4x4 &m1, const Matrix4x4 &m2)
   1.431 +{
   1.432 +	scalar_t *op1 = m1.m[0];
   1.433 +	const scalar_t *op2 = m2.m[0];
   1.434 +
   1.435 +	for(int i=0; i<16; i++) {
   1.436 +		*op1++ -= *op2++;
   1.437 +	}
   1.438 +}
   1.439 +
   1.440 +void operator *=(Matrix4x4 &m1, const Matrix4x4 &m2)
   1.441 +{
   1.442 +	Matrix4x4 res;
   1.443 +	for(int i=0; i<4; i++) {
   1.444 +		for(int j=0; j<4; j++) {
   1.445 +			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.446 +		}
   1.447 +	}
   1.448 +	memcpy(m1.m, res.m, 16 * sizeof(scalar_t));
   1.449 +}
   1.450 +
   1.451 +Matrix4x4 operator *(const Matrix4x4 &mat, scalar_t scalar)
   1.452 +{
   1.453 +	Matrix4x4 res;
   1.454 +	const scalar_t *mptr = mat.m[0];
   1.455 +	scalar_t *dptr = res.m[0];
   1.456 +
   1.457 +	for(int i=0; i<16; i++) {
   1.458 +		*dptr++ = *mptr++ * scalar;
   1.459 +	}
   1.460 +	return res;
   1.461 +}
   1.462 +
   1.463 +Matrix4x4 operator *(scalar_t scalar, const Matrix4x4 &mat)
   1.464 +{
   1.465 +	Matrix4x4 res;
   1.466 +	const scalar_t *mptr = mat.m[0];
   1.467 +	scalar_t *dptr = res.m[0];
   1.468 +
   1.469 +	for(int i=0; i<16; i++) {
   1.470 +		*dptr++ = *mptr++ * scalar;
   1.471 +	}
   1.472 +	return res;
   1.473 +}
   1.474 +
   1.475 +void operator *=(Matrix4x4 &mat, scalar_t scalar)
   1.476 +{
   1.477 +	scalar_t *mptr = mat.m[0];
   1.478 +
   1.479 +	for(int i=0; i<16; i++) {
   1.480 +		*mptr++ *= scalar;
   1.481 +	}
   1.482 +}
   1.483 +
   1.484 +void Matrix4x4::translate(const Vector3 &trans)
   1.485 +{
   1.486 +	Matrix4x4 tmat(1, 0, 0, trans.x, 0, 1, 0, trans.y, 0, 0, 1, trans.z, 0, 0, 0, 1);
   1.487 +	*this *= tmat;
   1.488 +}
   1.489 +
   1.490 +void Matrix4x4::set_translation(const Vector3 &trans)
   1.491 +{
   1.492 +	*this = Matrix4x4(1, 0, 0, trans.x, 0, 1, 0, trans.y, 0, 0, 1, trans.z, 0, 0, 0, 1);
   1.493 +}
   1.494 +
   1.495 +void Matrix4x4::rotate(const Vector3 &euler_angles)
   1.496 +{
   1.497 +	Matrix3x3 xrot, yrot, zrot;
   1.498 +
   1.499 +	xrot = Matrix3x3(	1,			0,					0,
   1.500 +						0,	cos(euler_angles.x),	-sin(euler_angles.x),
   1.501 +						0,	sin(euler_angles.x),	cos(euler_angles.x));
   1.502 +
   1.503 +	yrot = Matrix3x3(	cos(euler_angles.y),	0,	sin(euler_angles.y),
   1.504 +								0,				1,				0,
   1.505 +						-sin(euler_angles.y),	0,	cos(euler_angles.y));
   1.506 +
   1.507 +	zrot = Matrix3x3(	cos(euler_angles.z),	-sin(euler_angles.z),	0,
   1.508 +						sin(euler_angles.z),	cos(euler_angles.z),	0,
   1.509 +								0,						0,				1);
   1.510 +
   1.511 +	*this *= Matrix4x4(xrot * yrot * zrot);
   1.512 +}
   1.513 +
   1.514 +void Matrix4x4::set_rotation(const Vector3 &euler_angles)
   1.515 +{
   1.516 +	Matrix3x3 xrot, yrot, zrot;
   1.517 +
   1.518 +	xrot = Matrix3x3(	1,			0,					0,
   1.519 +						0,	cos(euler_angles.x),	-sin(euler_angles.x),
   1.520 +						0,	sin(euler_angles.x),	cos(euler_angles.x));
   1.521 +
   1.522 +	yrot = Matrix3x3(	cos(euler_angles.y),	0,	sin(euler_angles.y),
   1.523 +								0,				1,				0,
   1.524 +						-sin(euler_angles.y),	0,	cos(euler_angles.y));
   1.525 +
   1.526 +	zrot = Matrix3x3(	cos(euler_angles.z),	-sin(euler_angles.z),	0,
   1.527 +						sin(euler_angles.z),	cos(euler_angles.z),	0,
   1.528 +								0,						0,				1);
   1.529 +
   1.530 +	*this = Matrix4x4(xrot * yrot * zrot);
   1.531 +}
   1.532 +
   1.533 +void Matrix4x4::rotate(const Vector3 &axis, scalar_t angle)
   1.534 +{
   1.535 +	scalar_t sina = (scalar_t)sin(angle);
   1.536 +	scalar_t cosa = (scalar_t)cos(angle);
   1.537 +	scalar_t invcosa = 1-cosa;
   1.538 +	scalar_t nxsq = axis.x * axis.x;
   1.539 +	scalar_t nysq = axis.y * axis.y;
   1.540 +	scalar_t nzsq = axis.z * axis.z;
   1.541 +
   1.542 +	Matrix3x3 xform;
   1.543 +	xform[0][0] = nxsq + (1-nxsq) * cosa;
   1.544 +	xform[0][1] = axis.x * axis.y * invcosa - axis.z * sina;
   1.545 +	xform[0][2] = axis.x * axis.z * invcosa + axis.y * sina;
   1.546 +	xform[1][0] = axis.x * axis.y * invcosa + axis.z * sina;
   1.547 +	xform[1][1] = nysq + (1-nysq) * cosa;
   1.548 +	xform[1][2] = axis.y * axis.z * invcosa - axis.x * sina;
   1.549 +	xform[2][0] = axis.x * axis.z * invcosa - axis.y * sina;
   1.550 +	xform[2][1] = axis.y * axis.z * invcosa + axis.x * sina;
   1.551 +	xform[2][2] = nzsq + (1-nzsq) * cosa;
   1.552 +
   1.553 +	*this *= Matrix4x4(xform);
   1.554 +}
   1.555 +
   1.556 +void Matrix4x4::set_rotation(const Vector3 &axis, scalar_t angle)
   1.557 +{
   1.558 +	scalar_t sina = (scalar_t)sin(angle);
   1.559 +	scalar_t cosa = (scalar_t)cos(angle);
   1.560 +	scalar_t invcosa = 1-cosa;
   1.561 +	scalar_t nxsq = axis.x * axis.x;
   1.562 +	scalar_t nysq = axis.y * axis.y;
   1.563 +	scalar_t nzsq = axis.z * axis.z;
   1.564 +
   1.565 +	reset_identity();
   1.566 +	m[0][0] = nxsq + (1-nxsq) * cosa;
   1.567 +	m[0][1] = axis.x * axis.y * invcosa - axis.z * sina;
   1.568 +	m[0][2] = axis.x * axis.z * invcosa + axis.y * sina;
   1.569 +	m[1][0] = axis.x * axis.y * invcosa + axis.z * sina;
   1.570 +	m[1][1] = nysq + (1-nysq) * cosa;
   1.571 +	m[1][2] = axis.y * axis.z * invcosa - axis.x * sina;
   1.572 +	m[2][0] = axis.x * axis.z * invcosa - axis.y * sina;
   1.573 +	m[2][1] = axis.y * axis.z * invcosa + axis.x * sina;
   1.574 +	m[2][2] = nzsq + (1-nzsq) * cosa;
   1.575 +}
   1.576 +
   1.577 +void Matrix4x4::scale(const Vector4 &scale_vec)
   1.578 +{
   1.579 +	Matrix4x4 smat(	scale_vec.x, 0, 0, 0,
   1.580 +					0, scale_vec.y, 0, 0,
   1.581 +					0, 0, scale_vec.z, 0,
   1.582 +					0, 0, 0, scale_vec.w);
   1.583 +	*this *= smat;
   1.584 +}
   1.585 +
   1.586 +void Matrix4x4::set_scaling(const Vector4 &scale_vec)
   1.587 +{
   1.588 +	*this = Matrix4x4(	scale_vec.x, 0, 0, 0,
   1.589 +						0, scale_vec.y, 0, 0,
   1.590 +						0, 0, scale_vec.z, 0,
   1.591 +						0, 0, 0, scale_vec.w);
   1.592 +}
   1.593 +
   1.594 +void Matrix4x4::set_column_vector(const Vector4 &vec, unsigned int col_index)
   1.595 +{
   1.596 +	m[0][col_index] = vec.x;
   1.597 +	m[1][col_index] = vec.y;
   1.598 +	m[2][col_index] = vec.z;
   1.599 +	m[3][col_index] = vec.w;
   1.600 +}
   1.601 +
   1.602 +void Matrix4x4::set_row_vector(const Vector4 &vec, unsigned int row_index)
   1.603 +{
   1.604 +	m[row_index][0] = vec.x;
   1.605 +	m[row_index][1] = vec.y;
   1.606 +	m[row_index][2] = vec.z;
   1.607 +	m[row_index][3] = vec.w;
   1.608 +}
   1.609 +
   1.610 +Vector4 Matrix4x4::get_column_vector(unsigned int col_index) const
   1.611 +{
   1.612 +	return Vector4(m[0][col_index], m[1][col_index], m[2][col_index], m[3][col_index]);
   1.613 +}
   1.614 +
   1.615 +Vector4 Matrix4x4::get_row_vector(unsigned int row_index) const
   1.616 +{
   1.617 +	return Vector4(m[row_index][0], m[row_index][1], m[row_index][2], m[row_index][3]);
   1.618 +}
   1.619 +
   1.620 +void Matrix4x4::transpose()
   1.621 +{
   1.622 +	Matrix4x4 tmp = *this;
   1.623 +	for(int i=0; i<4; i++) {
   1.624 +		for(int j=0; j<4; j++) {
   1.625 +			m[i][j] = tmp[j][i];
   1.626 +		}
   1.627 +	}
   1.628 +}
   1.629 +
   1.630 +Matrix4x4 Matrix4x4::transposed() const
   1.631 +{
   1.632 +	Matrix4x4 res;
   1.633 +	for(int i=0; i<4; i++) {
   1.634 +		for(int j=0; j<4; j++) {
   1.635 +			res[i][j] = m[j][i];
   1.636 +		}
   1.637 +	}
   1.638 +	return res;
   1.639 +}
   1.640 +
   1.641 +scalar_t Matrix4x4::determinant() const
   1.642 +{
   1.643 +	scalar_t det11 =	(m[1][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
   1.644 +						(m[1][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
   1.645 +						(m[1][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
   1.646 +
   1.647 +	scalar_t det12 =	(m[1][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
   1.648 +						(m[1][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
   1.649 +						(m[1][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
   1.650 +
   1.651 +	scalar_t det13 =	(m[1][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
   1.652 +						(m[1][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
   1.653 +						(m[1][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
   1.654 +
   1.655 +	scalar_t det14 =	(m[1][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
   1.656 +						(m[1][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
   1.657 +						(m[1][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
   1.658 +
   1.659 +	return m[0][0] * det11 - m[0][1] * det12 + m[0][2] * det13 - m[0][3] * det14;
   1.660 +}
   1.661 +
   1.662 +
   1.663 +Matrix4x4 Matrix4x4::adjoint() const
   1.664 +{
   1.665 +	Matrix4x4 coef;
   1.666 +
   1.667 +	coef.m[0][0] =	(m[1][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
   1.668 +					(m[1][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
   1.669 +					(m[1][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
   1.670 +	coef.m[0][1] =	(m[1][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
   1.671 +					(m[1][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
   1.672 +					(m[1][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
   1.673 +	coef.m[0][2] =	(m[1][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
   1.674 +					(m[1][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
   1.675 +					(m[1][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
   1.676 +	coef.m[0][3] =	(m[1][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
   1.677 +					(m[1][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
   1.678 +					(m[1][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
   1.679 +
   1.680 +	coef.m[1][0] =	(m[0][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
   1.681 +					(m[0][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
   1.682 +					(m[0][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
   1.683 +	coef.m[1][1] =	(m[0][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
   1.684 +					(m[0][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
   1.685 +					(m[0][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
   1.686 +	coef.m[1][2] =	(m[0][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
   1.687 +					(m[0][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
   1.688 +					(m[0][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
   1.689 +	coef.m[1][3] =	(m[0][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
   1.690 +					(m[0][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
   1.691 +					(m[0][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
   1.692 +
   1.693 +	coef.m[2][0] =	(m[0][1] * (m[1][2] * m[3][3] - m[3][2] * m[1][3])) -
   1.694 +					(m[0][2] * (m[1][1] * m[3][3] - m[3][1] * m[1][3])) +
   1.695 +					(m[0][3] * (m[1][1] * m[3][2] - m[3][1] * m[1][2]));
   1.696 +	coef.m[2][1] =	(m[0][0] * (m[1][2] * m[3][3] - m[3][2] * m[1][3])) -
   1.697 +					(m[0][2] * (m[1][0] * m[3][3] - m[3][0] * m[1][3])) +
   1.698 +					(m[0][3] * (m[1][0] * m[3][2] - m[3][0] * m[1][2]));
   1.699 +	coef.m[2][2] =	(m[0][0] * (m[1][1] * m[3][3] - m[3][1] * m[1][3])) -
   1.700 +					(m[0][1] * (m[1][0] * m[3][3] - m[3][0] * m[1][3])) +
   1.701 +					(m[0][3] * (m[1][0] * m[3][1] - m[3][0] * m[1][1]));
   1.702 +	coef.m[2][3] =	(m[0][0] * (m[1][1] * m[3][2] - m[3][1] * m[1][2])) -
   1.703 +					(m[0][1] * (m[1][0] * m[3][2] - m[3][0] * m[1][2])) +
   1.704 +					(m[0][2] * (m[1][0] * m[3][1] - m[3][0] * m[1][1]));
   1.705 +
   1.706 +	coef.m[3][0] =	(m[0][1] * (m[1][2] * m[2][3] - m[2][2] * m[1][3])) -
   1.707 +					(m[0][2] * (m[1][1] * m[2][3] - m[2][1] * m[1][3])) +
   1.708 +					(m[0][3] * (m[1][1] * m[2][2] - m[2][1] * m[1][2]));
   1.709 +	coef.m[3][1] =	(m[0][0] * (m[1][2] * m[2][3] - m[2][2] * m[1][3])) -
   1.710 +					(m[0][2] * (m[1][0] * m[2][3] - m[2][0] * m[1][3])) +
   1.711 +					(m[0][3] * (m[1][0] * m[2][2] - m[2][0] * m[1][2]));
   1.712 +	coef.m[3][2] =	(m[0][0] * (m[1][1] * m[2][3] - m[2][1] * m[1][3])) -
   1.713 +					(m[0][1] * (m[1][0] * m[2][3] - m[2][0] * m[1][3])) +
   1.714 +					(m[0][3] * (m[1][0] * m[2][1] - m[2][0] * m[1][1]));
   1.715 +	coef.m[3][3] =	(m[0][0] * (m[1][1] * m[2][2] - m[2][1] * m[1][2])) -
   1.716 +					(m[0][1] * (m[1][0] * m[2][2] - m[2][0] * m[1][2])) +
   1.717 +					(m[0][2] * (m[1][0] * m[2][1] - m[2][0] * m[1][1]));
   1.718 +
   1.719 +	coef.transpose();
   1.720 +
   1.721 +	for(int i=0; i<4; i++) {
   1.722 +		for(int j=0; j<4; j++) {
   1.723 +			coef.m[i][j] = j%2 ? -coef.m[i][j] : coef.m[i][j];
   1.724 +			if(i%2) coef.m[i][j] = -coef.m[i][j];
   1.725 +		}
   1.726 +	}
   1.727 +
   1.728 +	return coef;
   1.729 +}
   1.730 +
   1.731 +Matrix4x4 Matrix4x4::inverse() const
   1.732 +{
   1.733 +	Matrix4x4 adj = adjoint();
   1.734 +
   1.735 +	return adj * (1.0f / determinant());
   1.736 +}
   1.737 +
   1.738 +ostream &operator <<(ostream &out, const Matrix4x4 &mat)
   1.739 +{
   1.740 +	for(int i=0; i<4; i++) {
   1.741 +		char str[100];
   1.742 +		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.743 +		out << str;
   1.744 +	}
   1.745 +	return out;
   1.746 +}