git.mutantstargoat.com Git - summerhack/blob - tools/curve_draw/vmath/matrix.cc

   1 /*
   2 This file is part of XRay, a photorealistic 3D rendering library.
   3 Copyright (C) 2005 John Tsiombikas
   4
   5 XRay is free software; you can redistribute it and/or modify
   6 it under the terms of the GNU General Public License as published by
   7 the Free Software Foundation; either version 2 of the License, or
   8 (at your option) any later version.
   9
  10 XRay is distributed in the hope that it will be useful,
  11 but WITHOUT ANY WARRANTY; without even the implied warranty of
  12 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  13 GNU General Public License for more details.
  14
  15 You should have received a copy of the GNU General Public License
  16 along with XRay; if not, write to the Free Software
  17 Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
  18 */
  19
  20 /**
  21  * @file matrix.cc
  22  * @author John Tsiombikas
  23  * @date 28 June 2005
  24  *
  25  * Matrix math.
  26  */
  27
  28 #include <cstdio>
  29 #include <cmath>
  30 #include "matrix.h"
  31 #include "vector.h"
  32 #include "quaternion.h"
  33
  34 using namespace std;
  35
  36 // ----------- Matrix3x3 --------------
  37
  38 Matrix3x3 Matrix3x3::identity_matrix = Matrix3x3(1, 0, 0, 0, 1, 0, 0, 0, 1);
  39
  40 Matrix3x3::Matrix3x3() {
  41         *this = identity_matrix;
  42 }
  43
  44 Matrix3x3::Matrix3x3(   scalar_t m11, scalar_t m12, scalar_t m13,
  45                                                 scalar_t m21, scalar_t m22, scalar_t m23,
  46                                                 scalar_t m31, scalar_t m32, scalar_t m33) {
  47         m[0][0] = m11; m[0][1] = m12; m[0][2] = m13;
  48         m[1][0] = m21; m[1][1] = m22; m[1][2] = m23;
  49         m[2][0] = m31; m[2][1] = m32; m[2][2] = m33;
  50 }
  51
  52 Matrix3x3::Matrix3x3(const Matrix4x4 &mat4x4) {
  53         for(int i=0; i<3; i++) {
  54                 for(int j=0; j<3; j++) {
  55                         m[i][j] = mat4x4[i][j];
  56                 }
  57         }
  58 }
  59
  60 Matrix3x3 operator +(const Matrix3x3 &m1, const Matrix3x3 &m2) {
  61         Matrix3x3 res;
  62         const scalar_t *op1 = m1.m[0], *op2 = m2.m[0];
  63         scalar_t *dest = res.m[0];
  64
  65         for(int i=0; i<9; i++) {
  66                 *dest++ = *op1++ + *op2++;
  67         }
  68         return res;
  69 }
  70
  71 Matrix3x3 operator -(const Matrix3x3 &m1, const Matrix3x3 &m2) {
  72         Matrix3x3 res;
  73         const scalar_t *op1 = m1.m[0], *op2 = m2.m[0];
  74         scalar_t *dest = res.m[0];
  75
  76         for(int i=0; i<9; i++) {
  77                 *dest++ = *op1++ - *op2++;
  78         }
  79         return res;
  80 }
  81
  82 Matrix3x3 operator *(const Matrix3x3 &m1, const Matrix3x3 &m2) {
  83         Matrix3x3 res;
  84         for(int i=0; i<3; i++) {
  85                 for(int j=0; j<3; j++) {
  86                         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];
  87                 }
  88         }
  89         return res;
  90 }
  91
  92 void operator +=(Matrix3x3 &m1, const Matrix3x3 &m2) {
  93         scalar_t *op1 = m1.m[0];
  94         const scalar_t *op2 = m2.m[0];
  95
  96         for(int i=0; i<9; i++) {
  97                 *op1++ += *op2++;
  98         }
  99 }
 100
 101 void operator -=(Matrix3x3 &m1, const Matrix3x3 &m2) {
 102         scalar_t *op1 = m1.m[0];
 103         const scalar_t *op2 = m2.m[0];
 104
 105         for(int i=0; i<9; i++) {
 106                 *op1++ -= *op2++;
 107         }
 108 }
 109
 110 void operator *=(Matrix3x3 &m1, const Matrix3x3 &m2) {
 111         Matrix3x3 res;
 112         for(int i=0; i<3; i++) {
 113                 for(int j=0; j<3; j++) {
 114                         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];
 115                 }
 116         }
 117         memcpy(m1.m, res.m, 9 * sizeof(scalar_t));
 118 }
 119
 120 Matrix3x3 operator *(const Matrix3x3 &mat, scalar_t scalar) {
 121         Matrix3x3 res;
 122         const scalar_t *mptr = mat.m[0];
 123         scalar_t *dptr = res.m[0];
 124
 125         for(int i=0; i<9; i++) {
 126                 *dptr++ = *mptr++ * scalar;
 127         }
 128         return res;
 129 }
 130
 131 Matrix3x3 operator *(scalar_t scalar, const Matrix3x3 &mat) {
 132         Matrix3x3 res;
 133         const scalar_t *mptr = mat.m[0];
 134         scalar_t *dptr = res.m[0];
 135
 136         for(int i=0; i<9; i++) {
 137                 *dptr++ = *mptr++ * scalar;
 138         }
 139         return res;
 140 }
 141
 142 void operator *=(Matrix3x3 &mat, scalar_t scalar) {
 143         scalar_t *mptr = mat.m[0];
 144
 145         for(int i=0; i<9; i++) {
 146                 *mptr++ *= scalar;
 147         }
 148 }
 149
 150 void Matrix3x3::translate(const Vector2 &trans) {
 151         Matrix3x3 tmat(1, 0, trans.x, 0, 1, trans.y, 0, 0, 1);
 152         *this *= tmat;
 153 }
 154
 155 void Matrix3x3::set_translation(const Vector2 &trans) {
 156         *this = Matrix3x3(1, 0, trans.x, 0, 1, trans.y, 0, 0, 1);
 157 }
 158
 159 void Matrix3x3::rotate(scalar_t angle) {
 160         scalar_t cos_a = cos(angle);
 161         scalar_t sin_a = sin(angle);
 162         Matrix3x3 rmat( cos_a,  -sin_a,         0,
 163                                         sin_a,  cos_a,          0,
 164                                         0,              0,                      1);
 165         *this *= rmat;
 166 }
 167
 168 void Matrix3x3::set_rotation(scalar_t angle) {
 169         scalar_t cos_a = cos(angle);
 170         scalar_t sin_a = sin(angle);
 171         *this = Matrix3x3(cos_a, -sin_a, 0, sin_a, cos_a, 0, 0, 0, 1);
 172 }
 173
 174 void Matrix3x3::rotate(const Vector3 &euler_angles) {
 175         Matrix3x3 xrot, yrot, zrot;
 176
 177         xrot = Matrix3x3(       1,                      0,                                      0,
 178                                                 0,      cos(euler_angles.x),    -sin(euler_angles.x),
 179                                                 0,      sin(euler_angles.x),    cos(euler_angles.x));
 180
 181         yrot = Matrix3x3(       cos(euler_angles.y),    0,      sin(euler_angles.y),
 182                                                                 0,                              1,                              0,
 183                                                 -sin(euler_angles.y),   0,      cos(euler_angles.y));
 184
 185         zrot = Matrix3x3(       cos(euler_angles.z),    -sin(euler_angles.z),   0,
 186                                                 sin(euler_angles.z),    cos(euler_angles.z),    0,
 187                                                                 0,                                              0,                              1);
 188
 189         *this *= xrot * yrot * zrot;
 190 }
 191
 192 void Matrix3x3::set_rotation(const Vector3 &euler_angles) {
 193         Matrix3x3 xrot, yrot, zrot;
 194
 195         xrot = Matrix3x3(       1,                      0,                                      0,
 196                                                 0,      cos(euler_angles.x),    -sin(euler_angles.x),
 197                                                 0,      sin(euler_angles.x),    cos(euler_angles.x));
 198
 199         yrot = Matrix3x3(       cos(euler_angles.y),    0,      sin(euler_angles.y),
 200                                                                 0,                              1,                              0,
 201                                                 -sin(euler_angles.y),   0,      cos(euler_angles.y));
 202
 203         zrot = Matrix3x3(       cos(euler_angles.z),    -sin(euler_angles.z),   0,
 204                                                 sin(euler_angles.z),    cos(euler_angles.z),    0,
 205                                                                 0,                                              0,                              1);
 206
 207         *this = xrot * yrot * zrot;
 208 }
 209
 210 void Matrix3x3::rotate(const Vector3 &axis, scalar_t angle) {
 211         scalar_t sina = (scalar_t)sin(angle);
 212         scalar_t cosa = (scalar_t)cos(angle);
 213         scalar_t invcosa = 1-cosa;
 214         scalar_t nxsq = axis.x * axis.x;
 215         scalar_t nysq = axis.y * axis.y;
 216         scalar_t nzsq = axis.z * axis.z;
 217
 218         Matrix3x3 xform;
 219         xform.m[0][0] = nxsq + (1-nxsq) * cosa;
 220         xform.m[0][1] = axis.x * axis.y * invcosa - axis.z * sina;
 221         xform.m[0][2] = axis.x * axis.z * invcosa + axis.y * sina;
 222         xform.m[1][0] = axis.x * axis.y * invcosa + axis.z * sina;
 223         xform.m[1][1] = nysq + (1-nysq) * cosa;
 224         xform.m[1][2] = axis.y * axis.z * invcosa - axis.x * sina;
 225         xform.m[2][0] = axis.x * axis.z * invcosa - axis.y * sina;
 226         xform.m[2][1] = axis.y * axis.z * invcosa + axis.x * sina;
 227         xform.m[2][2] = nzsq + (1-nzsq) * cosa;
 228
 229         *this *= xform;
 230 }
 231
 232 void Matrix3x3::set_rotation(const Vector3 &axis, scalar_t angle) {
 233         scalar_t sina = (scalar_t)sin(angle);
 234         scalar_t cosa = (scalar_t)cos(angle);
 235         scalar_t invcosa = 1-cosa;
 236         scalar_t nxsq = axis.x * axis.x;
 237         scalar_t nysq = axis.y * axis.y;
 238         scalar_t nzsq = axis.z * axis.z;
 239
 240         reset_identity();
 241         m[0][0] = nxsq + (1-nxsq) * cosa;
 242         m[0][1] = axis.x * axis.y * invcosa - axis.z * sina;
 243         m[0][2] = axis.x * axis.z * invcosa + axis.y * sina;
 244         m[1][0] = axis.x * axis.y * invcosa + axis.z * sina;
 245         m[1][1] = nysq + (1-nysq) * cosa;
 246         m[1][2] = axis.y * axis.z * invcosa - axis.x * sina;
 247         m[2][0] = axis.x * axis.z * invcosa - axis.y * sina;
 248         m[2][1] = axis.y * axis.z * invcosa + axis.x * sina;
 249         m[2][2] = nzsq + (1-nzsq) * cosa;
 250 }
 251
 252 void Matrix3x3::scale(const Vector3 &scale_vec) {
 253         Matrix3x3 smat( scale_vec.x, 0, 0,
 254                                         0, scale_vec.y, 0,
 255                                         0, 0, scale_vec.z);
 256         *this *= smat;
 257 }
 258
 259 void Matrix3x3::set_scaling(const Vector3 &scale_vec) {
 260         *this = Matrix3x3(      scale_vec.x, 0, 0,
 261                                                 0, scale_vec.y, 0,
 262                                                 0, 0, scale_vec.z);
 263 }
 264
 265 void Matrix3x3::set_column_vector(const Vector3 &vec, unsigned int col_index) {
 266         m[0][col_index] = vec.x;
 267         m[1][col_index] = vec.y;
 268         m[2][col_index] = vec.z;
 269 }
 270
 271 void Matrix3x3::set_row_vector(const Vector3 &vec, unsigned int row_index) {
 272         m[row_index][0] = vec.x;
 273         m[row_index][1] = vec.y;
 274         m[row_index][2] = vec.z;
 275 }
 276
 277 Vector3 Matrix3x3::get_column_vector(unsigned int col_index) const {
 278         return Vector3(m[0][col_index], m[1][col_index], m[2][col_index]);
 279 }
 280
 281 Vector3 Matrix3x3::get_row_vector(unsigned int row_index) const {
 282         return Vector3(m[row_index][0], m[row_index][1], m[row_index][2]);
 283 }
 284
 285 void Matrix3x3::transpose() {
 286         Matrix3x3 tmp = *this;
 287         for(int i=0; i<3; i++) {
 288                 for(int j=0; j<3; j++) {
 289                         m[i][j] = tmp[j][i];
 290                 }
 291         }
 292 }
 293
 294 Matrix3x3 Matrix3x3::transposed() const {
 295         Matrix3x3 res;
 296         for(int i=0; i<3; i++) {
 297                 for(int j=0; j<3; j++) {
 298                         res[i][j] = m[j][i];
 299                 }
 300         }
 301         return res;
 302 }
 303
 304 scalar_t Matrix3x3::determinant() const {
 305         return  m[0][0] * (m[1][1]*m[2][2] - m[1][2]*m[2][1]) -
 306                         m[0][1] * (m[1][0]*m[2][2] - m[1][2]*m[2][0]) +
 307                         m[0][2] * (m[1][0]*m[2][1] - m[1][1]*m[2][0]);
 308 }
 309
 310 Matrix3x3 Matrix3x3::inverse() const {
 311         // TODO: implement 3x3 inverse
 312         return *this;
 313 }
 314
 315 ostream &operator <<(ostream &out, const Matrix3x3 &mat) {
 316         for(int i=0; i<3; i++) {
 317                 char str[100];
 318                 sprintf(str, "[ %12.5f %12.5f %12.5f ]\n", (float)mat.m[i][0], (float)mat.m[i][1], (float)mat.m[i][2]);
 319                 out << str;
 320         }
 321         return out;
 322 }
 323
 324
 325
 326 ////////////////////// Matrix4x4 implementation ///////////////////////
 327
 328 Matrix4x4 Matrix4x4::identity_matrix = Matrix4x4(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1);
 329
 330 Matrix4x4::Matrix4x4() {
 331         *this = identity_matrix;
 332 }
 333
 334 Matrix4x4::Matrix4x4(   scalar_t m11, scalar_t m12, scalar_t m13, scalar_t m14,
 335                                                 scalar_t m21, scalar_t m22, scalar_t m23, scalar_t m24,
 336                                                 scalar_t m31, scalar_t m32, scalar_t m33, scalar_t m34,
 337                                                 scalar_t m41, scalar_t m42, scalar_t m43, scalar_t m44) {
 338         m[0][0] = m11; m[0][1] = m12; m[0][2] = m13; m[0][3] = m14;
 339         m[1][0] = m21; m[1][1] = m22; m[1][2] = m23; m[1][3] = m24;
 340         m[2][0] = m31; m[2][1] = m32; m[2][2] = m33; m[2][3] = m34;
 341         m[3][0] = m41; m[3][1] = m42; m[3][2] = m43; m[3][3] = m44;
 342
 343         glmatrix = 0;
 344 }
 345
 346 Matrix4x4::Matrix4x4(const Matrix3x3 &mat3x3) {
 347         reset_identity();
 348         for(int i=0; i<3; i++) {
 349                 for(int j=0; j<3; j++) {
 350                         m[i][j] = mat3x3[i][j];
 351                 }
 352         }
 353
 354         glmatrix = 0;
 355 }
 356
 357 Matrix4x4::~Matrix4x4() {
 358         if(glmatrix) delete [] glmatrix;
 359 }
 360
 361 Matrix4x4 operator +(const Matrix4x4 &m1, const Matrix4x4 &m2) {
 362         Matrix4x4 res;
 363         const scalar_t *op1 = m1.m[0], *op2 = m2.m[0];
 364         scalar_t *dest = res.m[0];
 365
 366         for(int i=0; i<16; i++) {
 367                 *dest++ = *op1++ + *op2++;
 368         }
 369         return res;
 370 }
 371
 372 Matrix4x4 operator -(const Matrix4x4 &m1, const Matrix4x4 &m2) {
 373         Matrix4x4 res;
 374         const scalar_t *op1 = m1.m[0], *op2 = m2.m[0];
 375         scalar_t *dest = res.m[0];
 376
 377         for(int i=0; i<16; i++) {
 378                 *dest++ = *op1++ - *op2++;
 379         }
 380         return res;
 381 }
 382
 383 Matrix4x4 operator *(const Matrix4x4 &m1, const Matrix4x4 &m2) {
 384         Matrix4x4 res;
 385
 386         for(int i=0; i<4; i++) {
 387                 for(int j=0; j<4; j++) {
 388                         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];
 389                 }
 390         }
 391
 392         return res;
 393 }
 394
 395 void operator +=(Matrix4x4 &m1, const Matrix4x4 &m2) {
 396         scalar_t *op1 = m1.m[0];
 397         const scalar_t *op2 = m2.m[0];
 398
 399         for(int i=0; i<16; i++) {
 400                 *op1++ += *op2++;
 401         }
 402 }
 403
 404 void operator -=(Matrix4x4 &m1, const Matrix4x4 &m2) {
 405         scalar_t *op1 = m1.m[0];
 406         const scalar_t *op2 = m2.m[0];
 407
 408         for(int i=0; i<16; i++) {
 409                 *op1++ -= *op2++;
 410         }
 411 }
 412
 413 void operator *=(Matrix4x4 &m1, const Matrix4x4 &m2) {
 414         Matrix4x4 res;
 415         for(int i=0; i<4; i++) {
 416                 for(int j=0; j<4; j++) {
 417                         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];
 418                 }
 419         }
 420         memcpy(m1.m, res.m, 16 * sizeof(scalar_t));
 421 }
 422
 423 Matrix4x4 operator *(const Matrix4x4 &mat, scalar_t scalar) {
 424         Matrix4x4 res;
 425         const scalar_t *mptr = mat.m[0];
 426         scalar_t *dptr = res.m[0];
 427
 428         for(int i=0; i<16; i++) {
 429                 *dptr++ = *mptr++ * scalar;
 430         }
 431         return res;
 432 }
 433
 434 Matrix4x4 operator *(scalar_t scalar, const Matrix4x4 &mat) {
 435         Matrix4x4 res;
 436         const scalar_t *mptr = mat.m[0];
 437         scalar_t *dptr = res.m[0];
 438
 439         for(int i=0; i<16; i++) {
 440                 *dptr++ = *mptr++ * scalar;
 441         }
 442         return res;
 443 }
 444
 445 void operator *=(Matrix4x4 &mat, scalar_t scalar) {
 446         scalar_t *mptr = mat.m[0];
 447
 448         for(int i=0; i<16; i++) {
 449                 *mptr++ *= scalar;
 450         }
 451 }
 452
 453 void Matrix4x4::translate(const Vector3 &trans) {
 454         Matrix4x4 tmat(1, 0, 0, trans.x, 0, 1, 0, trans.y, 0, 0, 1, trans.z, 0, 0, 0, 1);
 455         *this *= tmat;
 456 }
 457
 458 void Matrix4x4::set_translation(const Vector3 &trans) {
 459         *this = Matrix4x4(1, 0, 0, trans.x, 0, 1, 0, trans.y, 0, 0, 1, trans.z, 0, 0, 0, 1);
 460 }
 461
 462 void Matrix4x4::rotate(const Vector3 &euler_angles) {
 463         Matrix3x3 xrot, yrot, zrot;
 464
 465         xrot = Matrix3x3(       1,                      0,                                      0,
 466                                                 0,      cos(euler_angles.x),    -sin(euler_angles.x),
 467                                                 0,      sin(euler_angles.x),    cos(euler_angles.x));
 468
 469         yrot = Matrix3x3(       cos(euler_angles.y),    0,      sin(euler_angles.y),
 470                                                                 0,                              1,                              0,
 471                                                 -sin(euler_angles.y),   0,      cos(euler_angles.y));
 472
 473         zrot = Matrix3x3(       cos(euler_angles.z),    -sin(euler_angles.z),   0,
 474                                                 sin(euler_angles.z),    cos(euler_angles.z),    0,
 475                                                                 0,                                              0,                              1);
 476
 477         *this *= Matrix4x4(xrot * yrot * zrot);
 478 }
 479
 480 void Matrix4x4::set_rotation(const Vector3 &euler_angles) {
 481         Matrix3x3 xrot, yrot, zrot;
 482
 483         xrot = Matrix3x3(       1,                      0,                                      0,
 484                                                 0,      cos(euler_angles.x),    -sin(euler_angles.x),
 485                                                 0,      sin(euler_angles.x),    cos(euler_angles.x));
 486
 487         yrot = Matrix3x3(       cos(euler_angles.y),    0,      sin(euler_angles.y),
 488                                                                 0,                              1,                              0,
 489                                                 -sin(euler_angles.y),   0,      cos(euler_angles.y));
 490
 491         zrot = Matrix3x3(       cos(euler_angles.z),    -sin(euler_angles.z),   0,
 492                                                 sin(euler_angles.z),    cos(euler_angles.z),    0,
 493                                                                 0,                                              0,                              1);
 494
 495         *this = Matrix4x4(xrot * yrot * zrot);
 496 }
 497
 498 void Matrix4x4::rotate(const Vector3 &axis, scalar_t angle) {
 499         scalar_t sina = (scalar_t)sin(angle);
 500         scalar_t cosa = (scalar_t)cos(angle);
 501         scalar_t invcosa = 1-cosa;
 502         scalar_t nxsq = axis.x * axis.x;
 503         scalar_t nysq = axis.y * axis.y;
 504         scalar_t nzsq = axis.z * axis.z;
 505
 506         Matrix3x3 xform;
 507         xform[0][0] = nxsq + (1-nxsq) * cosa;
 508         xform[0][1] = axis.x * axis.y * invcosa - axis.z * sina;
 509         xform[0][2] = axis.x * axis.z * invcosa + axis.y * sina;
 510         xform[1][0] = axis.x * axis.y * invcosa + axis.z * sina;
 511         xform[1][1] = nysq + (1-nysq) * cosa;
 512         xform[1][2] = axis.y * axis.z * invcosa - axis.x * sina;
 513         xform[2][0] = axis.x * axis.z * invcosa - axis.y * sina;
 514         xform[2][1] = axis.y * axis.z * invcosa + axis.x * sina;
 515         xform[2][2] = nzsq + (1-nzsq) * cosa;
 516
 517         *this *= Matrix4x4(xform);
 518 }
 519
 520 void Matrix4x4::set_rotation(const Vector3 &axis, scalar_t angle) {
 521         scalar_t sina = (scalar_t)sin(angle);
 522         scalar_t cosa = (scalar_t)cos(angle);
 523         scalar_t invcosa = 1-cosa;
 524         scalar_t nxsq = axis.x * axis.x;
 525         scalar_t nysq = axis.y * axis.y;
 526         scalar_t nzsq = axis.z * axis.z;
 527
 528         reset_identity();
 529         m[0][0] = nxsq + (1-nxsq) * cosa;
 530         m[0][1] = axis.x * axis.y * invcosa - axis.z * sina;
 531         m[0][2] = axis.x * axis.z * invcosa + axis.y * sina;
 532         m[1][0] = axis.x * axis.y * invcosa + axis.z * sina;
 533         m[1][1] = nysq + (1-nysq) * cosa;
 534         m[1][2] = axis.y * axis.z * invcosa - axis.x * sina;
 535         m[2][0] = axis.x * axis.z * invcosa - axis.y * sina;
 536         m[2][1] = axis.y * axis.z * invcosa + axis.x * sina;
 537         m[2][2] = nzsq + (1-nzsq) * cosa;
 538 }
 539
 540 void Matrix4x4::scale(const Vector4 &scale_vec) {
 541         Matrix4x4 smat( scale_vec.x, 0, 0, 0,
 542                                         0, scale_vec.y, 0, 0,
 543                                         0, 0, scale_vec.z, 0,
 544                                         0, 0, 0, scale_vec.w);
 545         *this *= smat;
 546 }
 547
 548 void Matrix4x4::set_scaling(const Vector4 &scale_vec) {
 549         *this = Matrix4x4(      scale_vec.x, 0, 0, 0,
 550                                                 0, scale_vec.y, 0, 0,
 551                                                 0, 0, scale_vec.z, 0,
 552                                                 0, 0, 0, scale_vec.w);
 553 }
 554
 555 void Matrix4x4::set_column_vector(const Vector4 &vec, unsigned int col_index) {
 556         m[0][col_index] = vec.x;
 557         m[1][col_index] = vec.y;
 558         m[2][col_index] = vec.z;
 559         m[3][col_index] = vec.w;
 560 }
 561
 562 void Matrix4x4::set_row_vector(const Vector4 &vec, unsigned int row_index) {
 563         m[row_index][0] = vec.x;
 564         m[row_index][1] = vec.y;
 565         m[row_index][2] = vec.z;
 566         m[row_index][3] = vec.w;
 567 }
 568
 569 Vector4 Matrix4x4::get_column_vector(unsigned int col_index) const {
 570         return Vector4(m[0][col_index], m[1][col_index], m[2][col_index], m[3][col_index]);
 571 }
 572
 573 Vector4 Matrix4x4::get_row_vector(unsigned int row_index) const {
 574         return Vector4(m[row_index][0], m[row_index][1], m[row_index][2], m[row_index][3]);
 575 }
 576
 577 void Matrix4x4::transpose() {
 578         Matrix4x4 tmp = *this;
 579         for(int i=0; i<4; i++) {
 580                 for(int j=0; j<4; j++) {
 581                         m[i][j] = tmp[j][i];
 582                 }
 583         }
 584 }
 585
 586 Matrix4x4 Matrix4x4::transposed() const {
 587         Matrix4x4 res;
 588         for(int i=0; i<4; i++) {
 589                 for(int j=0; j<4; j++) {
 590                         res[i][j] = m[j][i];
 591                 }
 592         }
 593         return res;
 594 }
 595
 596 scalar_t Matrix4x4::determinant() const {
 597
 598         scalar_t det11 =        (m[1][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
 599                                                 (m[1][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
 600                                                 (m[1][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
 601
 602         scalar_t det12 =        (m[1][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
 603                                                 (m[1][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
 604                                                 (m[1][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
 605
 606         scalar_t det13 =        (m[1][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
 607                                                 (m[1][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
 608                                                 (m[1][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
 609
 610         scalar_t det14 =        (m[1][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
 611                                                 (m[1][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
 612                                                 (m[1][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
 613
 614         return m[0][0] * det11 - m[0][1] * det12 + m[0][2] * det13 - m[0][3] * det14;
 615 }
 616
 617
 618 Matrix4x4 Matrix4x4::adjoint() const {
 619
 620         Matrix4x4 coef;
 621
 622         coef.m[0][0] =  (m[1][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
 623                                         (m[1][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
 624                                         (m[1][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
 625         coef.m[0][1] =  (m[1][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
 626                                         (m[1][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
 627                                         (m[1][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
 628         coef.m[0][2] =  (m[1][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
 629                                         (m[1][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
 630                                         (m[1][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
 631         coef.m[0][3] =  (m[1][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
 632                                         (m[1][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
 633                                         (m[1][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
 634
 635         coef.m[1][0] =  (m[0][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
 636                                         (m[0][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
 637                                         (m[0][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
 638         coef.m[1][1] =  (m[0][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
 639                                         (m[0][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
 640                                         (m[0][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
 641         coef.m[1][2] =  (m[0][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
 642                                         (m[0][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
 643                                         (m[0][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
 644         coef.m[1][3] =  (m[0][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
 645                                         (m[0][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
 646                                         (m[0][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
 647
 648         coef.m[2][0] =  (m[0][1] * (m[1][2] * m[3][3] - m[3][2] * m[1][3])) -
 649                                         (m[0][2] * (m[1][1] * m[3][3] - m[3][1] * m[1][3])) +
 650                                         (m[0][3] * (m[1][1] * m[3][2] - m[3][1] * m[1][2]));
 651         coef.m[2][1] =  (m[0][0] * (m[1][2] * m[3][3] - m[3][2] * m[1][3])) -
 652                                         (m[0][2] * (m[1][0] * m[3][3] - m[3][0] * m[1][3])) +
 653                                         (m[0][3] * (m[1][0] * m[3][2] - m[3][0] * m[1][2]));
 654         coef.m[2][2] =  (m[0][0] * (m[1][1] * m[3][3] - m[3][1] * m[1][3])) -
 655                                         (m[0][1] * (m[1][0] * m[3][3] - m[3][0] * m[1][3])) +
 656                                         (m[0][3] * (m[1][0] * m[3][1] - m[3][0] * m[1][1]));
 657         coef.m[2][3] =  (m[0][0] * (m[1][1] * m[3][2] - m[3][1] * m[1][2])) -
 658                                         (m[0][1] * (m[1][0] * m[3][2] - m[3][0] * m[1][2])) +
 659                                         (m[0][2] * (m[1][0] * m[3][1] - m[3][0] * m[1][1]));
 660
 661         coef.m[3][0] =  (m[0][1] * (m[1][2] * m[2][3] - m[2][2] * m[1][3])) -
 662                                         (m[0][2] * (m[1][1] * m[2][3] - m[2][1] * m[1][3])) +
 663                                         (m[0][3] * (m[1][1] * m[2][2] - m[2][1] * m[1][2]));
 664         coef.m[3][1] =  (m[0][0] * (m[1][2] * m[2][3] - m[2][2] * m[1][3])) -
 665                                         (m[0][2] * (m[1][0] * m[2][3] - m[2][0] * m[1][3])) +
 666                                         (m[0][3] * (m[1][0] * m[2][2] - m[2][0] * m[1][2]));
 667         coef.m[3][2] =  (m[0][0] * (m[1][1] * m[2][3] - m[2][1] * m[1][3])) -
 668                                         (m[0][1] * (m[1][0] * m[2][3] - m[2][0] * m[1][3])) +
 669                                         (m[0][3] * (m[1][0] * m[2][1] - m[2][0] * m[1][1]));
 670         coef.m[3][3] =  (m[0][0] * (m[1][1] * m[2][2] - m[2][1] * m[1][2])) -
 671                                         (m[0][1] * (m[1][0] * m[2][2] - m[2][0] * m[1][2])) +
 672                                         (m[0][2] * (m[1][0] * m[2][1] - m[2][0] * m[1][1]));
 673
 674         coef.transpose();
 675
 676         for(int i=0; i<4; i++) {
 677                 for(int j=0; j<4; j++) {
 678                         coef.m[i][j] = j%2 ? -coef.m[i][j] : coef.m[i][j];
 679                         if(i%2) coef.m[i][j] = -coef.m[i][j];
 680                 }
 681         }
 682
 683         return coef;
 684 }
 685
 686 Matrix4x4 Matrix4x4::inverse() const {
 687
 688         Matrix4x4 AdjMat = adjoint();
 689
 690         return AdjMat * (1.0f / determinant());
 691 }
 692
 693 const scalar_t *Matrix4x4::opengl_matrix() const {
 694         return (const scalar_t*)m;
 695 }
 696
 697 ostream &operator <<(ostream &out, const Matrix4x4 &mat) {
 698         for(int i=0; i<4; i++) {
 699                 char str[100];
 700                 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]);
 701                 out << str;
 702         }
 703         return out;
 704 }