1.1 --- /dev/null	Thu Jan 01 00:00:00 1970 +0000
     1.2 +++ b/libs/vmath/matrix.cc	Sun Nov 01 00:36:56 2015 +0200
     1.3 @@ -0,0 +1,853 @@
     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 = Matrix3x3(1, 0, 0, 0, 1, 0, 0, 0, 1);
    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 +// Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
   1.263 +// article "Quaternion Calculus and Fast Animation".
   1.264 +// adapted from: http://www.geometrictools.com/LibMathematics/Algebra/Wm5Quaternion.inl
   1.265 +Quaternion Matrix3x3::get_rotation_quat() const
   1.266 +{
   1.267 +	static const int next[3] = {1, 2, 0};
   1.268 +
   1.269 +	float quat[4];
   1.270 +
   1.271 +	scalar_t trace = m[0][0] + m[1][1] + m[2][2];
   1.272 +	scalar_t root;
   1.273 +
   1.274 +	if(trace > 0.0f) {
   1.275 +		// |w| > 1/2
   1.276 +		root = sqrt(trace + 1.0f);	// 2w
   1.277 +		quat[0] = 0.5f * root;
   1.278 +		root = 0.5f / root;	// 1 / 4w
   1.279 +		quat[1] = (m[2][1] - m[1][2]) * root;
   1.280 +		quat[2] = (m[0][2] - m[2][0]) * root;
   1.281 +		quat[3] = (m[1][0] - m[0][1]) * root;
   1.282 +	} else {
   1.283 +		// |w| <= 1/2
   1.284 +		int i = 0;
   1.285 +		if(m[1][1] > m[0][0]) {
   1.286 +			i = 1;
   1.287 +		}
   1.288 +		if(m[2][2] > m[i][i]) {
   1.289 +			i = 2;
   1.290 +		}
   1.291 +		int j = next[i];
   1.292 +		int k = next[j];
   1.293 +
   1.294 +		root = sqrt(m[i][i] - m[j][j] - m[k][k] + 1.0f);
   1.295 +		quat[i + 1] = 0.5f * root;
   1.296 +		root = 0.5f / root;
   1.297 +		quat[0] = (m[k][j] - m[j][k]) * root;
   1.298 +		quat[j + 1] = (m[j][i] - m[i][j]) * root;
   1.299 +		quat[k + 1] = (m[k][i] - m[i][k]) * root;
   1.300 +	}
   1.301 +	return Quaternion(quat[0], quat[1], quat[2], quat[3]);
   1.302 +}
   1.303 +
   1.304 +void Matrix3x3::scale(const Vector3 &scale_vec)
   1.305 +{
   1.306 +	Matrix3x3 smat(	scale_vec.x, 0, 0,
   1.307 +					0, scale_vec.y, 0,
   1.308 +					0, 0, scale_vec.z);
   1.309 +	*this *= smat;
   1.310 +}
   1.311 +
   1.312 +void Matrix3x3::set_scaling(const Vector3 &scale_vec)
   1.313 +{
   1.314 +	*this = Matrix3x3(	scale_vec.x, 0, 0,
   1.315 +						0, scale_vec.y, 0,
   1.316 +						0, 0, scale_vec.z);
   1.317 +}
   1.318 +
   1.319 +void Matrix3x3::set_column_vector(const Vector3 &vec, unsigned int col_index)
   1.320 +{
   1.321 +	m[0][col_index] = vec.x;
   1.322 +	m[1][col_index] = vec.y;
   1.323 +	m[2][col_index] = vec.z;
   1.324 +}
   1.325 +
   1.326 +void Matrix3x3::set_row_vector(const Vector3 &vec, unsigned int row_index)
   1.327 +{
   1.328 +	m[row_index][0] = vec.x;
   1.329 +	m[row_index][1] = vec.y;
   1.330 +	m[row_index][2] = vec.z;
   1.331 +}
   1.332 +
   1.333 +Vector3 Matrix3x3::get_column_vector(unsigned int col_index) const
   1.334 +{
   1.335 +	return Vector3(m[0][col_index], m[1][col_index], m[2][col_index]);
   1.336 +}
   1.337 +
   1.338 +Vector3 Matrix3x3::get_row_vector(unsigned int row_index) const
   1.339 +{
   1.340 +	return Vector3(m[row_index][0], m[row_index][1], m[row_index][2]);
   1.341 +}
   1.342 +
   1.343 +void Matrix3x3::transpose()
   1.344 +{
   1.345 +	Matrix3x3 tmp = *this;
   1.346 +	for(int i=0; i<3; i++) {
   1.347 +		for(int j=0; j<3; j++) {
   1.348 +			m[i][j] = tmp[j][i];
   1.349 +		}
   1.350 +	}
   1.351 +}
   1.352 +
   1.353 +Matrix3x3 Matrix3x3::transposed() const
   1.354 +{
   1.355 +	Matrix3x3 res;
   1.356 +	for(int i=0; i<3; i++) {
   1.357 +		for(int j=0; j<3; j++) {
   1.358 +			res[i][j] = m[j][i];
   1.359 +		}
   1.360 +	}
   1.361 +	return res;
   1.362 +}
   1.363 +
   1.364 +scalar_t Matrix3x3::determinant() const
   1.365 +{
   1.366 +	return	m[0][0] * (m[1][1]*m[2][2] - m[1][2]*m[2][1]) -
   1.367 +			m[0][1] * (m[1][0]*m[2][2] - m[1][2]*m[2][0]) +
   1.368 +			m[0][2] * (m[1][0]*m[2][1] - m[1][1]*m[2][0]);
   1.369 +}
   1.370 +
   1.371 +Matrix3x3 Matrix3x3::inverse() const
   1.372 +{
   1.373 +	// TODO: implement 3x3 inverse
   1.374 +	return *this;
   1.375 +}
   1.376 +
   1.377 +ostream &operator <<(ostream &out, const Matrix3x3 &mat)
   1.378 +{
   1.379 +	for(int i=0; i<3; i++) {
   1.380 +		char str[100];
   1.381 +		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.382 +		out << str;
   1.383 +	}
   1.384 +	return out;
   1.385 +}
   1.386 +
   1.387 +
   1.388 +
   1.389 +/* ----------------- Matrix4x4 implementation --------------- */
   1.390 +
   1.391 +Matrix4x4 Matrix4x4::identity = Matrix4x4(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1);
   1.392 +
   1.393 +Matrix4x4::Matrix4x4()
   1.394 +{
   1.395 +	*this = Matrix4x4(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1);
   1.396 +}
   1.397 +
   1.398 +Matrix4x4::Matrix4x4(	scalar_t m11, scalar_t m12, scalar_t m13, scalar_t m14,
   1.399 +						scalar_t m21, scalar_t m22, scalar_t m23, scalar_t m24,
   1.400 +						scalar_t m31, scalar_t m32, scalar_t m33, scalar_t m34,
   1.401 +						scalar_t m41, scalar_t m42, scalar_t m43, scalar_t m44)
   1.402 +{
   1.403 +	m[0][0] = m11; m[0][1] = m12; m[0][2] = m13; m[0][3] = m14;
   1.404 +	m[1][0] = m21; m[1][1] = m22; m[1][2] = m23; m[1][3] = m24;
   1.405 +	m[2][0] = m31; m[2][1] = m32; m[2][2] = m33; m[2][3] = m34;
   1.406 +	m[3][0] = m41; m[3][1] = m42; m[3][2] = m43; m[3][3] = m44;
   1.407 +}
   1.408 +
   1.409 +Matrix4x4::Matrix4x4(const mat4_t cmat)
   1.410 +{
   1.411 +	memcpy(m, cmat, sizeof(mat4_t));
   1.412 +}
   1.413 +
   1.414 +Matrix4x4::Matrix4x4(const Matrix3x3 &mat3x3)
   1.415 +{
   1.416 +	reset_identity();
   1.417 +	for(int i=0; i<3; i++) {
   1.418 +		for(int j=0; j<3; j++) {
   1.419 +			m[i][j] = mat3x3[i][j];
   1.420 +		}
   1.421 +	}
   1.422 +}
   1.423 +
   1.424 +Matrix4x4 operator +(const Matrix4x4 &m1, const Matrix4x4 &m2)
   1.425 +{
   1.426 +	Matrix4x4 res;
   1.427 +	const scalar_t *op1 = m1.m[0], *op2 = m2.m[0];
   1.428 +	scalar_t *dest = res.m[0];
   1.429 +
   1.430 +	for(int i=0; i<16; i++) {
   1.431 +		*dest++ = *op1++ + *op2++;
   1.432 +	}
   1.433 +	return res;
   1.434 +}
   1.435 +
   1.436 +Matrix4x4 operator -(const Matrix4x4 &m1, const Matrix4x4 &m2)
   1.437 +{
   1.438 +	Matrix4x4 res;
   1.439 +	const scalar_t *op1 = m1.m[0], *op2 = m2.m[0];
   1.440 +	scalar_t *dest = res.m[0];
   1.441 +
   1.442 +	for(int i=0; i<16; i++) {
   1.443 +		*dest++ = *op1++ - *op2++;
   1.444 +	}
   1.445 +	return res;
   1.446 +}
   1.447 +
   1.448 +void operator +=(Matrix4x4 &m1, const Matrix4x4 &m2)
   1.449 +{
   1.450 +	scalar_t *op1 = m1.m[0];
   1.451 +	const scalar_t *op2 = m2.m[0];
   1.452 +
   1.453 +	for(int i=0; i<16; i++) {
   1.454 +		*op1++ += *op2++;
   1.455 +	}
   1.456 +}
   1.457 +
   1.458 +void operator -=(Matrix4x4 &m1, const Matrix4x4 &m2)
   1.459 +{
   1.460 +	scalar_t *op1 = m1.m[0];
   1.461 +	const scalar_t *op2 = m2.m[0];
   1.462 +
   1.463 +	for(int i=0; i<16; i++) {
   1.464 +		*op1++ -= *op2++;
   1.465 +	}
   1.466 +}
   1.467 +
   1.468 +Matrix4x4 operator *(const Matrix4x4 &mat, scalar_t scalar)
   1.469 +{
   1.470 +	Matrix4x4 res;
   1.471 +	const scalar_t *mptr = mat.m[0];
   1.472 +	scalar_t *dptr = res.m[0];
   1.473 +
   1.474 +	for(int i=0; i<16; i++) {
   1.475 +		*dptr++ = *mptr++ * scalar;
   1.476 +	}
   1.477 +	return res;
   1.478 +}
   1.479 +
   1.480 +Matrix4x4 operator *(scalar_t scalar, const Matrix4x4 &mat)
   1.481 +{
   1.482 +	Matrix4x4 res;
   1.483 +	const scalar_t *mptr = mat.m[0];
   1.484 +	scalar_t *dptr = res.m[0];
   1.485 +
   1.486 +	for(int i=0; i<16; i++) {
   1.487 +		*dptr++ = *mptr++ * scalar;
   1.488 +	}
   1.489 +	return res;
   1.490 +}
   1.491 +
   1.492 +void operator *=(Matrix4x4 &mat, scalar_t scalar)
   1.493 +{
   1.494 +	scalar_t *mptr = mat.m[0];
   1.495 +
   1.496 +	for(int i=0; i<16; i++) {
   1.497 +		*mptr++ *= scalar;
   1.498 +	}
   1.499 +}
   1.500 +
   1.501 +void Matrix4x4::translate(const Vector3 &trans)
   1.502 +{
   1.503 +	Matrix4x4 tmat(1, 0, 0, trans.x, 0, 1, 0, trans.y, 0, 0, 1, trans.z, 0, 0, 0, 1);
   1.504 +	*this *= tmat;
   1.505 +}
   1.506 +
   1.507 +void Matrix4x4::set_translation(const Vector3 &trans)
   1.508 +{
   1.509 +	*this = Matrix4x4(1, 0, 0, trans.x, 0, 1, 0, trans.y, 0, 0, 1, trans.z, 0, 0, 0, 1);
   1.510 +}
   1.511 +
   1.512 +Vector3 Matrix4x4::get_translation() const
   1.513 +{
   1.514 +	return Vector3(m[0][3], m[1][3], m[2][3]);
   1.515 +}
   1.516 +
   1.517 +void Matrix4x4::rotate(const Vector3 &euler_angles)
   1.518 +{
   1.519 +	Matrix3x3 xrot, yrot, zrot;
   1.520 +
   1.521 +	xrot = Matrix3x3(	1,			0,					0,
   1.522 +						0,	cos(euler_angles.x),	-sin(euler_angles.x),
   1.523 +						0,	sin(euler_angles.x),	cos(euler_angles.x));
   1.524 +
   1.525 +	yrot = Matrix3x3(	cos(euler_angles.y),	0,	sin(euler_angles.y),
   1.526 +								0,				1,				0,
   1.527 +						-sin(euler_angles.y),	0,	cos(euler_angles.y));
   1.528 +
   1.529 +	zrot = Matrix3x3(	cos(euler_angles.z),	-sin(euler_angles.z),	0,
   1.530 +						sin(euler_angles.z),	cos(euler_angles.z),	0,
   1.531 +								0,						0,				1);
   1.532 +
   1.533 +	*this *= Matrix4x4(xrot * yrot * zrot);
   1.534 +}
   1.535 +
   1.536 +void Matrix4x4::set_rotation(const Vector3 &euler_angles)
   1.537 +{
   1.538 +	Matrix3x3 xrot, yrot, zrot;
   1.539 +
   1.540 +	xrot = Matrix3x3(	1,			0,					0,
   1.541 +						0,	cos(euler_angles.x),	-sin(euler_angles.x),
   1.542 +						0,	sin(euler_angles.x),	cos(euler_angles.x));
   1.543 +
   1.544 +	yrot = Matrix3x3(	cos(euler_angles.y),	0,	sin(euler_angles.y),
   1.545 +								0,				1,				0,
   1.546 +						-sin(euler_angles.y),	0,	cos(euler_angles.y));
   1.547 +
   1.548 +	zrot = Matrix3x3(	cos(euler_angles.z),	-sin(euler_angles.z),	0,
   1.549 +						sin(euler_angles.z),	cos(euler_angles.z),	0,
   1.550 +								0,						0,				1);
   1.551 +
   1.552 +	*this = Matrix4x4(xrot * yrot * zrot);
   1.553 +}
   1.554 +
   1.555 +void Matrix4x4::rotate(const Vector3 &axis, scalar_t angle)
   1.556 +{
   1.557 +	scalar_t sina = (scalar_t)sin(angle);
   1.558 +	scalar_t cosa = (scalar_t)cos(angle);
   1.559 +	scalar_t invcosa = 1-cosa;
   1.560 +	scalar_t nxsq = axis.x * axis.x;
   1.561 +	scalar_t nysq = axis.y * axis.y;
   1.562 +	scalar_t nzsq = axis.z * axis.z;
   1.563 +
   1.564 +	Matrix4x4 xform;
   1.565 +	xform[0][0] = nxsq + (1-nxsq) * cosa;
   1.566 +	xform[0][1] = axis.x * axis.y * invcosa - axis.z * sina;
   1.567 +	xform[0][2] = axis.x * axis.z * invcosa + axis.y * sina;
   1.568 +	xform[1][0] = axis.x * axis.y * invcosa + axis.z * sina;
   1.569 +	xform[1][1] = nysq + (1-nysq) * cosa;
   1.570 +	xform[1][2] = axis.y * axis.z * invcosa - axis.x * sina;
   1.571 +	xform[2][0] = axis.x * axis.z * invcosa - axis.y * sina;
   1.572 +	xform[2][1] = axis.y * axis.z * invcosa + axis.x * sina;
   1.573 +	xform[2][2] = nzsq + (1-nzsq) * cosa;
   1.574 +
   1.575 +	*this *= xform;
   1.576 +}
   1.577 +
   1.578 +void Matrix4x4::set_rotation(const Vector3 &axis, scalar_t angle)
   1.579 +{
   1.580 +	scalar_t sina = (scalar_t)sin(angle);
   1.581 +	scalar_t cosa = (scalar_t)cos(angle);
   1.582 +	scalar_t invcosa = 1-cosa;
   1.583 +	scalar_t nxsq = axis.x * axis.x;
   1.584 +	scalar_t nysq = axis.y * axis.y;
   1.585 +	scalar_t nzsq = axis.z * axis.z;
   1.586 +
   1.587 +	reset_identity();
   1.588 +	m[0][0] = nxsq + (1-nxsq) * cosa;
   1.589 +	m[0][1] = axis.x * axis.y * invcosa - axis.z * sina;
   1.590 +	m[0][2] = axis.x * axis.z * invcosa + axis.y * sina;
   1.591 +	m[1][0] = axis.x * axis.y * invcosa + axis.z * sina;
   1.592 +	m[1][1] = nysq + (1-nysq) * cosa;
   1.593 +	m[1][2] = axis.y * axis.z * invcosa - axis.x * sina;
   1.594 +	m[2][0] = axis.x * axis.z * invcosa - axis.y * sina;
   1.595 +	m[2][1] = axis.y * axis.z * invcosa + axis.x * sina;
   1.596 +	m[2][2] = nzsq + (1-nzsq) * cosa;
   1.597 +}
   1.598 +
   1.599 +void Matrix4x4::rotate(const Quaternion &quat)
   1.600 +{
   1.601 +	*this *= quat.get_rotation_matrix();
   1.602 +}
   1.603 +
   1.604 +void Matrix4x4::set_rotation(const Quaternion &quat)
   1.605 +{
   1.606 +	*this = quat.get_rotation_matrix();
   1.607 +}
   1.608 +
   1.609 +Quaternion Matrix4x4::get_rotation_quat() const
   1.610 +{
   1.611 +	Matrix3x3 mat3 = *this;
   1.612 +	return mat3.get_rotation_quat();
   1.613 +}
   1.614 +
   1.615 +void Matrix4x4::scale(const Vector4 &scale_vec)
   1.616 +{
   1.617 +	Matrix4x4 smat(	scale_vec.x, 0, 0, 0,
   1.618 +					0, scale_vec.y, 0, 0,
   1.619 +					0, 0, scale_vec.z, 0,
   1.620 +					0, 0, 0, scale_vec.w);
   1.621 +	*this *= smat;
   1.622 +}
   1.623 +
   1.624 +void Matrix4x4::set_scaling(const Vector4 &scale_vec)
   1.625 +{
   1.626 +	*this = Matrix4x4(	scale_vec.x, 0, 0, 0,
   1.627 +						0, scale_vec.y, 0, 0,
   1.628 +						0, 0, scale_vec.z, 0,
   1.629 +						0, 0, 0, scale_vec.w);
   1.630 +}
   1.631 +
   1.632 +Vector3 Matrix4x4::get_scaling() const
   1.633 +{
   1.634 +	Vector3 vi = get_row_vector(0);
   1.635 +	Vector3 vj = get_row_vector(1);
   1.636 +	Vector3 vk = get_row_vector(2);
   1.637 +
   1.638 +	return Vector3(vi.length(), vj.length(), vk.length());
   1.639 +}
   1.640 +
   1.641 +void Matrix4x4::set_frustum(float left, float right, float bottom, float top, float znear, float zfar)
   1.642 +{
   1.643 +	float dx = right - left;
   1.644 +	float dy = top - bottom;
   1.645 +	float dz = zfar - znear;
   1.646 +
   1.647 +	float a = (right + left) / dx;
   1.648 +	float b = (top + bottom) / dy;
   1.649 +	float c = -(zfar + znear) / dz;
   1.650 +	float d = -2.0 * zfar * znear / dz;
   1.651 +
   1.652 +	*this = Matrix4x4(2.0 * znear / dx, 0, a, 0,
   1.653 +			0, 2.0 * znear / dy, b, 0,
   1.654 +			0, 0, c, d,
   1.655 +			0, 0, -1, 0);
   1.656 +}
   1.657 +
   1.658 +void Matrix4x4::set_perspective(float vfov, float aspect, float znear, float zfar)
   1.659 +{
   1.660 +	float f = 1.0f / tan(vfov * 0.5f);
   1.661 +    float dz = znear - zfar;
   1.662 +
   1.663 +	reset_identity();
   1.664 +
   1.665 +	m[0][0] = f / aspect;
   1.666 +    m[1][1] = f;
   1.667 +    m[2][2] = (zfar + znear) / dz;
   1.668 +    m[3][2] = -1.0f;
   1.669 +    m[2][3] = 2.0f * zfar * znear / dz;
   1.670 +    m[3][3] = 0.0f;
   1.671 +}
   1.672 +
   1.673 +void Matrix4x4::set_orthographic(float left, float right, float bottom, float top, float znear, float zfar)
   1.674 +{
   1.675 +	float dx = right - left;
   1.676 +	float dy = top - bottom;
   1.677 +	float dz = zfar - znear;
   1.678 +
   1.679 +	reset_identity();
   1.680 +
   1.681 +	m[0][0] = 2.0 / dx;
   1.682 +	m[1][1] = 2.0 / dy;
   1.683 +	m[2][2] = -2.0 / dz;
   1.684 +	m[0][3] = -(right + left) / dx;
   1.685 +	m[1][3] = -(top + bottom) / dy;
   1.686 +	m[2][3] = -(zfar + znear) / dz;
   1.687 +}
   1.688 +
   1.689 +void Matrix4x4::set_lookat(const Vector3 &pos, const Vector3 &targ, const Vector3 &up)
   1.690 +{
   1.691 +	Vector3 vk = (targ - pos).normalized();
   1.692 +	Vector3 vj = up.normalized();
   1.693 +	Vector3 vi = cross_product(vk, vj).normalized();
   1.694 +	vj = cross_product(vi, vk);
   1.695 +
   1.696 +	*this = Matrix4x4(
   1.697 +			vi.x, vi.y, vi.z, 0,
   1.698 +			vj.x, vj.y, vj.z, 0,
   1.699 +			-vk.x, -vk.y, -vk.z, 0,
   1.700 +			0, 0, 0, 1);
   1.701 +	translate(-pos);
   1.702 +}
   1.703 +
   1.704 +void Matrix4x4::set_column_vector(const Vector4 &vec, unsigned int col_index)
   1.705 +{
   1.706 +	m[0][col_index] = vec.x;
   1.707 +	m[1][col_index] = vec.y;
   1.708 +	m[2][col_index] = vec.z;
   1.709 +	m[3][col_index] = vec.w;
   1.710 +}
   1.711 +
   1.712 +void Matrix4x4::set_row_vector(const Vector4 &vec, unsigned int row_index)
   1.713 +{
   1.714 +	m[row_index][0] = vec.x;
   1.715 +	m[row_index][1] = vec.y;
   1.716 +	m[row_index][2] = vec.z;
   1.717 +	m[row_index][3] = vec.w;
   1.718 +}
   1.719 +
   1.720 +Vector4 Matrix4x4::get_column_vector(unsigned int col_index) const
   1.721 +{
   1.722 +	return Vector4(m[0][col_index], m[1][col_index], m[2][col_index], m[3][col_index]);
   1.723 +}
   1.724 +
   1.725 +Vector4 Matrix4x4::get_row_vector(unsigned int row_index) const
   1.726 +{
   1.727 +	return Vector4(m[row_index][0], m[row_index][1], m[row_index][2], m[row_index][3]);
   1.728 +}
   1.729 +
   1.730 +void Matrix4x4::transpose()
   1.731 +{
   1.732 +	Matrix4x4 tmp = *this;
   1.733 +	for(int i=0; i<4; i++) {
   1.734 +		for(int j=0; j<4; j++) {
   1.735 +			m[i][j] = tmp[j][i];
   1.736 +		}
   1.737 +	}
   1.738 +}
   1.739 +
   1.740 +Matrix4x4 Matrix4x4::transposed() const
   1.741 +{
   1.742 +	Matrix4x4 res;
   1.743 +	for(int i=0; i<4; i++) {
   1.744 +		for(int j=0; j<4; j++) {
   1.745 +			res[i][j] = m[j][i];
   1.746 +		}
   1.747 +	}
   1.748 +	return res;
   1.749 +}
   1.750 +
   1.751 +scalar_t Matrix4x4::determinant() const
   1.752 +{
   1.753 +	scalar_t det11 =	(m[1][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
   1.754 +						(m[1][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
   1.755 +						(m[1][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
   1.756 +
   1.757 +	scalar_t det12 =	(m[1][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
   1.758 +						(m[1][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
   1.759 +						(m[1][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
   1.760 +
   1.761 +	scalar_t det13 =	(m[1][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
   1.762 +						(m[1][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
   1.763 +						(m[1][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
   1.764 +
   1.765 +	scalar_t det14 =	(m[1][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
   1.766 +						(m[1][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
   1.767 +						(m[1][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
   1.768 +
   1.769 +	return m[0][0] * det11 - m[0][1] * det12 + m[0][2] * det13 - m[0][3] * det14;
   1.770 +}
   1.771 +
   1.772 +
   1.773 +Matrix4x4 Matrix4x4::adjoint() const
   1.774 +{
   1.775 +	Matrix4x4 coef;
   1.776 +
   1.777 +	coef.m[0][0] =	(m[1][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
   1.778 +					(m[1][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
   1.779 +					(m[1][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
   1.780 +	coef.m[0][1] =	(m[1][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
   1.781 +					(m[1][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
   1.782 +					(m[1][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
   1.783 +	coef.m[0][2] =	(m[1][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
   1.784 +					(m[1][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
   1.785 +					(m[1][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
   1.786 +	coef.m[0][3] =	(m[1][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
   1.787 +					(m[1][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
   1.788 +					(m[1][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
   1.789 +
   1.790 +	coef.m[1][0] =	(m[0][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
   1.791 +					(m[0][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
   1.792 +					(m[0][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
   1.793 +	coef.m[1][1] =	(m[0][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
   1.794 +					(m[0][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
   1.795 +					(m[0][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
   1.796 +	coef.m[1][2] =	(m[0][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
   1.797 +					(m[0][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
   1.798 +					(m[0][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
   1.799 +	coef.m[1][3] =	(m[0][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
   1.800 +					(m[0][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
   1.801 +					(m[0][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
   1.802 +
   1.803 +	coef.m[2][0] =	(m[0][1] * (m[1][2] * m[3][3] - m[3][2] * m[1][3])) -
   1.804 +					(m[0][2] * (m[1][1] * m[3][3] - m[3][1] * m[1][3])) +
   1.805 +					(m[0][3] * (m[1][1] * m[3][2] - m[3][1] * m[1][2]));
   1.806 +	coef.m[2][1] =	(m[0][0] * (m[1][2] * m[3][3] - m[3][2] * m[1][3])) -
   1.807 +					(m[0][2] * (m[1][0] * m[3][3] - m[3][0] * m[1][3])) +
   1.808 +					(m[0][3] * (m[1][0] * m[3][2] - m[3][0] * m[1][2]));
   1.809 +	coef.m[2][2] =	(m[0][0] * (m[1][1] * m[3][3] - m[3][1] * m[1][3])) -
   1.810 +					(m[0][1] * (m[1][0] * m[3][3] - m[3][0] * m[1][3])) +
   1.811 +					(m[0][3] * (m[1][0] * m[3][1] - m[3][0] * m[1][1]));
   1.812 +	coef.m[2][3] =	(m[0][0] * (m[1][1] * m[3][2] - m[3][1] * m[1][2])) -
   1.813 +					(m[0][1] * (m[1][0] * m[3][2] - m[3][0] * m[1][2])) +
   1.814 +					(m[0][2] * (m[1][0] * m[3][1] - m[3][0] * m[1][1]));
   1.815 +
   1.816 +	coef.m[3][0] =	(m[0][1] * (m[1][2] * m[2][3] - m[2][2] * m[1][3])) -
   1.817 +					(m[0][2] * (m[1][1] * m[2][3] - m[2][1] * m[1][3])) +
   1.818 +					(m[0][3] * (m[1][1] * m[2][2] - m[2][1] * m[1][2]));
   1.819 +	coef.m[3][1] =	(m[0][0] * (m[1][2] * m[2][3] - m[2][2] * m[1][3])) -
   1.820 +					(m[0][2] * (m[1][0] * m[2][3] - m[2][0] * m[1][3])) +
   1.821 +					(m[0][3] * (m[1][0] * m[2][2] - m[2][0] * m[1][2]));
   1.822 +	coef.m[3][2] =	(m[0][0] * (m[1][1] * m[2][3] - m[2][1] * m[1][3])) -
   1.823 +					(m[0][1] * (m[1][0] * m[2][3] - m[2][0] * m[1][3])) +
   1.824 +					(m[0][3] * (m[1][0] * m[2][1] - m[2][0] * m[1][1]));
   1.825 +	coef.m[3][3] =	(m[0][0] * (m[1][1] * m[2][2] - m[2][1] * m[1][2])) -
   1.826 +					(m[0][1] * (m[1][0] * m[2][2] - m[2][0] * m[1][2])) +
   1.827 +					(m[0][2] * (m[1][0] * m[2][1] - m[2][0] * m[1][1]));
   1.828 +
   1.829 +	coef.transpose();
   1.830 +
   1.831 +	for(int i=0; i<4; i++) {
   1.832 +		for(int j=0; j<4; j++) {
   1.833 +			coef.m[i][j] = j%2 ? -coef.m[i][j] : coef.m[i][j];
   1.834 +			if(i%2) coef.m[i][j] = -coef.m[i][j];
   1.835 +		}
   1.836 +	}
   1.837 +
   1.838 +	return coef;
   1.839 +}
   1.840 +
   1.841 +Matrix4x4 Matrix4x4::inverse() const
   1.842 +{
   1.843 +	Matrix4x4 adj = adjoint();
   1.844 +
   1.845 +	return adj * (1.0f / determinant());
   1.846 +}
   1.847 +
   1.848 +ostream &operator <<(ostream &out, const Matrix4x4 &mat)
   1.849 +{
   1.850 +	for(int i=0; i<4; i++) {
   1.851 +		char str[100];
   1.852 +		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.853 +		out << str;
   1.854 +	}
   1.855 +	return out;
   1.856 +}