1.1 --- /dev/null	Thu Jan 01 00:00:00 1970 +0000
     1.2 +++ b/libs/vmath/matrix.cc	Sun May 31 00:40:26 2015 +0300
     1.3 @@ -0,0 +1,872 @@
     1.4 +/*
     1.5 +libvmath - a vector math library
     1.6 +Copyright (C) 2004-2015 John Tsiombikas <nuclear@member.fsf.org>
     1.7 +
     1.8 +This program is free software: you can redistribute it and/or modify
     1.9 +it under the terms of the GNU Lesser General Public License as published
    1.10 +by the Free Software Foundation, either version 3 of the License, or
    1.11 +(at your option) any later version.
    1.12 +
    1.13 +This program is distributed in the hope that it will be useful,
    1.14 +but WITHOUT ANY WARRANTY; without even the implied warranty of
    1.15 +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    1.16 +GNU Lesser General Public License for more details.
    1.17 +
    1.18 +You should have received a copy of the GNU Lesser General Public License
    1.19 +along with this program.  If not, see <http://www.gnu.org/licenses/>.
    1.20 +*/
    1.21 +#include <cstdio>
    1.22 +#include <cmath>
    1.23 +#include "matrix.h"
    1.24 +#include "vector.h"
    1.25 +#include "quat.h"
    1.26 +
    1.27 +using namespace std;
    1.28 +
    1.29 +// ----------- Matrix3x3 --------------
    1.30 +
    1.31 +Matrix3x3 Matrix3x3::identity = Matrix3x3(1, 0, 0, 0, 1, 0, 0, 0, 1);
    1.32 +
    1.33 +Matrix3x3::Matrix3x3()
    1.34 +{
    1.35 +	*this = Matrix3x3(1, 0, 0, 0, 1, 0, 0, 0, 1);
    1.36 +}
    1.37 +
    1.38 +Matrix3x3::Matrix3x3(	scalar_t m11, scalar_t m12, scalar_t m13,
    1.39 +						scalar_t m21, scalar_t m22, scalar_t m23,
    1.40 +						scalar_t m31, scalar_t m32, scalar_t m33)
    1.41 +{
    1.42 +	m[0][0] = m11; m[0][1] = m12; m[0][2] = m13;
    1.43 +	m[1][0] = m21; m[1][1] = m22; m[1][2] = m23;
    1.44 +	m[2][0] = m31; m[2][1] = m32; m[2][2] = m33;
    1.45 +}
    1.46 +
    1.47 +Matrix3x3::Matrix3x3(const Vector3 &ivec, const Vector3 &jvec, const Vector3 &kvec)
    1.48 +{
    1.49 +	set_row_vector(ivec, 0);
    1.50 +	set_row_vector(jvec, 1);
    1.51 +	set_row_vector(kvec, 2);
    1.52 +}
    1.53 +
    1.54 +Matrix3x3::Matrix3x3(const mat3_t cmat)
    1.55 +{
    1.56 +	memcpy(m, cmat, sizeof(mat3_t));
    1.57 +}
    1.58 +
    1.59 +Matrix3x3::Matrix3x3(const Matrix4x4 &mat4x4)
    1.60 +{
    1.61 +	for(int i=0; i<3; i++) {
    1.62 +		for(int j=0; j<3; j++) {
    1.63 +			m[i][j] = mat4x4[i][j];
    1.64 +		}
    1.65 +	}
    1.66 +}
    1.67 +
    1.68 +Matrix3x3 operator +(const Matrix3x3 &m1, const Matrix3x3 &m2)
    1.69 +{
    1.70 +	Matrix3x3 res;
    1.71 +	const scalar_t *op1 = m1.m[0], *op2 = m2.m[0];
    1.72 +	scalar_t *dest = res.m[0];
    1.73 +
    1.74 +	for(int i=0; i<9; i++) {
    1.75 +		*dest++ = *op1++ + *op2++;
    1.76 +	}
    1.77 +	return res;
    1.78 +}
    1.79 +
    1.80 +Matrix3x3 operator -(const Matrix3x3 &m1, const Matrix3x3 &m2)
    1.81 +{
    1.82 +	Matrix3x3 res;
    1.83 +	const scalar_t *op1 = m1.m[0], *op2 = m2.m[0];
    1.84 +	scalar_t *dest = res.m[0];
    1.85 +
    1.86 +	for(int i=0; i<9; i++) {
    1.87 +		*dest++ = *op1++ - *op2++;
    1.88 +	}
    1.89 +	return res;
    1.90 +}
    1.91 +
    1.92 +Matrix3x3 operator *(const Matrix3x3 &m1, const Matrix3x3 &m2)
    1.93 +{
    1.94 +	Matrix3x3 res;
    1.95 +	for(int i=0; i<3; i++) {
    1.96 +		for(int j=0; j<3; j++) {
    1.97 +			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.98 +		}
    1.99 +	}
   1.100 +	return res;
   1.101 +}
   1.102 +
   1.103 +void operator +=(Matrix3x3 &m1, const Matrix3x3 &m2)
   1.104 +{
   1.105 +	scalar_t *op1 = m1.m[0];
   1.106 +	const scalar_t *op2 = m2.m[0];
   1.107 +
   1.108 +	for(int i=0; i<9; i++) {
   1.109 +		*op1++ += *op2++;
   1.110 +	}
   1.111 +}
   1.112 +
   1.113 +void operator -=(Matrix3x3 &m1, const Matrix3x3 &m2)
   1.114 +{
   1.115 +	scalar_t *op1 = m1.m[0];
   1.116 +	const scalar_t *op2 = m2.m[0];
   1.117 +
   1.118 +	for(int i=0; i<9; i++) {
   1.119 +		*op1++ -= *op2++;
   1.120 +	}
   1.121 +}
   1.122 +
   1.123 +void operator *=(Matrix3x3 &m1, const Matrix3x3 &m2)
   1.124 +{
   1.125 +	Matrix3x3 res;
   1.126 +	for(int i=0; i<3; i++) {
   1.127 +		for(int j=0; j<3; j++) {
   1.128 +			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.129 +		}
   1.130 +	}
   1.131 +	memcpy(m1.m, res.m, 9 * sizeof(scalar_t));
   1.132 +}
   1.133 +
   1.134 +Matrix3x3 operator *(const Matrix3x3 &mat, scalar_t scalar)
   1.135 +{
   1.136 +	Matrix3x3 res;
   1.137 +	const scalar_t *mptr = mat.m[0];
   1.138 +	scalar_t *dptr = res.m[0];
   1.139 +
   1.140 +	for(int i=0; i<9; i++) {
   1.141 +		*dptr++ = *mptr++ * scalar;
   1.142 +	}
   1.143 +	return res;
   1.144 +}
   1.145 +
   1.146 +Matrix3x3 operator *(scalar_t scalar, const Matrix3x3 &mat)
   1.147 +{
   1.148 +	Matrix3x3 res;
   1.149 +	const scalar_t *mptr = mat.m[0];
   1.150 +	scalar_t *dptr = res.m[0];
   1.151 +
   1.152 +	for(int i=0; i<9; i++) {
   1.153 +		*dptr++ = *mptr++ * scalar;
   1.154 +	}
   1.155 +	return res;
   1.156 +}
   1.157 +
   1.158 +void operator *=(Matrix3x3 &mat, scalar_t scalar)
   1.159 +{
   1.160 +	scalar_t *mptr = mat.m[0];
   1.161 +
   1.162 +	for(int i=0; i<9; i++) {
   1.163 +		*mptr++ *= scalar;
   1.164 +	}
   1.165 +}
   1.166 +
   1.167 +void Matrix3x3::translate(const Vector2 &trans)
   1.168 +{
   1.169 +	Matrix3x3 tmat(1, 0, trans.x, 0, 1, trans.y, 0, 0, 1);
   1.170 +	*this *= tmat;
   1.171 +}
   1.172 +
   1.173 +void Matrix3x3::set_translation(const Vector2 &trans)
   1.174 +{
   1.175 +	*this = Matrix3x3(1, 0, trans.x, 0, 1, trans.y, 0, 0, 1);
   1.176 +}
   1.177 +
   1.178 +void Matrix3x3::rotate(scalar_t angle)
   1.179 +{
   1.180 +	scalar_t cos_a = cos(angle);
   1.181 +	scalar_t sin_a = sin(angle);
   1.182 +	Matrix3x3 rmat(	cos_a,	-sin_a,		0,
   1.183 +					sin_a,	cos_a,		0,
   1.184 +					0,		0,			1);
   1.185 +	*this *= rmat;
   1.186 +}
   1.187 +
   1.188 +void Matrix3x3::set_rotation(scalar_t angle)
   1.189 +{
   1.190 +	scalar_t cos_a = cos(angle);
   1.191 +	scalar_t sin_a = sin(angle);
   1.192 +	*this = Matrix3x3(cos_a, -sin_a, 0, sin_a, cos_a, 0, 0, 0, 1);
   1.193 +}
   1.194 +
   1.195 +void Matrix3x3::rotate(const Vector3 &euler_angles)
   1.196 +{
   1.197 +	Matrix3x3 xrot, yrot, zrot;
   1.198 +
   1.199 +	xrot = Matrix3x3(	1,			0,					0,
   1.200 +						0,	cos(euler_angles.x),	-sin(euler_angles.x),
   1.201 +						0,	sin(euler_angles.x),	cos(euler_angles.x));
   1.202 +
   1.203 +	yrot = Matrix3x3(	cos(euler_angles.y),	0,	sin(euler_angles.y),
   1.204 +								0,				1,				0,
   1.205 +						-sin(euler_angles.y),	0,	cos(euler_angles.y));
   1.206 +
   1.207 +	zrot = Matrix3x3(	cos(euler_angles.z),	-sin(euler_angles.z),	0,
   1.208 +						sin(euler_angles.z),	cos(euler_angles.z),	0,
   1.209 +								0,						0,				1);
   1.210 +
   1.211 +	*this *= xrot * yrot * zrot;
   1.212 +}
   1.213 +
   1.214 +void Matrix3x3::set_rotation(const Vector3 &euler_angles)
   1.215 +{
   1.216 +	Matrix3x3 xrot, yrot, zrot;
   1.217 +
   1.218 +	xrot = Matrix3x3(	1,			0,					0,
   1.219 +						0,	cos(euler_angles.x),	-sin(euler_angles.x),
   1.220 +						0,	sin(euler_angles.x),	cos(euler_angles.x));
   1.221 +
   1.222 +	yrot = Matrix3x3(	cos(euler_angles.y),	0,	sin(euler_angles.y),
   1.223 +								0,				1,				0,
   1.224 +						-sin(euler_angles.y),	0,	cos(euler_angles.y));
   1.225 +
   1.226 +	zrot = Matrix3x3(	cos(euler_angles.z),	-sin(euler_angles.z),	0,
   1.227 +						sin(euler_angles.z),	cos(euler_angles.z),	0,
   1.228 +								0,						0,				1);
   1.229 +
   1.230 +	*this = xrot * yrot * zrot;
   1.231 +}
   1.232 +
   1.233 +void Matrix3x3::rotate(const Vector3 &axis, scalar_t angle)
   1.234 +{
   1.235 +	scalar_t sina = (scalar_t)sin(angle);
   1.236 +	scalar_t cosa = (scalar_t)cos(angle);
   1.237 +	scalar_t invcosa = 1-cosa;
   1.238 +	scalar_t nxsq = axis.x * axis.x;
   1.239 +	scalar_t nysq = axis.y * axis.y;
   1.240 +	scalar_t nzsq = axis.z * axis.z;
   1.241 +
   1.242 +	Matrix3x3 xform;
   1.243 +	xform.m[0][0] = nxsq + (1-nxsq) * cosa;
   1.244 +	xform.m[0][1] = axis.x * axis.y * invcosa - axis.z * sina;
   1.245 +	xform.m[0][2] = axis.x * axis.z * invcosa + axis.y * sina;
   1.246 +
   1.247 +	xform.m[1][0] = axis.x * axis.y * invcosa + axis.z * sina;
   1.248 +	xform.m[1][1] = nysq + (1-nysq) * cosa;
   1.249 +	xform.m[1][2] = axis.y * axis.z * invcosa - axis.x * sina;
   1.250 +
   1.251 +	xform.m[2][0] = axis.x * axis.z * invcosa - axis.y * sina;
   1.252 +	xform.m[2][1] = axis.y * axis.z * invcosa + axis.x * sina;
   1.253 +	xform.m[2][2] = nzsq + (1-nzsq) * cosa;
   1.254 +
   1.255 +	*this *= xform;
   1.256 +}
   1.257 +
   1.258 +void Matrix3x3::set_rotation(const Vector3 &axis, scalar_t angle)
   1.259 +{
   1.260 +	scalar_t sina = (scalar_t)sin(angle);
   1.261 +	scalar_t cosa = (scalar_t)cos(angle);
   1.262 +	scalar_t invcosa = 1-cosa;
   1.263 +	scalar_t nxsq = axis.x * axis.x;
   1.264 +	scalar_t nysq = axis.y * axis.y;
   1.265 +	scalar_t nzsq = axis.z * axis.z;
   1.266 +
   1.267 +	reset_identity();
   1.268 +	m[0][0] = nxsq + (1-nxsq) * cosa;
   1.269 +	m[0][1] = axis.x * axis.y * invcosa - axis.z * sina;
   1.270 +	m[0][2] = axis.x * axis.z * invcosa + axis.y * sina;
   1.271 +	m[1][0] = axis.x * axis.y * invcosa + axis.z * sina;
   1.272 +	m[1][1] = nysq + (1-nysq) * cosa;
   1.273 +	m[1][2] = axis.y * axis.z * invcosa - axis.x * sina;
   1.274 +	m[2][0] = axis.x * axis.z * invcosa - axis.y * sina;
   1.275 +	m[2][1] = axis.y * axis.z * invcosa + axis.x * sina;
   1.276 +	m[2][2] = nzsq + (1-nzsq) * cosa;
   1.277 +}
   1.278 +
   1.279 +// Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
   1.280 +// article "Quaternion Calculus and Fast Animation".
   1.281 +// adapted from: http://www.geometrictools.com/LibMathematics/Algebra/Wm5Quaternion.inl
   1.282 +Quaternion Matrix3x3::get_rotation_quat() const
   1.283 +{
   1.284 +	static const int next[3] = {1, 2, 0};
   1.285 +
   1.286 +	float quat[4];
   1.287 +
   1.288 +	scalar_t trace = m[0][0] + m[1][1] + m[2][2];
   1.289 +	scalar_t root;
   1.290 +
   1.291 +	if(trace > 0.0f) {
   1.292 +		// |w| > 1/2
   1.293 +		root = sqrt(trace + 1.0f);	// 2w
   1.294 +		quat[0] = 0.5f * root;
   1.295 +		root = 0.5f / root;	// 1 / 4w
   1.296 +		quat[1] = (m[2][1] - m[1][2]) * root;
   1.297 +		quat[2] = (m[0][2] - m[2][0]) * root;
   1.298 +		quat[3] = (m[1][0] - m[0][1]) * root;
   1.299 +	} else {
   1.300 +		// |w| <= 1/2
   1.301 +		int i = 0;
   1.302 +		if(m[1][1] > m[0][0]) {
   1.303 +			i = 1;
   1.304 +		}
   1.305 +		if(m[2][2] > m[i][i]) {
   1.306 +			i = 2;
   1.307 +		}
   1.308 +		int j = next[i];
   1.309 +		int k = next[j];
   1.310 +
   1.311 +		root = sqrt(m[i][i] - m[j][j] - m[k][k] + 1.0f);
   1.312 +		quat[i + 1] = 0.5f * root;
   1.313 +		root = 0.5f / root;
   1.314 +		quat[0] = (m[k][j] - m[j][k]) * root;
   1.315 +		quat[j + 1] = (m[j][i] - m[i][j]) * root;
   1.316 +		quat[k + 1] = (m[k][i] - m[i][k]) * root;
   1.317 +	}
   1.318 +	return Quaternion(quat[0], quat[1], quat[2], quat[3]);
   1.319 +}
   1.320 +
   1.321 +void Matrix3x3::scale(const Vector3 &scale_vec)
   1.322 +{
   1.323 +	Matrix3x3 smat(	scale_vec.x, 0, 0,
   1.324 +					0, scale_vec.y, 0,
   1.325 +					0, 0, scale_vec.z);
   1.326 +	*this *= smat;
   1.327 +}
   1.328 +
   1.329 +void Matrix3x3::set_scaling(const Vector3 &scale_vec)
   1.330 +{
   1.331 +	*this = Matrix3x3(	scale_vec.x, 0, 0,
   1.332 +						0, scale_vec.y, 0,
   1.333 +						0, 0, scale_vec.z);
   1.334 +}
   1.335 +
   1.336 +void Matrix3x3::set_column_vector(const Vector3 &vec, unsigned int col_index)
   1.337 +{
   1.338 +	m[0][col_index] = vec.x;
   1.339 +	m[1][col_index] = vec.y;
   1.340 +	m[2][col_index] = vec.z;
   1.341 +}
   1.342 +
   1.343 +void Matrix3x3::set_row_vector(const Vector3 &vec, unsigned int row_index)
   1.344 +{
   1.345 +	m[row_index][0] = vec.x;
   1.346 +	m[row_index][1] = vec.y;
   1.347 +	m[row_index][2] = vec.z;
   1.348 +}
   1.349 +
   1.350 +Vector3 Matrix3x3::get_column_vector(unsigned int col_index) const
   1.351 +{
   1.352 +	return Vector3(m[0][col_index], m[1][col_index], m[2][col_index]);
   1.353 +}
   1.354 +
   1.355 +Vector3 Matrix3x3::get_row_vector(unsigned int row_index) const
   1.356 +{
   1.357 +	return Vector3(m[row_index][0], m[row_index][1], m[row_index][2]);
   1.358 +}
   1.359 +
   1.360 +void Matrix3x3::transpose()
   1.361 +{
   1.362 +	Matrix3x3 tmp = *this;
   1.363 +	for(int i=0; i<3; i++) {
   1.364 +		for(int j=0; j<3; j++) {
   1.365 +			m[i][j] = tmp[j][i];
   1.366 +		}
   1.367 +	}
   1.368 +}
   1.369 +
   1.370 +Matrix3x3 Matrix3x3::transposed() const
   1.371 +{
   1.372 +	Matrix3x3 res;
   1.373 +	for(int i=0; i<3; i++) {
   1.374 +		for(int j=0; j<3; j++) {
   1.375 +			res[i][j] = m[j][i];
   1.376 +		}
   1.377 +	}
   1.378 +	return res;
   1.379 +}
   1.380 +
   1.381 +scalar_t Matrix3x3::determinant() const
   1.382 +{
   1.383 +	return	m[0][0] * (m[1][1]*m[2][2] - m[1][2]*m[2][1]) -
   1.384 +			m[0][1] * (m[1][0]*m[2][2] - m[1][2]*m[2][0]) +
   1.385 +			m[0][2] * (m[1][0]*m[2][1] - m[1][1]*m[2][0]);
   1.386 +}
   1.387 +
   1.388 +Matrix3x3 Matrix3x3::inverse() const
   1.389 +{
   1.390 +	// TODO: implement 3x3 inverse
   1.391 +	return *this;
   1.392 +}
   1.393 +
   1.394 +/*ostream &operator <<(ostream &out, const Matrix3x3 &mat)
   1.395 +{
   1.396 +	for(int i=0; i<3; i++) {
   1.397 +		char str[100];
   1.398 +		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.399 +		out << str;
   1.400 +	}
   1.401 +	return out;
   1.402 +}*/
   1.403 +
   1.404 +
   1.405 +
   1.406 +/* ----------------- Matrix4x4 implementation --------------- */
   1.407 +
   1.408 +Matrix4x4 Matrix4x4::identity = Matrix4x4(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1);
   1.409 +
   1.410 +Matrix4x4::Matrix4x4()
   1.411 +{
   1.412 +	*this = Matrix4x4(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1);
   1.413 +}
   1.414 +
   1.415 +Matrix4x4::Matrix4x4(	scalar_t m11, scalar_t m12, scalar_t m13, scalar_t m14,
   1.416 +						scalar_t m21, scalar_t m22, scalar_t m23, scalar_t m24,
   1.417 +						scalar_t m31, scalar_t m32, scalar_t m33, scalar_t m34,
   1.418 +						scalar_t m41, scalar_t m42, scalar_t m43, scalar_t m44)
   1.419 +{
   1.420 +	m[0][0] = m11; m[0][1] = m12; m[0][2] = m13; m[0][3] = m14;
   1.421 +	m[1][0] = m21; m[1][1] = m22; m[1][2] = m23; m[1][3] = m24;
   1.422 +	m[2][0] = m31; m[2][1] = m32; m[2][2] = m33; m[2][3] = m34;
   1.423 +	m[3][0] = m41; m[3][1] = m42; m[3][2] = m43; m[3][3] = m44;
   1.424 +}
   1.425 +
   1.426 +Matrix4x4::Matrix4x4(const mat4_t cmat)
   1.427 +{
   1.428 +	memcpy(m, cmat, sizeof(mat4_t));
   1.429 +}
   1.430 +
   1.431 +Matrix4x4::Matrix4x4(const Matrix3x3 &mat3x3)
   1.432 +{
   1.433 +	reset_identity();
   1.434 +	for(int i=0; i<3; i++) {
   1.435 +		for(int j=0; j<3; j++) {
   1.436 +			m[i][j] = mat3x3[i][j];
   1.437 +		}
   1.438 +	}
   1.439 +}
   1.440 +
   1.441 +Matrix4x4 operator +(const Matrix4x4 &m1, const Matrix4x4 &m2)
   1.442 +{
   1.443 +	Matrix4x4 res;
   1.444 +	const scalar_t *op1 = m1.m[0], *op2 = m2.m[0];
   1.445 +	scalar_t *dest = res.m[0];
   1.446 +
   1.447 +	for(int i=0; i<16; i++) {
   1.448 +		*dest++ = *op1++ + *op2++;
   1.449 +	}
   1.450 +	return res;
   1.451 +}
   1.452 +
   1.453 +Matrix4x4 operator -(const Matrix4x4 &m1, const Matrix4x4 &m2)
   1.454 +{
   1.455 +	Matrix4x4 res;
   1.456 +	const scalar_t *op1 = m1.m[0], *op2 = m2.m[0];
   1.457 +	scalar_t *dest = res.m[0];
   1.458 +
   1.459 +	for(int i=0; i<16; i++) {
   1.460 +		*dest++ = *op1++ - *op2++;
   1.461 +	}
   1.462 +	return res;
   1.463 +}
   1.464 +
   1.465 +void operator +=(Matrix4x4 &m1, const Matrix4x4 &m2)
   1.466 +{
   1.467 +	scalar_t *op1 = m1.m[0];
   1.468 +	const scalar_t *op2 = m2.m[0];
   1.469 +
   1.470 +	for(int i=0; i<16; i++) {
   1.471 +		*op1++ += *op2++;
   1.472 +	}
   1.473 +}
   1.474 +
   1.475 +void operator -=(Matrix4x4 &m1, const Matrix4x4 &m2)
   1.476 +{
   1.477 +	scalar_t *op1 = m1.m[0];
   1.478 +	const scalar_t *op2 = m2.m[0];
   1.479 +
   1.480 +	for(int i=0; i<16; i++) {
   1.481 +		*op1++ -= *op2++;
   1.482 +	}
   1.483 +}
   1.484 +
   1.485 +Matrix4x4 operator *(const Matrix4x4 &mat, scalar_t scalar)
   1.486 +{
   1.487 +	Matrix4x4 res;
   1.488 +	const scalar_t *mptr = mat.m[0];
   1.489 +	scalar_t *dptr = res.m[0];
   1.490 +
   1.491 +	for(int i=0; i<16; i++) {
   1.492 +		*dptr++ = *mptr++ * scalar;
   1.493 +	}
   1.494 +	return res;
   1.495 +}
   1.496 +
   1.497 +Matrix4x4 operator *(scalar_t scalar, const Matrix4x4 &mat)
   1.498 +{
   1.499 +	Matrix4x4 res;
   1.500 +	const scalar_t *mptr = mat.m[0];
   1.501 +	scalar_t *dptr = res.m[0];
   1.502 +
   1.503 +	for(int i=0; i<16; i++) {
   1.504 +		*dptr++ = *mptr++ * scalar;
   1.505 +	}
   1.506 +	return res;
   1.507 +}
   1.508 +
   1.509 +void operator *=(Matrix4x4 &mat, scalar_t scalar)
   1.510 +{
   1.511 +	scalar_t *mptr = mat.m[0];
   1.512 +
   1.513 +	for(int i=0; i<16; i++) {
   1.514 +		*mptr++ *= scalar;
   1.515 +	}
   1.516 +}
   1.517 +
   1.518 +void Matrix4x4::translate(const Vector3 &trans)
   1.519 +{
   1.520 +	Matrix4x4 tmat(1, 0, 0, trans.x, 0, 1, 0, trans.y, 0, 0, 1, trans.z, 0, 0, 0, 1);
   1.521 +	*this *= tmat;
   1.522 +}
   1.523 +
   1.524 +void Matrix4x4::set_translation(const Vector3 &trans)
   1.525 +{
   1.526 +	*this = Matrix4x4(1, 0, 0, trans.x, 0, 1, 0, trans.y, 0, 0, 1, trans.z, 0, 0, 0, 1);
   1.527 +}
   1.528 +
   1.529 +Vector3 Matrix4x4::get_translation() const
   1.530 +{
   1.531 +	return Vector3(m[0][3], m[1][3], m[2][3]);
   1.532 +}
   1.533 +
   1.534 +void Matrix4x4::rotate(const Vector3 &euler_angles)
   1.535 +{
   1.536 +	Matrix3x3 xrot, yrot, zrot;
   1.537 +
   1.538 +	xrot = Matrix3x3(	1,			0,					0,
   1.539 +						0,	cos(euler_angles.x),	-sin(euler_angles.x),
   1.540 +						0,	sin(euler_angles.x),	cos(euler_angles.x));
   1.541 +
   1.542 +	yrot = Matrix3x3(	cos(euler_angles.y),	0,	sin(euler_angles.y),
   1.543 +								0,				1,				0,
   1.544 +						-sin(euler_angles.y),	0,	cos(euler_angles.y));
   1.545 +
   1.546 +	zrot = Matrix3x3(	cos(euler_angles.z),	-sin(euler_angles.z),	0,
   1.547 +						sin(euler_angles.z),	cos(euler_angles.z),	0,
   1.548 +								0,						0,				1);
   1.549 +
   1.550 +	*this *= Matrix4x4(xrot * yrot * zrot);
   1.551 +}
   1.552 +
   1.553 +void Matrix4x4::set_rotation(const Vector3 &euler_angles)
   1.554 +{
   1.555 +	Matrix3x3 xrot, yrot, zrot;
   1.556 +
   1.557 +	xrot = Matrix3x3(	1,			0,					0,
   1.558 +						0,	cos(euler_angles.x),	-sin(euler_angles.x),
   1.559 +						0,	sin(euler_angles.x),	cos(euler_angles.x));
   1.560 +
   1.561 +	yrot = Matrix3x3(	cos(euler_angles.y),	0,	sin(euler_angles.y),
   1.562 +								0,				1,				0,
   1.563 +						-sin(euler_angles.y),	0,	cos(euler_angles.y));
   1.564 +
   1.565 +	zrot = Matrix3x3(	cos(euler_angles.z),	-sin(euler_angles.z),	0,
   1.566 +						sin(euler_angles.z),	cos(euler_angles.z),	0,
   1.567 +								0,						0,				1);
   1.568 +
   1.569 +	*this = Matrix4x4(xrot * yrot * zrot);
   1.570 +}
   1.571 +
   1.572 +void Matrix4x4::rotate(const Vector3 &axis, scalar_t angle)
   1.573 +{
   1.574 +	scalar_t sina = (scalar_t)sin(angle);
   1.575 +	scalar_t cosa = (scalar_t)cos(angle);
   1.576 +	scalar_t invcosa = 1-cosa;
   1.577 +	scalar_t nxsq = axis.x * axis.x;
   1.578 +	scalar_t nysq = axis.y * axis.y;
   1.579 +	scalar_t nzsq = axis.z * axis.z;
   1.580 +
   1.581 +	Matrix4x4 xform;
   1.582 +	xform[0][0] = nxsq + (1-nxsq) * cosa;
   1.583 +	xform[0][1] = axis.x * axis.y * invcosa - axis.z * sina;
   1.584 +	xform[0][2] = axis.x * axis.z * invcosa + axis.y * sina;
   1.585 +	xform[1][0] = axis.x * axis.y * invcosa + axis.z * sina;
   1.586 +	xform[1][1] = nysq + (1-nysq) * cosa;
   1.587 +	xform[1][2] = axis.y * axis.z * invcosa - axis.x * sina;
   1.588 +	xform[2][0] = axis.x * axis.z * invcosa - axis.y * sina;
   1.589 +	xform[2][1] = axis.y * axis.z * invcosa + axis.x * sina;
   1.590 +	xform[2][2] = nzsq + (1-nzsq) * cosa;
   1.591 +
   1.592 +	*this *= xform;
   1.593 +}
   1.594 +
   1.595 +void Matrix4x4::set_rotation(const Vector3 &axis, scalar_t angle)
   1.596 +{
   1.597 +	scalar_t sina = (scalar_t)sin(angle);
   1.598 +	scalar_t cosa = (scalar_t)cos(angle);
   1.599 +	scalar_t invcosa = 1-cosa;
   1.600 +	scalar_t nxsq = axis.x * axis.x;
   1.601 +	scalar_t nysq = axis.y * axis.y;
   1.602 +	scalar_t nzsq = axis.z * axis.z;
   1.603 +
   1.604 +	reset_identity();
   1.605 +	m[0][0] = nxsq + (1-nxsq) * cosa;
   1.606 +	m[0][1] = axis.x * axis.y * invcosa - axis.z * sina;
   1.607 +	m[0][2] = axis.x * axis.z * invcosa + axis.y * sina;
   1.608 +	m[1][0] = axis.x * axis.y * invcosa + axis.z * sina;
   1.609 +	m[1][1] = nysq + (1-nysq) * cosa;
   1.610 +	m[1][2] = axis.y * axis.z * invcosa - axis.x * sina;
   1.611 +	m[2][0] = axis.x * axis.z * invcosa - axis.y * sina;
   1.612 +	m[2][1] = axis.y * axis.z * invcosa + axis.x * sina;
   1.613 +	m[2][2] = nzsq + (1-nzsq) * cosa;
   1.614 +}
   1.615 +
   1.616 +void Matrix4x4::rotate(const Quaternion &quat)
   1.617 +{
   1.618 +	*this *= quat.get_rotation_matrix();
   1.619 +}
   1.620 +
   1.621 +void Matrix4x4::set_rotation(const Quaternion &quat)
   1.622 +{
   1.623 +	*this = quat.get_rotation_matrix();
   1.624 +}
   1.625 +
   1.626 +Quaternion Matrix4x4::get_rotation_quat() const
   1.627 +{
   1.628 +	Matrix3x3 mat3 = *this;
   1.629 +	return mat3.get_rotation_quat();
   1.630 +}
   1.631 +
   1.632 +void Matrix4x4::scale(const Vector4 &scale_vec)
   1.633 +{
   1.634 +	Matrix4x4 smat(	scale_vec.x, 0, 0, 0,
   1.635 +					0, scale_vec.y, 0, 0,
   1.636 +					0, 0, scale_vec.z, 0,
   1.637 +					0, 0, 0, scale_vec.w);
   1.638 +	*this *= smat;
   1.639 +}
   1.640 +
   1.641 +void Matrix4x4::set_scaling(const Vector4 &scale_vec)
   1.642 +{
   1.643 +	*this = Matrix4x4(	scale_vec.x, 0, 0, 0,
   1.644 +						0, scale_vec.y, 0, 0,
   1.645 +						0, 0, scale_vec.z, 0,
   1.646 +						0, 0, 0, scale_vec.w);
   1.647 +}
   1.648 +
   1.649 +Vector3 Matrix4x4::get_scaling() const
   1.650 +{
   1.651 +	Vector3 vi = get_row_vector(0);
   1.652 +	Vector3 vj = get_row_vector(1);
   1.653 +	Vector3 vk = get_row_vector(2);
   1.654 +
   1.655 +	return Vector3(vi.length(), vj.length(), vk.length());
   1.656 +}
   1.657 +
   1.658 +void Matrix4x4::set_frustum(float left, float right, float bottom, float top, float znear, float zfar)
   1.659 +{
   1.660 +	float dx = right - left;
   1.661 +	float dy = top - bottom;
   1.662 +	float dz = zfar - znear;
   1.663 +
   1.664 +	float a = (right + left) / dx;
   1.665 +	float b = (top + bottom) / dy;
   1.666 +	float c = -(zfar + znear) / dz;
   1.667 +	float d = -2.0 * zfar * znear / dz;
   1.668 +
   1.669 +	*this = Matrix4x4(2.0 * znear / dx, 0, a, 0,
   1.670 +			0, 2.0 * znear / dy, b, 0,
   1.671 +			0, 0, c, d,
   1.672 +			0, 0, -1, 0);
   1.673 +}
   1.674 +
   1.675 +void Matrix4x4::set_perspective(float vfov, float aspect, float znear, float zfar)
   1.676 +{
   1.677 +	float f = 1.0f / tan(vfov * 0.5f);
   1.678 +    float dz = znear - zfar;
   1.679 +
   1.680 +	reset_identity();
   1.681 +
   1.682 +	m[0][0] = f / aspect;
   1.683 +    m[1][1] = f;
   1.684 +    m[2][2] = (zfar + znear) / dz;
   1.685 +    m[3][2] = -1.0f;
   1.686 +    m[2][3] = 2.0f * zfar * znear / dz;
   1.687 +    m[3][3] = 0.0f;
   1.688 +}
   1.689 +
   1.690 +void Matrix4x4::set_orthographic(float left, float right, float bottom, float top, float znear, float zfar)
   1.691 +{
   1.692 +	float dx = right - left;
   1.693 +	float dy = top - bottom;
   1.694 +	float dz = zfar - znear;
   1.695 +
   1.696 +	reset_identity();
   1.697 +
   1.698 +	m[0][0] = 2.0 / dx;
   1.699 +	m[1][1] = 2.0 / dy;
   1.700 +	m[2][2] = -2.0 / dz;
   1.701 +	m[0][3] = -(right + left) / dx;
   1.702 +	m[1][3] = -(top + bottom) / dy;
   1.703 +	m[2][3] = -(zfar + znear) / dz;
   1.704 +}
   1.705 +
   1.706 +void Matrix4x4::set_lookat(const Vector3 &pos, const Vector3 &targ, const Vector3 &up)
   1.707 +{
   1.708 +	Vector3 vk = (targ - pos).normalized();
   1.709 +	Vector3 vj = up.normalized();
   1.710 +	Vector3 vi = cross_product(vk, vj).normalized();
   1.711 +	vj = cross_product(vi, vk);
   1.712 +
   1.713 +	*this = Matrix4x4(
   1.714 +			vi.x, vi.y, vi.z, 0,
   1.715 +			vj.x, vj.y, vj.z, 0,
   1.716 +			-vk.x, -vk.y, -vk.z, 0,
   1.717 +			0, 0, 0, 1);
   1.718 +	translate(-pos);
   1.719 +}
   1.720 +
   1.721 +void Matrix4x4::set_column_vector(const Vector4 &vec, unsigned int col_index)
   1.722 +{
   1.723 +	m[0][col_index] = vec.x;
   1.724 +	m[1][col_index] = vec.y;
   1.725 +	m[2][col_index] = vec.z;
   1.726 +	m[3][col_index] = vec.w;
   1.727 +}
   1.728 +
   1.729 +void Matrix4x4::set_row_vector(const Vector4 &vec, unsigned int row_index)
   1.730 +{
   1.731 +	m[row_index][0] = vec.x;
   1.732 +	m[row_index][1] = vec.y;
   1.733 +	m[row_index][2] = vec.z;
   1.734 +	m[row_index][3] = vec.w;
   1.735 +}
   1.736 +
   1.737 +Vector4 Matrix4x4::get_column_vector(unsigned int col_index) const
   1.738 +{
   1.739 +	return Vector4(m[0][col_index], m[1][col_index], m[2][col_index], m[3][col_index]);
   1.740 +}
   1.741 +
   1.742 +Vector4 Matrix4x4::get_row_vector(unsigned int row_index) const
   1.743 +{
   1.744 +	return Vector4(m[row_index][0], m[row_index][1], m[row_index][2], m[row_index][3]);
   1.745 +}
   1.746 +
   1.747 +void Matrix4x4::transpose()
   1.748 +{
   1.749 +	Matrix4x4 tmp = *this;
   1.750 +	for(int i=0; i<4; i++) {
   1.751 +		for(int j=0; j<4; j++) {
   1.752 +			m[i][j] = tmp[j][i];
   1.753 +		}
   1.754 +	}
   1.755 +}
   1.756 +
   1.757 +Matrix4x4 Matrix4x4::transposed() const
   1.758 +{
   1.759 +	Matrix4x4 res;
   1.760 +	for(int i=0; i<4; i++) {
   1.761 +		for(int j=0; j<4; j++) {
   1.762 +			res[i][j] = m[j][i];
   1.763 +		}
   1.764 +	}
   1.765 +	return res;
   1.766 +}
   1.767 +
   1.768 +scalar_t Matrix4x4::determinant() const
   1.769 +{
   1.770 +	scalar_t det11 =	(m[1][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
   1.771 +						(m[1][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
   1.772 +						(m[1][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
   1.773 +
   1.774 +	scalar_t det12 =	(m[1][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
   1.775 +						(m[1][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
   1.776 +						(m[1][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
   1.777 +
   1.778 +	scalar_t det13 =	(m[1][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
   1.779 +						(m[1][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
   1.780 +						(m[1][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
   1.781 +
   1.782 +	scalar_t det14 =	(m[1][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
   1.783 +						(m[1][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
   1.784 +						(m[1][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
   1.785 +
   1.786 +	return m[0][0] * det11 - m[0][1] * det12 + m[0][2] * det13 - m[0][3] * det14;
   1.787 +}
   1.788 +
   1.789 +
   1.790 +Matrix4x4 Matrix4x4::adjoint() const
   1.791 +{
   1.792 +	Matrix4x4 coef;
   1.793 +
   1.794 +	coef.m[0][0] =	(m[1][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
   1.795 +					(m[1][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
   1.796 +					(m[1][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
   1.797 +	coef.m[0][1] =	(m[1][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
   1.798 +					(m[1][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
   1.799 +					(m[1][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
   1.800 +	coef.m[0][2] =	(m[1][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
   1.801 +					(m[1][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
   1.802 +					(m[1][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
   1.803 +	coef.m[0][3] =	(m[1][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
   1.804 +					(m[1][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
   1.805 +					(m[1][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
   1.806 +
   1.807 +	coef.m[1][0] =	(m[0][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
   1.808 +					(m[0][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
   1.809 +					(m[0][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
   1.810 +	coef.m[1][1] =	(m[0][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
   1.811 +					(m[0][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
   1.812 +					(m[0][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
   1.813 +	coef.m[1][2] =	(m[0][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
   1.814 +					(m[0][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
   1.815 +					(m[0][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
   1.816 +	coef.m[1][3] =	(m[0][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
   1.817 +					(m[0][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
   1.818 +					(m[0][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
   1.819 +
   1.820 +	coef.m[2][0] =	(m[0][1] * (m[1][2] * m[3][3] - m[3][2] * m[1][3])) -
   1.821 +					(m[0][2] * (m[1][1] * m[3][3] - m[3][1] * m[1][3])) +
   1.822 +					(m[0][3] * (m[1][1] * m[3][2] - m[3][1] * m[1][2]));
   1.823 +	coef.m[2][1] =	(m[0][0] * (m[1][2] * m[3][3] - m[3][2] * m[1][3])) -
   1.824 +					(m[0][2] * (m[1][0] * m[3][3] - m[3][0] * m[1][3])) +
   1.825 +					(m[0][3] * (m[1][0] * m[3][2] - m[3][0] * m[1][2]));
   1.826 +	coef.m[2][2] =	(m[0][0] * (m[1][1] * m[3][3] - m[3][1] * m[1][3])) -
   1.827 +					(m[0][1] * (m[1][0] * m[3][3] - m[3][0] * m[1][3])) +
   1.828 +					(m[0][3] * (m[1][0] * m[3][1] - m[3][0] * m[1][1]));
   1.829 +	coef.m[2][3] =	(m[0][0] * (m[1][1] * m[3][2] - m[3][1] * m[1][2])) -
   1.830 +					(m[0][1] * (m[1][0] * m[3][2] - m[3][0] * m[1][2])) +
   1.831 +					(m[0][2] * (m[1][0] * m[3][1] - m[3][0] * m[1][1]));
   1.832 +
   1.833 +	coef.m[3][0] =	(m[0][1] * (m[1][2] * m[2][3] - m[2][2] * m[1][3])) -
   1.834 +					(m[0][2] * (m[1][1] * m[2][3] - m[2][1] * m[1][3])) +
   1.835 +					(m[0][3] * (m[1][1] * m[2][2] - m[2][1] * m[1][2]));
   1.836 +	coef.m[3][1] =	(m[0][0] * (m[1][2] * m[2][3] - m[2][2] * m[1][3])) -
   1.837 +					(m[0][2] * (m[1][0] * m[2][3] - m[2][0] * m[1][3])) +
   1.838 +					(m[0][3] * (m[1][0] * m[2][2] - m[2][0] * m[1][2]));
   1.839 +	coef.m[3][2] =	(m[0][0] * (m[1][1] * m[2][3] - m[2][1] * m[1][3])) -
   1.840 +					(m[0][1] * (m[1][0] * m[2][3] - m[2][0] * m[1][3])) +
   1.841 +					(m[0][3] * (m[1][0] * m[2][1] - m[2][0] * m[1][1]));
   1.842 +	coef.m[3][3] =	(m[0][0] * (m[1][1] * m[2][2] - m[2][1] * m[1][2])) -
   1.843 +					(m[0][1] * (m[1][0] * m[2][2] - m[2][0] * m[1][2])) +
   1.844 +					(m[0][2] * (m[1][0] * m[2][1] - m[2][0] * m[1][1]));
   1.845 +
   1.846 +	coef.transpose();
   1.847 +
   1.848 +	for(int i=0; i<4; i++) {
   1.849 +		for(int j=0; j<4; j++) {
   1.850 +			coef.m[i][j] = j%2 ? -coef.m[i][j] : coef.m[i][j];
   1.851 +			if(i%2) coef.m[i][j] = -coef.m[i][j];
   1.852 +		}
   1.853 +	}
   1.854 +
   1.855 +	return coef;
   1.856 +}
   1.857 +
   1.858 +Matrix4x4 Matrix4x4::inverse() const
   1.859 +{
   1.860 +	Matrix4x4 adj = adjoint();
   1.861 +
   1.862 +	return adj * (1.0f / determinant());
   1.863 +}
   1.864 +
   1.865 +/*
   1.866 +ostream &operator <<(ostream &out, const Matrix4x4 &mat)
   1.867 +{
   1.868 +	for(int i=0; i<4; i++) {
   1.869 +		char str[100];
   1.870 +		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.871 +		out << str;
   1.872 +	}
   1.873 +	return out;
   1.874 +}
   1.875 +*/