Extras/glui/algebra3.cpp

   1 /*
   2
   3   algebra3.cpp, algebra3.h -  C++ Vector and Matrix Algebra routines
   4
   5   GLUI User Interface Toolkit (LGPL)
   6   Copyright (c) 1998 Paul Rademacher
   7
   8   WWW:    http://sourceforge.net/projects/glui/
   9   Forums: http://sourceforge.net/forum/?group_id=92496
  10
  11   This library is free software; you can redistribute it and/or
  12   modify it under the terms of the GNU Lesser General Public
  13   License as published by the Free Software Foundation; either
  14   version 2.1 of the License, or (at your option) any later version.
  15
  16   This library is distributed in the hope that it will be useful,
  17   but WITHOUT ANY WARRANTY; without even the implied warranty of
  18   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  19   Lesser General Public License for more details.
  20
  21   You should have received a copy of the GNU Lesser General Public
  22   License along with this library; if not, write to the Free Software
  23   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  24
  25 */
  26
  27 /**************************************************************************
  28
  29   There are three vector classes and two matrix classes: vec2, vec3,
  30   vec4, mat3, and mat4.
  31
  32   All the standard arithmetic operations are defined, with '*'
  33   for dot product of two vectors and multiplication of two matrices,
  34   and '^' for cross product of two vectors.
  35
  36   Additional functions include length(), normalize(), homogenize for
  37   vectors, and print(), set(), apply() for all classes.
  38
  39   There is a function transpose() for matrices, but note that it
  40   does not actually change the matrix,
  41
  42   When multiplied with a matrix, a vector is treated as a row vector
  43   if it precedes the matrix (v*M), and as a column vector if it
  44   follows the matrix (M*v).
  45
  46   Matrices are stored in row-major form.
  47
  48   A vector of one dimension (2d, 3d, or 4d) can be cast to a vector
  49   of a higher or lower dimension.  If casting to a higher dimension,
  50   the new component is set by default to 1.0, unless a value is
  51   specified:
  52      vec3 a(1.0, 2.0, 3.0 );
  53      vec4 b( a, 4.0 );       // now b == {1.0, 2.0, 3.0, 4.0};
  54   When casting to a lower dimension, the vector is homogenized in
  55   the lower dimension.  E.g., if a 4d {X,Y,Z,W} is cast to 3d, the
  56   resulting vector is {X/W, Y/W, Z/W}.  It is up to the user to
  57   insure the fourth component is not zero before casting.
  58
  59   There are also the following function for building matrices:
  60      identity2D(), translation2D(), rotation2D(),
  61      scaling2D(),  identity3D(),    translation3D(),
  62      rotation3D(), rotation3Drad(),  scaling3D(),
  63      perspective3D()
  64
  65
  66   ---------------------------------------------------------------------
  67
  68   Author: Jean-Francois DOUEg
  69   Revised: Paul Rademacher
  70   Version 3.2 - Feb 1998
  71   Revised: Nigel Stewart (GLUI Code Cleaning)
  72
  73 **************************************************************************/
  74
  75 #include "algebra3.h"
  76 #include "glui_internal.h"
  77 #include <cmath>
  78
  79 #ifdef VEC_ERROR_FATAL
  80 #ifndef VEC_ERROR
  81 #define VEC_ERROR(E) { printf( "VERROR %s\n", E ); exit(1); }
  82 #endif
  83 #else
  84 #ifndef VEC_ERROR
  85 #define VEC_ERROR(E) { printf( "VERROR %s\n", E ); }
  86 #endif
  87 #endif
  88
  89 /****************************************************************
  90  *                                                              *
  91  *          vec2 Member functions                               *
  92  *                                                              *
  93  ****************************************************************/
  94
  95 /******************** vec2 CONSTRUCTORS ********************/
  96
  97 vec2::vec2()
  98 {
  99     n[VX] = n[VY] = 0.0;
 100 }
 101
 102 vec2::vec2(float x, float y)
 103 {
 104     n[VX] = x;
 105     n[VY] = y;
 106 }
 107
 108 vec2::vec2(const vec2 &v)
 109 {
 110     n[VX] = v.n[VX];
 111     n[VY] = v.n[VY];
 112 }
 113
 114 vec2::vec2(const vec3 &v) // it is up to caller to avoid divide-by-zero
 115 {
 116     n[VX] = v.n[VX]/v.n[VZ];
 117     n[VY] = v.n[VY]/v.n[VZ];
 118 }
 119
 120 vec2::vec2(const vec3 &v, int dropAxis)
 121 {
 122     switch (dropAxis)
 123     {
 124         case VX: n[VX] = v.n[VY]; n[VY] = v.n[VZ]; break;
 125         case VY: n[VX] = v.n[VX]; n[VY] = v.n[VZ]; break;
 126         default: n[VX] = v.n[VX]; n[VY] = v.n[VY]; break;
 127     }
 128 }
 129
 130 /******************** vec2 ASSIGNMENT OPERATORS ******************/
 131
 132 vec2 & vec2::operator=(const vec2 &v)
 133 {
 134     n[VX] = v.n[VX];
 135     n[VY] = v.n[VY];
 136     return *this;
 137 }
 138
 139 vec2 & vec2::operator+=(const vec2 &v)
 140 {
 141     n[VX] += v.n[VX];
 142     n[VY] += v.n[VY];
 143     return *this;
 144 }
 145
 146 vec2 & vec2::operator-=(const vec2 &v)
 147 {
 148     n[VX] -= v.n[VX];
 149     n[VY] -= v.n[VY];
 150     return *this;
 151 }
 152
 153 vec2 &vec2::operator*=(float d)
 154 {
 155     n[VX] *= d;
 156     n[VY] *= d;
 157     return *this;
 158 }
 159
 160 vec2 &vec2::operator/=(float d)
 161 {
 162     float d_inv = 1.0f/d;
 163     n[VX] *= d_inv;
 164     n[VY] *= d_inv;
 165     return *this;
 166 }
 167
 168 float &vec2::operator[](int i)
 169 {
 170     if (i < VX || i > VY)
 171       //VEC_ERROR("vec2 [] operator: illegal access; index = " << i << '\n')
 172       VEC_ERROR("vec2 [] operator: illegal access" );
 173     return n[i];
 174 }
 175
 176 const float &vec2::operator[](int i) const
 177 {
 178     if (i < VX || i > VY)
 179       //VEC_ERROR("vec2 [] operator: illegal access; index = " << i << '\n')
 180       VEC_ERROR("vec2 [] operator: illegal access" );
 181
 182     return n[i];
 183 }
 184
 185 /******************** vec2 SPECIAL FUNCTIONS ********************/
 186
 187 float vec2::length() const
 188 {
 189     return (float) sqrt(length2());
 190 }
 191
 192 float vec2::length2() const
 193 {
 194     return n[VX]*n[VX] + n[VY]*n[VY];
 195 }
 196
 197 vec2 &vec2::normalize() // it is up to caller to avoid divide-by-zero
 198 {
 199     *this /= length();
 200     return *this;
 201 }
 202
 203 vec2 &vec2::apply(V_FCT_PTR fct)
 204 {
 205     n[VX] = (*fct)(n[VX]);
 206     n[VY] = (*fct)(n[VY]);
 207     return *this;
 208 }
 209
 210 void vec2::set( float x, float y )
 211 {
 212   n[VX] = x;   n[VY] = y;
 213 }
 214
 215 /******************** vec2 FRIENDS *****************************/
 216
 217 vec2 operator-(const vec2 &a)
 218 {
 219     return vec2(-a.n[VX],-a.n[VY]);
 220 }
 221
 222 vec2 operator+(const vec2 &a, const vec2& b)
 223 {
 224     return vec2(a.n[VX]+b.n[VX], a.n[VY]+b.n[VY]);
 225 }
 226
 227 vec2 operator-(const vec2 &a, const vec2& b)
 228 {
 229     return vec2(a.n[VX]-b.n[VX], a.n[VY]-b.n[VY]);
 230 }
 231
 232 vec2 operator*(const vec2 &a, float d)
 233 {
 234     return vec2(d*a.n[VX], d*a.n[VY]);
 235 }
 236
 237 vec2 operator*(float d, const vec2 &a)
 238 {
 239     return a*d;
 240 }
 241
 242 vec2 operator*(const mat3 &a, const vec2 &v)
 243 {
 244   vec3 av;
 245
 246   av.n[VX] = a.v[0].n[VX]*v.n[VX] + a.v[0].n[VY]*v.n[VY] + a.v[0].n[VZ];
 247   av.n[VY] = a.v[1].n[VX]*v.n[VX] + a.v[1].n[VY]*v.n[VY] + a.v[1].n[VZ];
 248   av.n[VZ] = a.v[2].n[VX]*v.n[VX] + a.v[2].n[VY]*v.n[VY] + a.v[2].n[VZ];
 249
 250   return av;
 251 }
 252
 253 vec2 operator*(const vec2 &v, const mat3 &a)
 254 {
 255     return a.transpose() * v;
 256 }
 257
 258 vec3 operator*(const mat3 &a, const vec3 &v)
 259 {
 260     vec3 av;
 261
 262     av.n[VX] = a.v[0].n[VX]*v.n[VX] + a.v[0].n[VY]*v.n[VY] + a.v[0].n[VZ]*v.n[VZ];
 263     av.n[VY] = a.v[1].n[VX]*v.n[VX] + a.v[1].n[VY]*v.n[VY] + a.v[1].n[VZ]*v.n[VZ];
 264     av.n[VZ] = a.v[2].n[VX]*v.n[VX] + a.v[2].n[VY]*v.n[VY] + a.v[2].n[VZ]*v.n[VZ];
 265
 266     return av;
 267 }
 268
 269 vec3 operator*(const vec3 &v, const mat3 &a)
 270 {
 271     return a.transpose() * v;
 272 }
 273
 274 float operator*(const vec2 &a, const vec2 &b)
 275 {
 276     return a.n[VX]*b.n[VX] + a.n[VY]*b.n[VY];
 277 }
 278
 279 vec2 operator/(const vec2 &a, float d)
 280 {
 281     float d_inv = 1.0f/d;
 282     return vec2(a.n[VX]*d_inv, a.n[VY]*d_inv);
 283 }
 284
 285 vec3 operator^(const vec2 &a, const vec2 &b)
 286 {
 287     return vec3(0.0, 0.0, a.n[VX] * b.n[VY] - b.n[VX] * a.n[VY]);
 288 }
 289
 290 int operator==(const vec2 &a, const vec2 &b)
 291 {
 292     return (a.n[VX] == b.n[VX]) && (a.n[VY] == b.n[VY]);
 293 }
 294
 295 int operator!=(const vec2 &a, const vec2 &b)
 296 {
 297     return !(a == b);
 298 }
 299
 300 /*ostream& operator << (ostream& s, vec2& v)
 301 { return s << "| " << v.n[VX] << ' ' << v.n[VY] << " |"; }
 302 */
 303
 304 /*istream& operator >> (istream& s, vec2& v) {
 305     vec2    v_tmp;
 306     char    c = ' ';
 307
 308     while (isspace(c))
 309     s >> c;
 310     // The vectors can be formatted either as x y or | x y |
 311     if (c == '|') {
 312     s >> v_tmp[VX] >> v_tmp[VY];
 313     while (s >> c && isspace(c)) ;
 314     if (c != '|')
 315         ;//s.set(_bad);
 316     }
 317     else {
 318     s.putback(c);
 319     s >> v_tmp[VX] >> v_tmp[VY];
 320     }
 321     if (s)
 322     v = v_tmp;
 323     return s;
 324 }
 325 */
 326
 327 void swap(vec2 &a, vec2 &b)
 328 {
 329     vec2 tmp(a);
 330     a = b;
 331     b = tmp;
 332 }
 333
 334 vec2 min_vec(const vec2 &a, const vec2 &b)
 335 {
 336     return vec2(MIN(a.n[VX], b.n[VX]), MIN(a.n[VY], b.n[VY]));
 337 }
 338
 339 vec2 max_vec(const vec2 &a, const vec2 &b)
 340 {
 341     return vec2(MAX(a.n[VX], b.n[VX]), MAX(a.n[VY], b.n[VY]));
 342 }
 343
 344 vec2 prod(const vec2 &a, const vec2 &b)
 345 {
 346     return vec2(a.n[VX] * b.n[VX], a.n[VY] * b.n[VY]);
 347 }
 348
 349 /****************************************************************
 350  *                                                              *
 351  *          vec3 Member functions                               *
 352  *                                                              *
 353  ****************************************************************/
 354
 355 // CONSTRUCTORS
 356
 357 vec3::vec3()
 358 {
 359     n[VX] = n[VY] = n[VZ] = 0.0;
 360 }
 361
 362 vec3::vec3(float x, float y, float z)
 363 {
 364     n[VX] = x;
 365     n[VY] = y;
 366     n[VZ] = z;
 367 }
 368
 369 vec3::vec3(const vec3 &v)
 370 {
 371     n[VX] = v.n[VX]; n[VY] = v.n[VY]; n[VZ] = v.n[VZ];
 372 }
 373
 374 vec3::vec3(const vec2 &v)
 375 {
 376     n[VX] = v.n[VX];
 377     n[VY] = v.n[VY];
 378     n[VZ] = 1.0;
 379 }
 380
 381 vec3::vec3(const vec2 &v, float d)
 382 {
 383     n[VX] = v.n[VX];
 384     n[VY] = v.n[VY];
 385     n[VZ] = d;
 386 }
 387
 388 vec3::vec3(const vec4 &v) // it is up to caller to avoid divide-by-zero
 389 {
 390     n[VX] = v.n[VX] / v.n[VW];
 391     n[VY] = v.n[VY] / v.n[VW];
 392     n[VZ] = v.n[VZ] / v.n[VW];
 393 }
 394
 395 vec3::vec3(const vec4 &v, int dropAxis)
 396 {
 397     switch (dropAxis)
 398     {
 399         case VX: n[VX] = v.n[VY]; n[VY] = v.n[VZ]; n[VZ] = v.n[VW]; break;
 400         case VY: n[VX] = v.n[VX]; n[VY] = v.n[VZ]; n[VZ] = v.n[VW]; break;
 401         case VZ: n[VX] = v.n[VX]; n[VY] = v.n[VY]; n[VZ] = v.n[VW]; break;
 402         default: n[VX] = v.n[VX]; n[VY] = v.n[VY]; n[VZ] = v.n[VZ]; break;
 403     }
 404 }
 405
 406
 407 // ASSIGNMENT OPERATORS
 408
 409 vec3 &vec3::operator=(const vec3 &v)
 410 {
 411     n[VX] = v.n[VX];
 412     n[VY] = v.n[VY];
 413     n[VZ] = v.n[VZ];
 414     return *this;
 415 }
 416
 417 vec3 &vec3::operator+=(const vec3 &v)
 418 {
 419     n[VX] += v.n[VX];
 420     n[VY] += v.n[VY];
 421     n[VZ] += v.n[VZ];
 422     return *this;
 423 }
 424
 425 vec3 &vec3::operator-=(const vec3& v)
 426 {
 427     n[VX] -= v.n[VX];
 428     n[VY] -= v.n[VY];
 429     n[VZ] -= v.n[VZ];
 430     return *this;
 431 }
 432
 433 vec3 &vec3::operator*=(float d)
 434 {
 435     n[VX] *= d;
 436     n[VY] *= d;
 437     n[VZ] *= d;
 438     return *this;
 439 }
 440
 441 vec3 &vec3::operator/=(float d)
 442 {
 443     float d_inv = 1.0f/d;
 444     n[VX] *= d_inv;
 445     n[VY] *= d_inv;
 446     n[VZ] *= d_inv;
 447     return *this;
 448 }
 449
 450 float &vec3::operator[](int i)
 451 {
 452     if (i < VX || i > VZ)
 453         //VEC_ERROR("vec3 [] operator: illegal access; index = " << i << '\n')
 454         VEC_ERROR("vec3 [] operator: illegal access" );
 455
 456     return n[i];
 457 }
 458
 459 const float &vec3::operator[](int i) const
 460 {
 461     if (i < VX || i > VZ)
 462         //VEC_ERROR("vec3 [] operator: illegal access; index = " << i << '\n')
 463         VEC_ERROR("vec3 [] operator: illegal access" );
 464
 465     return n[i];
 466 }
 467
 468 // SPECIAL FUNCTIONS
 469
 470 float vec3::length() const
 471 {
 472     return (float) sqrt(length2());
 473 }
 474
 475 float vec3::length2() const
 476 {
 477     return n[VX]*n[VX] + n[VY]*n[VY] + n[VZ]*n[VZ];
 478 }
 479
 480 vec3 &vec3::normalize() // it is up to caller to avoid divide-by-zero
 481 {
 482     *this /= length();
 483     return *this;
 484 }
 485
 486 vec3 &vec3::homogenize(void) // it is up to caller to avoid divide-by-zero
 487 {
 488     n[VX] /= n[VZ];
 489     n[VY] /= n[VZ];
 490     n[VZ] = 1.0;
 491     return *this;
 492 }
 493
 494 vec3 &vec3::apply(V_FCT_PTR fct)
 495 {
 496     n[VX] = (*fct)(n[VX]);
 497     n[VY] = (*fct)(n[VY]);
 498     n[VZ] = (*fct)(n[VZ]);
 499     return *this;
 500 }
 501
 502 void vec3::set(float x, float y, float z)   // set vector
 503 {
 504     n[VX] = x;
 505     n[VY] = y;
 506     n[VZ] = z;
 507 }
 508
 509 void vec3::print(FILE *file, const char *name) const  // print vector to a file
 510 {
 511     fprintf( file, "%s: <%f, %f, %f>\n", name, n[VX], n[VY], n[VZ] );
 512 }
 513
 514 // FRIENDS
 515
 516 vec3 operator-(const vec3 &a)
 517 {
 518     return vec3(-a.n[VX],-a.n[VY],-a.n[VZ]);
 519 }
 520
 521 vec3 operator+(const vec3 &a, const vec3 &b)
 522 {
 523     return vec3(a.n[VX]+ b.n[VX], a.n[VY] + b.n[VY], a.n[VZ] + b.n[VZ]);
 524 }
 525
 526 vec3 operator-(const vec3 &a, const vec3 &b)
 527 {
 528     return vec3(a.n[VX]-b.n[VX], a.n[VY]-b.n[VY], a.n[VZ]-b.n[VZ]);
 529 }
 530
 531 vec3 operator*(const vec3 &a, float d)
 532 {
 533     return vec3(d*a.n[VX], d*a.n[VY], d*a.n[VZ]);
 534 }
 535
 536 vec3 operator*(float d, const vec3 &a)
 537 {
 538     return a*d;
 539 }
 540
 541 vec3 operator*(const mat4 &a, const vec3 &v)
 542 {
 543     return a*vec4(v);
 544 }
 545
 546 vec3 operator*(const vec3 &v, mat4 &a)
 547 {
 548     return a.transpose()*v;
 549 }
 550
 551 float operator*(const vec3 &a, const vec3 &b)
 552 {
 553     return a.n[VX]*b.n[VX] + a.n[VY]*b.n[VY] + a.n[VZ]*b.n[VZ];
 554 }
 555
 556 vec3 operator/(const vec3 &a, float d)
 557 {
 558     float d_inv = 1.0f/d;
 559     return vec3(a.n[VX]*d_inv, a.n[VY]*d_inv, a.n[VZ]*d_inv);
 560 }
 561
 562 vec3 operator^(const vec3 &a, const vec3 &b)
 563 {
 564     return
 565         vec3(a.n[VY]*b.n[VZ] - a.n[VZ]*b.n[VY],
 566              a.n[VZ]*b.n[VX] - a.n[VX]*b.n[VZ],
 567              a.n[VX]*b.n[VY] - a.n[VY]*b.n[VX]);
 568 }
 569
 570 int operator==(const vec3 &a, const vec3 &b)
 571 {
 572     return (a.n[VX] == b.n[VX]) && (a.n[VY] == b.n[VY]) && (a.n[VZ] == b.n[VZ]);
 573 }
 574
 575 int operator!=(const vec3 &a, const vec3 &b)
 576 {
 577     return !(a == b);
 578 }
 579
 580 /*ostream& operator << (ostream& s, vec3& v)
 581 { return s << "| " << v.n[VX] << ' ' << v.n[VY] << ' ' << v.n[VZ] << " |"; }
 582
 583 istream& operator >> (istream& s, vec3& v) {
 584     vec3    v_tmp;
 585     char    c = ' ';
 586
 587     while (isspace(c))
 588     s >> c;
 589     // The vectors can be formatted either as x y z or | x y z |
 590     if (c == '|') {
 591     s >> v_tmp[VX] >> v_tmp[VY] >> v_tmp[VZ];
 592     while (s >> c && isspace(c)) ;
 593     if (c != '|')
 594         ;//s.set(_bad);
 595     }
 596     else {
 597     s.putback(c);
 598     s >> v_tmp[VX] >> v_tmp[VY] >> v_tmp[VZ];
 599     }
 600     if (s)
 601     v = v_tmp;
 602     return s;
 603 }
 604 */
 605
 606 void swap(vec3 &a, vec3 &b)
 607 {
 608     vec3 tmp(a);
 609     a = b;
 610     b = tmp;
 611 }
 612
 613 vec3 min_vec(const vec3 &a, const vec3 &b)
 614 {
 615     return vec3(
 616         MIN(a.n[VX], b.n[VX]),
 617         MIN(a.n[VY], b.n[VY]),
 618         MIN(a.n[VZ], b.n[VZ]));
 619 }
 620
 621 vec3 max_vec(const vec3 &a, const vec3 &b)
 622 {
 623     return vec3(
 624         MAX(a.n[VX], b.n[VX]),
 625         MAX(a.n[VY], b.n[VY]),
 626         MAX(a.n[VZ], b.n[VZ]));
 627 }
 628
 629 vec3 prod(const vec3 &a, const vec3 &b)
 630 {
 631     return vec3(a.n[VX]*b.n[VX], a.n[VY]*b.n[VY], a.n[VZ]*b.n[VZ]);
 632 }
 633
 634 /****************************************************************
 635  *                                                              *
 636  *          vec4 Member functions                               *
 637  *                                                              *
 638  ****************************************************************/
 639
 640 // CONSTRUCTORS
 641
 642 vec4::vec4()
 643 {
 644     n[VX] = n[VY] = n[VZ] = 0.0;
 645     n[VW] = 1.0;
 646 }
 647
 648 vec4::vec4(float x, float y, float z, float w)
 649 {
 650     n[VX] = x;
 651     n[VY] = y;
 652     n[VZ] = z;
 653     n[VW] = w;
 654 }
 655
 656 vec4::vec4(const vec4 &v)
 657 {
 658     n[VX] = v.n[VX];
 659     n[VY] = v.n[VY];
 660     n[VZ] = v.n[VZ];
 661     n[VW] = v.n[VW];
 662 }
 663
 664 vec4::vec4(const vec3 &v)
 665 {
 666     n[VX] = v.n[VX];
 667     n[VY] = v.n[VY];
 668     n[VZ] = v.n[VZ];
 669     n[VW] = 1.0;
 670 }
 671
 672 vec4::vec4(const vec3 &v, float d)
 673 {
 674     n[VX] = v.n[VX];
 675     n[VY] = v.n[VY];
 676     n[VZ] = v.n[VZ];
 677     n[VW] = d;
 678 }
 679
 680 // ASSIGNMENT OPERATORS
 681
 682 vec4 &vec4::operator=(const vec4 &v)
 683 {
 684     n[VX] = v.n[VX];
 685     n[VY] = v.n[VY];
 686     n[VZ] = v.n[VZ];
 687     n[VW] = v.n[VW];
 688     return *this;
 689 }
 690
 691 vec4 &vec4::operator+=(const vec4 &v)
 692 {
 693     n[VX] += v.n[VX];
 694     n[VY] += v.n[VY];
 695     n[VZ] += v.n[VZ];
 696     n[VW] += v.n[VW];
 697     return *this;
 698 }
 699
 700 vec4 &vec4::operator-=(const vec4 &v)
 701 {
 702     n[VX] -= v.n[VX];
 703     n[VY] -= v.n[VY];
 704     n[VZ] -= v.n[VZ];
 705     n[VW] -= v.n[VW];
 706     return *this;
 707 }
 708
 709 vec4 &vec4::operator*=(float d)
 710 {
 711     n[VX] *= d;
 712     n[VY] *= d;
 713     n[VZ] *= d;
 714     n[VW] *= d;
 715     return *this;
 716 }
 717
 718 vec4 &vec4::operator/=(float d)
 719 {
 720     float d_inv = 1.0f/d;
 721     n[VX] *= d_inv;
 722     n[VY] *= d_inv;
 723     n[VZ] *= d_inv;
 724     n[VW] *= d_inv;
 725     return *this;
 726 }
 727
 728 float &vec4::operator[](int i)
 729 {
 730     if (i < VX || i > VW)
 731         //VEC_ERROR("vec4 [] operator: illegal access; index = " << i << '\n')
 732         VEC_ERROR("vec4 [] operator: illegal access" );
 733
 734     return n[i];
 735 }
 736
 737 const float &vec4::operator[](int i) const
 738 {
 739     if (i < VX || i > VW)
 740         //VEC_ERROR("vec4 [] operator: illegal access; index = " << i << '\n')
 741         VEC_ERROR("vec4 [] operator: illegal access" );
 742
 743     return n[i];
 744 }
 745
 746 // SPECIAL FUNCTIONS
 747
 748 float vec4::length() const
 749 {
 750     return (float) sqrt(length2());
 751 }
 752
 753 float vec4::length2() const
 754 {
 755     return n[VX]*n[VX] + n[VY]*n[VY] + n[VZ]*n[VZ] + n[VW]*n[VW];
 756 }
 757
 758 vec4 &vec4::normalize() // it is up to caller to avoid divide-by-zero
 759 {
 760     *this /= length();
 761     return *this;
 762 }
 763
 764 vec4 &vec4::homogenize() // it is up to caller to avoid divide-by-zero
 765 {
 766     n[VX] /= n[VW];
 767     n[VY] /= n[VW];
 768     n[VZ] /= n[VW];
 769     n[VW] = 1.0;
 770     return *this;
 771 }
 772
 773 vec4 &vec4::apply(V_FCT_PTR fct)
 774 {
 775     n[VX] = (*fct)(n[VX]);
 776     n[VY] = (*fct)(n[VY]);
 777     n[VZ] = (*fct)(n[VZ]);
 778     n[VW] = (*fct)(n[VW]);
 779     return *this;
 780 }
 781
 782 void vec4::print(FILE *file, const char *name) const // print vector to a file
 783 {
 784     fprintf( file, "%s: <%f, %f, %f, %f>\n", name, n[VX], n[VY], n[VZ], n[VW]);
 785 }
 786
 787 void vec4::set(float x, float y, float z, float a)
 788 {
 789     n[0] = x;
 790     n[1] = y;
 791     n[2] = z;
 792     n[3] = a;
 793 }
 794
 795
 796 // FRIENDS
 797
 798 vec4 operator-(const vec4 &a)
 799 {
 800     return vec4(-a.n[VX],-a.n[VY],-a.n[VZ],-a.n[VW]);
 801 }
 802
 803 vec4 operator+(const vec4 &a, const vec4 &b)
 804 {
 805     return vec4(
 806         a.n[VX] + b.n[VX],
 807         a.n[VY] + b.n[VY],
 808         a.n[VZ] + b.n[VZ],
 809         a.n[VW] + b.n[VW]);
 810 }
 811
 812 vec4 operator-(const vec4 &a, const vec4 &b)
 813 {
 814     return vec4(
 815         a.n[VX] - b.n[VX],
 816         a.n[VY] - b.n[VY],
 817         a.n[VZ] - b.n[VZ],
 818         a.n[VW] - b.n[VW]);
 819 }
 820
 821 vec4 operator*(const vec4 &a, float d)
 822 {
 823     return vec4(d*a.n[VX], d*a.n[VY], d*a.n[VZ], d*a.n[VW]);
 824 }
 825
 826 vec4 operator*(float d, const vec4 &a)
 827 {
 828     return a*d;
 829 }
 830
 831 vec4 operator*(const mat4 &a, const vec4 &v)
 832 {
 833     #define ROWCOL(i) \
 834         a.v[i].n[0]*v.n[VX] + \
 835         a.v[i].n[1]*v.n[VY] + \
 836         a.v[i].n[2]*v.n[VZ] + \
 837         a.v[i].n[3]*v.n[VW]
 838
 839     return vec4(ROWCOL(0), ROWCOL(1), ROWCOL(2), ROWCOL(3));
 840
 841     #undef ROWCOL
 842 }
 843
 844 vec4 operator*(const vec4 &v, const mat4 &a)
 845 {
 846     return a.transpose()*v;
 847 }
 848
 849 float operator*(const vec4 &a, const vec4 &b)
 850 {
 851     return
 852         a.n[VX]*b.n[VX] +
 853         a.n[VY]*b.n[VY] +
 854         a.n[VZ]*b.n[VZ] +
 855         a.n[VW]*b.n[VW];
 856 }
 857
 858 vec4 operator/(const vec4 &a, float d)
 859 {
 860     float d_inv = 1.0f/d;
 861     return vec4(
 862         a.n[VX]*d_inv,
 863         a.n[VY]*d_inv,
 864         a.n[VZ]*d_inv,
 865         a.n[VW]*d_inv);
 866 }
 867
 868 int operator==(const vec4 &a, const vec4 &b)
 869 {
 870     return
 871         (a.n[VX] == b.n[VX]) &&
 872         (a.n[VY] == b.n[VY]) &&
 873         (a.n[VZ] == b.n[VZ]) &&
 874         (a.n[VW] == b.n[VW]);
 875 }
 876
 877 int operator!=(const vec4 &a, const vec4 &b)
 878 {
 879     return !(a == b);
 880 }
 881
 882 /*ostream& operator << (ostream& s, vec4& v)
 883 { return s << "| " << v.n[VX] << ' ' << v.n[VY] << ' ' << v.n[VZ] << ' '
 884   << v.n[VW] << " |"; }
 885
 886 istream& operator >> (istream& s, vec4& v) {
 887     vec4    v_tmp;
 888     char    c = ' ';
 889
 890     while (isspace(c))
 891     s >> c;
 892     // The vectors can be formatted either as x y z w or | x y z w |
 893     if (c == '|') {
 894     s >> v_tmp[VX] >> v_tmp[VY] >> v_tmp[VZ] >> v_tmp[VW];
 895     while (s >> c && isspace(c)) ;
 896     if (c != '|')
 897         ;//s.set(_bad);
 898     }
 899     else {
 900     s.putback(c);
 901     s >> v_tmp[VX] >> v_tmp[VY] >> v_tmp[VZ] >> v_tmp[VW];
 902     }
 903     if (s)
 904     v = v_tmp;
 905     return s;
 906 }
 907 */
 908
 909 void swap(vec4 &a, vec4 &b)
 910 {
 911     vec4 tmp(a);
 912     a = b;
 913     b = tmp;
 914 }
 915
 916 vec4 min_vec(const vec4 &a, const vec4 &b)
 917 {
 918     return vec4(
 919         MIN(a.n[VX], b.n[VX]),
 920         MIN(a.n[VY], b.n[VY]),
 921         MIN(a.n[VZ], b.n[VZ]),
 922         MIN(a.n[VW], b.n[VW]));
 923 }
 924
 925 vec4 max_vec(const vec4 &a, const vec4 &b)
 926 {
 927     return vec4(
 928         MAX(a.n[VX], b.n[VX]),
 929         MAX(a.n[VY], b.n[VY]),
 930         MAX(a.n[VZ], b.n[VZ]),
 931         MAX(a.n[VW], b.n[VW]));
 932 }
 933
 934 vec4 prod(const vec4 &a, const vec4 &b)
 935 {
 936     return vec4(
 937         a.n[VX] * b.n[VX],
 938         a.n[VY] * b.n[VY],
 939         a.n[VZ] * b.n[VZ],
 940         a.n[VW] * b.n[VW]);
 941 }
 942
 943 /****************************************************************
 944  *                                                              *
 945  *          mat3 member functions                               *
 946  *                                                              *
 947  ****************************************************************/
 948
 949 // CONSTRUCTORS
 950
 951 mat3::mat3()
 952 {
 953     *this = identity2D();
 954 }
 955
 956 mat3::mat3(const vec3 &v0, const vec3 &v1, const vec3 &v2)
 957 {
 958     set(v0, v1, v2);
 959 }
 960
 961 mat3::mat3(const mat3 &m)
 962 {
 963     v[0] = m.v[0];
 964     v[1] = m.v[1];
 965     v[2] = m.v[2];
 966 }
 967
 968 // ASSIGNMENT OPERATORS
 969
 970 mat3 &mat3::operator=(const mat3 &m)
 971 {
 972     v[0] = m.v[0];
 973     v[1] = m.v[1];
 974     v[2] = m.v[2];
 975     return *this;
 976 }
 977
 978 mat3 &mat3::operator+=(const mat3& m)
 979 {
 980     v[0] += m.v[0];
 981     v[1] += m.v[1];
 982     v[2] += m.v[2];
 983     return *this;
 984 }
 985
 986 mat3 &mat3::operator-=(const mat3& m)
 987 {
 988     v[0] -= m.v[0];
 989     v[1] -= m.v[1];
 990     v[2] -= m.v[2];
 991     return *this;
 992 }
 993
 994 mat3 &mat3::operator*=(float d)
 995 {
 996     v[0] *= d;
 997     v[1] *= d;
 998     v[2] *= d;
 999     return *this;
1000 }
1001
1002 mat3 &mat3::operator/=(float d)
1003 {
1004     v[0] /= d;
1005     v[1] /= d;
1006     v[2] /= d;
1007     return *this;
1008 }
1009
1010 vec3 &mat3::operator[](int i)
1011 {
1012     if (i < VX || i > VZ)
1013       //VEC_ERROR("mat3 [] operator: illegal access; index = " << i << '\n')
1014       VEC_ERROR("mat3 [] operator: illegal access" );
1015
1016     return v[i];
1017 }
1018
1019 const vec3 &mat3::operator[](int i) const
1020 {
1021     if (i < VX || i > VZ)
1022       //VEC_ERROR("mat3 [] operator: illegal access; index = " << i << '\n')
1023       VEC_ERROR("mat3 [] operator: illegal access" );
1024
1025     return v[i];
1026 }
1027
1028 void mat3::set(const vec3 &v0, const vec3 &v1, const vec3 &v2)
1029 {
1030     v[0] = v0;
1031     v[1] = v1;
1032     v[2] = v2;
1033 }
1034
1035 // SPECIAL FUNCTIONS
1036
1037 mat3 mat3::transpose() const
1038 {
1039     return mat3(
1040         vec3(v[0][0], v[1][0], v[2][0]),
1041         vec3(v[0][1], v[1][1], v[2][1]),
1042         vec3(v[0][2], v[1][2], v[2][2]));
1043 }
1044
1045 mat3 mat3::inverse() const       // Gauss-Jordan elimination with partial pivoting
1046 {
1047     mat3 a(*this);          // As a evolves from original mat into identity
1048     mat3 b(identity2D());   // b evolves from identity into inverse(a)
1049     int  i, j, i1;
1050
1051     // Loop over cols of a from left to right, eliminating above and below diag
1052     for (j=0; j<3; j++)     // Find largest pivot in column j among rows j..2
1053     {
1054         i1 = j;         // Row with largest pivot candidate
1055         for (i=j+1; i<3; i++)
1056             if (fabs(a.v[i].n[j]) > fabs(a.v[i1].n[j]))
1057                 i1 = i;
1058
1059         // Swap rows i1 and j in a and b to put pivot on diagonal
1060         swap(a.v[i1], a.v[j]);
1061         swap(b.v[i1], b.v[j]);
1062
1063         // Scale row j to have a unit diagonal
1064         if (a.v[j].n[j]==0.)
1065             VEC_ERROR("mat3::inverse: singular matrix; can't invert\n");
1066
1067         b.v[j] /= a.v[j].n[j];
1068         a.v[j] /= a.v[j].n[j];
1069
1070         // Eliminate off-diagonal elems in col j of a, doing identical ops to b
1071         for (i=0; i<3; i++)
1072             if (i!=j)
1073             {
1074                 b.v[i] -= a.v[i].n[j]*b.v[j];
1075                 a.v[i] -= a.v[i].n[j]*a.v[j];
1076             }
1077     }
1078
1079     return b;
1080 }
1081
1082 mat3 &mat3::apply(V_FCT_PTR fct)
1083 {
1084     v[VX].apply(fct);
1085     v[VY].apply(fct);
1086     v[VZ].apply(fct);
1087     return *this;
1088 }
1089
1090
1091 // FRIENDS
1092
1093 mat3 operator-(const mat3 &a)
1094 {
1095     return mat3(-a.v[0], -a.v[1], -a.v[2]);
1096 }
1097
1098 mat3 operator+(const mat3 &a, const mat3 &b)
1099 {
1100     return mat3(a.v[0]+b.v[0], a.v[1]+b.v[1], a.v[2]+b.v[2]);
1101 }
1102
1103 mat3 operator-(const mat3 &a, const mat3 &b)
1104 {
1105     return mat3(a.v[0]-b.v[0], a.v[1]-b.v[1], a.v[2]-b.v[2]);
1106 }
1107
1108 mat3 operator*(const mat3 &a, const mat3 &b)
1109 {
1110     #define ROWCOL(i, j) \
1111     a.v[i].n[0]*b.v[0][j] + a.v[i].n[1]*b.v[1][j] + a.v[i].n[2]*b.v[2][j]
1112
1113     return mat3(
1114         vec3(ROWCOL(0,0), ROWCOL(0,1), ROWCOL(0,2)),
1115         vec3(ROWCOL(1,0), ROWCOL(1,1), ROWCOL(1,2)),
1116         vec3(ROWCOL(2,0), ROWCOL(2,1), ROWCOL(2,2)));
1117
1118     #undef ROWCOL
1119 }
1120
1121 mat3 operator*(const mat3 &a, float d)
1122 {
1123     return mat3(a.v[0]*d, a.v[1]*d, a.v[2]*d);
1124 }
1125
1126 mat3 operator*(float d, const mat3 &a)
1127 {
1128     return a*d;
1129 }
1130
1131 mat3 operator/(const mat3 &a, float d)
1132 {
1133     return mat3(a.v[0]/d, a.v[1]/d, a.v[2]/d);
1134 }
1135
1136 int operator==(const mat3 &a, const mat3 &b)
1137 {
1138     return
1139         (a.v[0] == b.v[0]) &&
1140         (a.v[1] == b.v[1]) &&
1141         (a.v[2] == b.v[2]);
1142 }
1143
1144 int operator!=(const mat3 &a, const mat3 &b)
1145 {
1146     return !(a == b);
1147 }
1148
1149 /*ostream& operator << (ostream& s, mat3& m)
1150 { return s << m.v[VX] << '\n' << m.v[VY] << '\n' << m.v[VZ]; }
1151
1152 istream& operator >> (istream& s, mat3& m) {
1153     mat3    m_tmp;
1154
1155     s >> m_tmp[VX] >> m_tmp[VY] >> m_tmp[VZ];
1156     if (s)
1157     m = m_tmp;
1158     return s;
1159 }
1160 */
1161
1162 void swap(mat3 &a, mat3 &b)
1163 {
1164     mat3 tmp(a);
1165     a = b;
1166     b = tmp;
1167 }
1168
1169 void mat3::print(FILE *file, const char *name) const
1170 {
1171     int i, j;
1172
1173     fprintf( stderr, "%s:\n", name );
1174
1175     for( i = 0; i < 3; i++ )
1176     {
1177         fprintf( stderr, "   " );
1178         for( j = 0; j < 3; j++ )
1179         {
1180             fprintf( stderr, "%f  ", v[i][j] );
1181         }
1182         fprintf( stderr, "\n" );
1183     }
1184 }
1185
1186
1187
1188 /****************************************************************
1189  *                                                              *
1190  *          mat4 member functions                               *
1191  *                                                              *
1192  ****************************************************************/
1193
1194 // CONSTRUCTORS
1195
1196 mat4::mat4()
1197 {
1198     *this = identity3D();
1199 }
1200
1201 mat4::mat4(const vec4& v0, const vec4& v1, const vec4& v2, const vec4& v3)
1202 {
1203     v[0] = v0;
1204     v[1] = v1;
1205     v[2] = v2;
1206     v[3] = v3;
1207 }
1208
1209 mat4::mat4(const mat4 &m)
1210 {
1211     v[0] = m.v[0];
1212     v[1] = m.v[1];
1213     v[2] = m.v[2];
1214     v[3] = m.v[3];
1215 }
1216
1217 mat4::mat4(
1218      float a00, float a01, float a02, float a03,
1219      float a10, float a11, float a12, float a13,
1220      float a20, float a21, float a22, float a23,
1221      float a30, float a31, float a32, float a33 )
1222 {
1223   v[0][0] = a00;  v[0][1] = a01;  v[0][2] = a02;  v[0][3] = a03;
1224   v[1][0] = a10;  v[1][1] = a11;  v[1][2] = a12;  v[1][3] = a13;
1225   v[2][0] = a20;  v[2][1] = a21;  v[2][2] = a22;  v[2][3] = a23;
1226   v[3][0] = a30;  v[3][1] = a31;  v[3][2] = a32;  v[3][3] = a33;
1227 }
1228
1229 // ASSIGNMENT OPERATORS
1230
1231 mat4 &mat4::operator=(const mat4 &m)
1232 {
1233     v[0] = m.v[0];
1234     v[1] = m.v[1];
1235     v[2] = m.v[2];
1236     v[3] = m.v[3];
1237     return *this;
1238 }
1239
1240 mat4 &mat4::operator+=(const mat4 &m)
1241 {
1242     v[0] += m.v[0];
1243     v[1] += m.v[1];
1244     v[2] += m.v[2];
1245     v[3] += m.v[3];
1246     return *this;
1247 }
1248
1249 mat4 &mat4::operator-=(const mat4 &m)
1250 {
1251     v[0] -= m.v[0];
1252     v[1] -= m.v[1];
1253     v[2] -= m.v[2];
1254     v[3] -= m.v[3];
1255     return *this;
1256 }
1257
1258 mat4 &mat4::operator*=(float d)
1259 {
1260     v[0] *= d;
1261     v[1] *= d;
1262     v[2] *= d;
1263     v[3] *= d;
1264     return *this;
1265 }
1266
1267 mat4 &mat4::operator/=(float d)
1268 {
1269     v[0] /= d;
1270     v[1] /= d;
1271     v[2] /= d;
1272     v[3] /= d;
1273     return *this;
1274 }
1275
1276 vec4 &mat4::operator[](int i)
1277 {
1278     if (i < VX || i > VW)
1279         //VEC_ERROR("mat4 [] operator: illegal access; index = " << i << '\n')
1280         VEC_ERROR("mat4 [] operator: illegal access" );
1281     return v[i];
1282 }
1283
1284 const vec4 &mat4::operator[](int i) const
1285 {
1286     if (i < VX || i > VW)
1287         //VEC_ERROR("mat4 [] operator: illegal access; index = " << i << '\n')
1288         VEC_ERROR("mat4 [] operator: illegal access" );
1289     return v[i];
1290 }
1291
1292 // SPECIAL FUNCTIONS;
1293
1294 mat4 mat4::transpose() const
1295 {
1296     return mat4(
1297         vec4(v[0][0], v[1][0], v[2][0], v[3][0]),
1298         vec4(v[0][1], v[1][1], v[2][1], v[3][1]),
1299         vec4(v[0][2], v[1][2], v[2][2], v[3][2]),
1300         vec4(v[0][3], v[1][3], v[2][3], v[3][3]));
1301 }
1302
1303 mat4 mat4::inverse() const       // Gauss-Jordan elimination with partial pivoting
1304 {
1305     mat4 a(*this);          // As a evolves from original mat into identity
1306     mat4 b(identity3D());   // b evolves from identity into inverse(a)
1307     int i, j, i1;
1308
1309     // Loop over cols of a from left to right, eliminating above and below diag
1310     for (j=0; j<4; j++)    // Find largest pivot in column j among rows j..3
1311     {
1312         i1 = j;         // Row with largest pivot candidate
1313         for (i=j+1; i<4; i++)
1314             if (fabs(a.v[i].n[j]) > fabs(a.v[i1].n[j]))
1315                 i1 = i;
1316
1317         // Swap rows i1 and j in a and b to put pivot on diagonal
1318         swap(a.v[i1], a.v[j]);
1319         swap(b.v[i1], b.v[j]);
1320
1321         // Scale row j to have a unit diagonal
1322         if (a.v[j].n[j]==0.)
1323             VEC_ERROR("mat4::inverse: singular matrix; can't invert\n");
1324
1325         b.v[j] /= a.v[j].n[j];
1326         a.v[j] /= a.v[j].n[j];
1327
1328         // Eliminate off-diagonal elems in col j of a, doing identical ops to b
1329         for (i=0; i<4; i++)
1330             if (i!=j)
1331             {
1332                 b.v[i] -= a.v[i].n[j]*b.v[j];
1333                 a.v[i] -= a.v[i].n[j]*a.v[j];
1334             }
1335     }
1336
1337     return b;
1338 }
1339
1340 mat4 &mat4::apply(V_FCT_PTR fct)
1341 {
1342     v[VX].apply(fct);
1343     v[VY].apply(fct);
1344     v[VZ].apply(fct);
1345     v[VW].apply(fct);
1346     return *this;
1347 }
1348
1349 void mat4::print(FILE *file, const char *name) const
1350 {
1351     int i, j;
1352
1353     fprintf( stderr, "%s:\n", name );
1354
1355     for( i = 0; i < 4; i++ )
1356     {
1357         fprintf( stderr, "   " );
1358         for( j = 0; j < 4; j++ )
1359         {
1360             fprintf( stderr, "%f  ", v[i][j] );
1361         }
1362         fprintf( stderr, "\n" );
1363     }
1364 }
1365
1366 void mat4::swap_rows(int i, int j)
1367 {
1368     vec4 t;
1369
1370     t    = v[i];
1371     v[i] = v[j];
1372     v[j] = t;
1373 }
1374
1375 void mat4::swap_cols(int i, int j)
1376 {
1377     float t;
1378     int k;
1379
1380     for (k=0; k<4; k++)
1381     {
1382         t       = v[k][i];
1383         v[k][i] = v[k][j];
1384         v[k][j] = t;
1385     }
1386 }
1387
1388
1389 // FRIENDS
1390
1391 mat4 operator-(const mat4 &a)
1392 {
1393     return mat4(-a.v[0],-a.v[1],-a.v[2],-a.v[3]);
1394 }
1395
1396 mat4 operator+(const mat4 &a, const mat4 &b)
1397 {
1398     return mat4(
1399         a.v[0] + b.v[0],
1400         a.v[1] + b.v[1],
1401         a.v[2] + b.v[2],
1402         a.v[3] + b.v[3]);
1403 }
1404
1405 mat4 operator-(const mat4 &a, const mat4 &b)
1406 {
1407     return mat4(
1408         a.v[0] - b.v[0],
1409         a.v[1] - b.v[1],
1410         a.v[2] - b.v[2],
1411         a.v[3] - b.v[3]);
1412 }
1413
1414 mat4 operator*(const mat4 &a, const mat4 &b)
1415 {
1416     #define ROWCOL(i, j) \
1417         a.v[i].n[0]*b.v[0][j] + \
1418         a.v[i].n[1]*b.v[1][j] + \
1419         a.v[i].n[2]*b.v[2][j] + \
1420         a.v[i].n[3]*b.v[3][j]
1421
1422     return mat4(
1423         vec4(ROWCOL(0,0), ROWCOL(0,1), ROWCOL(0,2), ROWCOL(0,3)),
1424         vec4(ROWCOL(1,0), ROWCOL(1,1), ROWCOL(1,2), ROWCOL(1,3)),
1425         vec4(ROWCOL(2,0), ROWCOL(2,1), ROWCOL(2,2), ROWCOL(2,3)),
1426         vec4(ROWCOL(3,0), ROWCOL(3,1), ROWCOL(3,2), ROWCOL(3,3))
1427         );
1428
1429     #undef ROWCOL
1430 }
1431
1432 mat4 operator*(const mat4 &a, float d)
1433 {
1434     return mat4(a.v[0]*d, a.v[1]*d, a.v[2]*d, a.v[3]*d);
1435 }
1436
1437 mat4 operator*(float d, const mat4 &a)
1438 {
1439     return a*d;
1440 }
1441
1442 mat4 operator/(const mat4 &a, float d)
1443 {
1444     return mat4(a.v[0]/d, a.v[1]/d, a.v[2]/d, a.v[3]/d);
1445 }
1446
1447 int operator==(const mat4 &a, const mat4 &b)
1448 {
1449     return
1450         (a.v[0] == b.v[0]) &&
1451         (a.v[1] == b.v[1]) &&
1452         (a.v[2] == b.v[2]) &&
1453         (a.v[3] == b.v[3]);
1454 }
1455
1456 int operator!=(const mat4 &a, const mat4 &b)
1457 {
1458     return !(a == b);
1459 }
1460
1461 /*ostream& operator << (ostream& s, mat4& m)
1462 { return s << m.v[VX] << '\n' << m.v[VY] << '\n' << m.v[VZ] << '\n' << m.v[VW]; }
1463
1464 istream& operator >> (istream& s, mat4& m)
1465 {
1466     mat4    m_tmp;
1467
1468     s >> m_tmp[VX] >> m_tmp[VY] >> m_tmp[VZ] >> m_tmp[VW];
1469     if (s)
1470     m = m_tmp;
1471     return s;
1472 }
1473 */
1474
1475 void swap(mat4 &a, mat4 &b)
1476 {
1477     mat4 tmp(a);
1478     a = b;
1479     b = tmp;
1480 }
1481
1482 /****************************************************************
1483  *                                                              *
1484  *         2D functions and 3D functions                        *
1485  *                                                              *
1486  ****************************************************************/
1487
1488 mat3 identity2D()
1489 {
1490     return mat3(
1491         vec3(1.0, 0.0, 0.0),
1492         vec3(0.0, 1.0, 0.0),
1493         vec3(0.0, 0.0, 1.0));
1494 }
1495
1496 mat3 translation2D(const vec2 &v)
1497 {
1498     return mat3(
1499         vec3(1.0, 0.0, v[VX]),
1500         vec3(0.0, 1.0, v[VY]),
1501         vec3(0.0, 0.0, 1.0));
1502 }
1503
1504 mat3 rotation2D(const vec2 &Center, float angleDeg)
1505 {
1506     float angleRad = (float) (angleDeg * M_PI / 180.0);
1507     float c = (float) cos(angleRad);
1508     float s = (float) sin(angleRad);
1509
1510     return mat3(
1511         vec3(c,    -s, Center[VX] * (1.0f-c) + Center[VY] * s),
1512         vec3(s,     c, Center[VY] * (1.0f-c) - Center[VX] * s),
1513         vec3(0.0, 0.0, 1.0));
1514 }
1515
1516 mat3 scaling2D(const vec2 &scaleVector)
1517 {
1518     return mat3(
1519         vec3(scaleVector[VX], 0.0, 0.0),
1520         vec3(0.0, scaleVector[VY], 0.0),
1521         vec3(0.0, 0.0, 1.0));
1522 }
1523
1524 mat4 identity3D()
1525 {
1526     return mat4(
1527         vec4(1.0, 0.0, 0.0, 0.0),
1528         vec4(0.0, 1.0, 0.0, 0.0),
1529         vec4(0.0, 0.0, 1.0, 0.0),
1530         vec4(0.0, 0.0, 0.0, 1.0));
1531 }
1532
1533 mat4 translation3D(const vec3 &v)
1534 {
1535     return mat4(
1536         vec4(1.0, 0.0, 0.0, v[VX]),
1537         vec4(0.0, 1.0, 0.0, v[VY]),
1538         vec4(0.0, 0.0, 1.0, v[VZ]),
1539         vec4(0.0, 0.0, 0.0, 1.0));
1540 }
1541
1542 mat4 rotation3D(const vec3 &Axis, float angleDeg)
1543 {
1544     float angleRad = (float) (angleDeg * M_PI / 180.0);
1545     float c = (float) cos(angleRad);
1546     float s = (float) sin(angleRad);
1547     float t = 1.0f - c;
1548
1549     vec3 axis(Axis);
1550     axis.normalize();
1551
1552     return mat4(
1553         vec4(t * axis[VX] * axis[VX] + c,
1554              t * axis[VX] * axis[VY] - s * axis[VZ],
1555              t * axis[VX] * axis[VZ] + s * axis[VY],
1556              0.0),
1557         vec4(t * axis[VX] * axis[VY] + s * axis[VZ],
1558              t * axis[VY] * axis[VY] + c,
1559              t * axis[VY] * axis[VZ] - s * axis[VX],
1560              0.0),
1561         vec4(t * axis[VX] * axis[VZ] - s * axis[VY],
1562              t * axis[VY] * axis[VZ] + s * axis[VX],
1563              t * axis[VZ] * axis[VZ] + c,
1564              0.0),
1565         vec4(0.0, 0.0, 0.0, 1.0));
1566 }
1567
1568 mat4 rotation3Drad(const vec3 &Axis, float angleRad)
1569 {
1570     float c = (float) cos(angleRad);
1571     float s = (float) sin(angleRad);
1572     float t = 1.0f - c;
1573
1574     vec3 axis(Axis);
1575     axis.normalize();
1576
1577     return mat4(
1578         vec4(t * axis[VX] * axis[VX] + c,
1579              t * axis[VX] * axis[VY] - s * axis[VZ],
1580              t * axis[VX] * axis[VZ] + s * axis[VY],
1581              0.0),
1582         vec4(t * axis[VX] * axis[VY] + s * axis[VZ],
1583              t * axis[VY] * axis[VY] + c,
1584              t * axis[VY] * axis[VZ] - s * axis[VX],
1585              0.0),
1586         vec4(t * axis[VX] * axis[VZ] - s * axis[VY],
1587              t * axis[VY] * axis[VZ] + s * axis[VX],
1588              t * axis[VZ] * axis[VZ] + c,
1589              0.0),
1590         vec4(0.0, 0.0, 0.0, 1.0));
1591 }
1592
1593 mat4 scaling3D(const vec3 &scaleVector)
1594 {
1595     return mat4(
1596         vec4(scaleVector[VX], 0.0, 0.0, 0.0),
1597         vec4(0.0, scaleVector[VY], 0.0, 0.0),
1598         vec4(0.0, 0.0, scaleVector[VZ], 0.0),
1599         vec4(0.0, 0.0, 0.0, 1.0));
1600 }
1601
1602 mat4 perspective3D(float d)
1603 {
1604     return mat4(
1605         vec4(1.0f, 0.0f, 0.0f,   0.0f),
1606         vec4(0.0f, 1.0f, 0.0f,   0.0f),
1607         vec4(0.0f, 0.0f, 1.0f,   0.0f),
1608         vec4(0.0f, 0.0f, 1.0f/d, 0.0f));
1609 }