git.mutantstargoat.com Git - summerhack/blob - src/3dengfx/src/n3dmath2/n3dmath2_mat.cpp

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