"Fossies" - the Fresh Open Source Software Archive

Member "ivtools-ivtools-2.0.4/src/OverlayUnidraw/algebra3.h" (9 Oct 2020, 40492 Bytes) of package /linux/misc/ivtools-ivtools-2.0.4.tar.gz:


As a special service "Fossies" has tried to format the requested source page into HTML format using (guessed) C and C++ source code syntax highlighting (style: standard) with prefixed line numbers and code folding option. Alternatively you can here view or download the uninterpreted source code file. For more information about "algebra3.h" see the Fossies "Dox" file reference documentation.

    1 /****************************************************************
    2 *                                                               *
    3 * C++ Vector and Matrix Algebra routines                        *
    4 * Author: Jean-Francois DOUE                                    *
    5 * Version 3.1 --- October 1993                                  *
    6 *                                                               *
    7 ****************************************************************/
    8 //
    9 //  From "Graphics Gems IV / Edited by Paul S. Heckbert
   10 //  Academic Press, 1994, ISBN 0-12-336156-9
   11 //  "You are free to use and modify this code in any way 
   12 //  you like." (p. xv)
   13 //
   14 //  Modified by J. Nagle, March 1997
   15 //  -   All functions are inline.
   16 //  -   All functions are const-correct.
   17 //  -   All checking is via the standard "assert" macro.
   18 //  -   Stream I/O is disabled for portability, but can be
   19 //      re-enabled by defining ALGEBRA3IOSTREAMS.
   20 //  
   21 //      Modified by J. Gautier April 1997 to compile with GCC in a
   22 //        Linux/InterViews environment, also removed warnings by adding
   23 //        inline declarations to some member functions.
   24 //
   25 #ifndef ALGEBRA3H
   26 #define ALGEBRA3H
   27 
   28 #include <stdlib.h>
   29 #include <assert.h>
   30 //#include <yvals.h>
   31 #include <math.h>
   32 #include <ctype.h>
   33 
   34 // this line defines a new type: pointer to a function which returns a
   35 // double and takes as argument a double
   36 typedef double (*V_FCT_PTR)(double);
   37 
   38 // min-max macros
   39 #define MIN(A,B) ((A) < (B) ? (A) : (B))
   40 #define MAX(A,B) ((A) > (B) ? (A) : (B))
   41 
   42 // InterViews already defines these, so change the function names
   43 #if 0
   44 #undef min                  // allow as function names
   45 #undef max
   46 #endif
   47 
   48 // error handling macro
   49 #define ALGEBRA_ERROR(E) { assert(false); }
   50 
   51 class vec2;
   52 class vec3;
   53 class vec4;
   54 class mat3;
   55 class mat4;
   56 
   57 enum {VX, VY, VZ, VW};          // axes
   58 enum {PA, PB, PC, PD};          // planes
   59 enum {RED, GREEN, BLUE};        // colors
   60 enum {KA, KD, KS, ES};          // phong coefficients
   61 
   62 //
   63 //  PI
   64 //
   65 #ifndef M_PI
   66 const double M_PI = 3.14159265358979323846;     // per CRC handbook, 14th. ed.
   67 #endif
   68 #ifndef M_PI_2
   69 const double M_PI_2 = (M_PI/2.0);               // PI/2
   70 #endif
   71 #ifndef M2_PI
   72 const double M2_PI = (M_PI*2.0);                // PI*2
   73 #endif
   74 
   75 /****************************************************************
   76 *                                                               *
   77 *               2D Vector                                       *
   78 *                                                               *
   79 ****************************************************************/
   80 
   81 class vec2
   82 {
   83 protected:
   84 
   85  double n[2];
   86 
   87 public:
   88 
   89 // Constructors
   90 
   91 vec2();
   92 vec2(const double x, const double y);
   93 vec2(const double d);
   94 vec2(const vec2& v);                // copy constructor
   95 vec2(const vec3& v);                // cast v3 to v2
   96 vec2(const vec3& v, int dropAxis);  // cast v3 to v2
   97 
   98 // Assignment operators
   99 
  100 vec2& operator  = ( const vec2& v );    // assignment of a vec2
  101 vec2& operator += ( const vec2& v );    // incrementation by a vec2
  102 vec2& operator -= ( const vec2& v );    // decrementation by a vec2
  103 vec2& operator *= ( const double d );   // multiplication by a constant
  104 vec2& operator /= ( const double d );   // division by a constant
  105 double& operator [] ( int i);           // indexing
  106 double vec2::operator [] ( int i) const;// read-only indexing
  107 
  108 // special functions
  109 
  110 double length() const;          // length of a vec2
  111 inline double length2() const;          // squared length of a vec2
  112 vec2& normalize() ;             // normalize a vec2 in place
  113 vec2& apply(V_FCT_PTR fct);     // apply a func. to each component
  114 
  115 // friends
  116 
  117 friend vec2 operator - (const vec2& v);                     // -v1
  118 friend vec2 operator + (const vec2& a, const vec2& b);      // v1 + v2
  119 friend vec2 operator - (const vec2& a, const vec2& b);      // v1 - v2
  120 friend vec2 operator * (const vec2& a, const double d);     // v1 * 3.0
  121 friend vec2 operator * (const double d, const vec2& a);     // 3.0 * v1
  122 friend vec2 operator * (const mat3& a, const vec2& v);      // M . v
  123 friend vec2 operator * (const vec2& v, const mat3& a);      // v . M
  124 friend double operator * (const vec2& a, const vec2& b);    // dot product
  125 friend vec2 operator / (const vec2& a, const double d);     // v1 / 3.0
  126 friend vec3 operator ^ (const vec2& a, const vec2& b);      // cross product
  127 friend int operator == (const vec2& a, const vec2& b);      // v1 == v2 ?
  128 friend int operator != (const vec2& a, const vec2& b);      // v1 != v2 ?
  129 
  130 #ifdef ALGEBRA3IOSTREAMS
  131 friend ostream& operator << (ostream& s, const vec2& v);    // output to stream
  132 friend istream& operator >> (istream& s, vec2& v);      // input from strm.
  133 #endif ALGEBRA3IOSTREAMS
  134 
  135 friend void swap(vec2& a, vec2& b);                     // swap v1 & v2
  136 friend vec2 vec2min(const vec2& a, const vec2& b);          // min(v1, v2)
  137 friend vec2 vec2max(const vec2& a, const vec2& b);          // max(v1, v2)
  138 friend vec2 prod(const vec2& a, const vec2& b);         // term by term *
  139 
  140 // necessary friend declarations
  141 
  142 friend class vec3;
  143 };
  144 
  145 /****************************************************************
  146 *                                                               *
  147 *               3D Vector                                       *
  148 *                                                               *
  149 ****************************************************************/
  150 
  151 class vec3
  152 {
  153 protected:
  154 
  155  double n[3];
  156 
  157 public:
  158 
  159 // Constructors
  160 
  161 inline vec3();
  162 inline vec3(const double x, const double y, const double z);
  163 vec3(const double d);
  164 vec3(const vec3& v);                    // copy constructor
  165 vec3(const vec2& v);                    // cast v2 to v3
  166 vec3(const vec2& v, double d);          // cast v2 to v3
  167 vec3(const vec4& v);                    // cast v4 to v3
  168 vec3(const vec4& v, int dropAxis);      // cast v4 to v3
  169 
  170 // Assignment operators
  171 
  172 vec3& operator  = ( const vec3& v );        // assignment of a vec3
  173 vec3& operator += ( const vec3& v );        // incrementation by a vec3
  174 vec3& operator -= ( const vec3& v );        // decrementation by a vec3
  175 vec3& operator *= ( const double d );       // multiplication by a constant
  176 vec3& operator /= ( const double d );       // division by a constant
  177 double& operator [] ( int i);               // indexing
  178 double operator[] (int i) const;            // read-only indexing
  179 
  180 // special functions
  181 
  182 double length() const;              // length of a vec3
  183 inline double length2() const;              // squared length of a vec3
  184 vec3& normalize();                  // normalize a vec3 in place
  185 vec3& apply(V_FCT_PTR fct);         // apply a func. to each component
  186 
  187 // friends
  188 
  189 friend vec3 operator - (const vec3& v);                     // -v1
  190 friend vec3 operator + (const vec3& a, const vec3& b);      // v1 + v2
  191 friend vec3 operator - (const vec3& a, const vec3& b);      // v1 - v2
  192 friend vec3 operator * (const vec3& a, const double d);     // v1 * 3.0
  193 friend vec3 operator * (const double d, const vec3& a);     // 3.0 * v1
  194 friend vec3 operator * (const mat4& a, const vec3& v);      // M . v
  195 friend vec3 operator * (const vec3& v, const mat4& a);      // v . M
  196 friend double operator * (const vec3& a, const vec3& b);    // dot product
  197 friend vec3 operator / (const vec3& a, const double d);     // v1 / 3.0
  198 friend vec3 operator ^ (const vec3& a, const vec3& b);      // cross product
  199 friend int operator == (const vec3& a, const vec3& b);      // v1 == v2 ?
  200 friend int operator != (const vec3& a, const vec3& b);      // v1 != v2 ?
  201 
  202 #ifdef ALGEBRA3IOSTREAMS
  203 friend ostream& operator << (ostream& s, const vec3& v);       // output to stream
  204 friend istream& operator >> (istream& s, vec3& v);      // input from strm.
  205 #endif // ALGEBRA3IOSTREAMS
  206 
  207 friend void swap(vec3& a, vec3& b);                     // swap v1 & v2
  208 friend vec3 vec3min(const vec3& a, const vec3& b);          // min(v1, v2)
  209 friend vec3 vec3max(const vec3& a, const vec3& b);          // max(v1, v2)
  210 friend vec3 prod(const vec3& a, const vec3& b);         // term by term *
  211 
  212 // necessary friend declarations
  213 
  214 friend class vec2;
  215 friend class vec4;
  216 friend class mat3;
  217 friend vec2 operator * (const mat3& a, const vec2& v);  // linear transform
  218 friend mat3 operator * (const mat3& a, const mat3& b);  // matrix 3 product
  219 };
  220 
  221 /****************************************************************
  222 *                                                               *
  223 *               4D Vector                                       *
  224 *                                                               *
  225 ****************************************************************/
  226 
  227 class vec4
  228 {
  229 protected:
  230 
  231  double n[4];
  232 
  233 public:
  234 
  235 // Constructors
  236 
  237 vec4();
  238 vec4(const double x, const double y, const double z, const double w);
  239 vec4(const double d);
  240 vec4(const vec4& v);                // copy constructor
  241 inline vec4(const vec3& v);             // cast vec3 to vec4
  242 vec4(const vec3& v, const double d);        // cast vec3 to vec4
  243 
  244 // Assignment operators
  245 
  246 vec4& operator  = ( const vec4& v );        // assignment of a vec4
  247 vec4& operator += ( const vec4& v );        // incrementation by a vec4
  248 vec4& operator -= ( const vec4& v );        // decrementation by a vec4
  249 vec4& operator *= ( const double d );       // multiplication by a constant
  250 vec4& operator /= ( const double d );       // division by a constant
  251 double& operator [] ( int i);               // indexing
  252 double operator[] (int i) const;            // read-only indexing
  253 
  254 // special functions
  255 
  256 double length() const;          // length of a vec4
  257 inline double length2() const;          // squared length of a vec4
  258 vec4& normalize();              // normalize a vec4 in place
  259 vec4& apply(V_FCT_PTR fct);     // apply a func. to each component
  260 
  261 // friends
  262 
  263 friend vec4 operator - (const vec4& v);                     // -v1
  264 friend vec4 operator + (const vec4& a, const vec4& b);      // v1 + v2
  265 friend vec4 operator - (const vec4& a, const vec4& b);      // v1 - v2
  266 friend vec4 operator * (const vec4& a, const double d);     // v1 * 3.0
  267 friend vec4 operator * (const double d, const vec4& a);     // 3.0 * v1
  268 friend vec4 operator * (const mat4& a, const vec4& v);      // M . v
  269 friend vec4 operator * (const vec4& v, const mat4& a);      // v . M
  270 friend double operator * (const vec4& a, const vec4& b);    // dot product
  271 friend vec4 operator / (const vec4& a, const double d);     // v1 / 3.0
  272 friend int operator == (const vec4& a, const vec4& b);      // v1 == v2 ?
  273 friend int operator != (const vec4& a, const vec4& b);      // v1 != v2 ?
  274 
  275 #ifdef ALGEBRA3IOSTREAMS
  276 friend ostream& operator << (ostream& s, const vec4& v);    // output to stream
  277 friend istream& operator >> (istream& s, vec4& v);      // input from strm.
  278 #endif //  ALGEBRA3IOSTREAMS
  279 
  280 friend void swap(vec4& a, vec4& b);                     // swap v1 & v2
  281 friend vec4 vec4min(const vec4& a, const vec4& b);          // min(v1, v2)
  282 friend vec4 vec4max(const vec4& a, const vec4& b);          // max(v1, v2)
  283 friend vec4 prod(const vec4& a, const vec4& b);         // term by term *
  284 
  285 // necessary friend declarations
  286 
  287 friend class vec3;
  288 friend class mat4;
  289 friend vec3 operator * (const mat4& a, const vec3& v);  // linear transform
  290 friend mat4 operator * (const mat4& a, const mat4& b);  // matrix 4 product
  291 };
  292 
  293 /****************************************************************
  294 *                                                               *
  295 *              3x3 Matrix                                       *
  296 *                                                               *
  297 ****************************************************************/
  298 
  299 class mat3
  300 {
  301 protected:
  302 
  303  vec3 v[3];
  304 
  305 public:
  306 
  307 // Constructors
  308 
  309 mat3();
  310 mat3(const vec3& v0, const vec3& v1, const vec3& v2);
  311 mat3(const double d);
  312 mat3(const mat3& m);
  313 
  314 // Assignment operators
  315 
  316 mat3& operator  = ( const mat3& m );        // assignment of a mat3
  317 mat3& operator += ( const mat3& m );        // incrementation by a mat3
  318 mat3& operator -= ( const mat3& m );        // decrementation by a mat3
  319 mat3& operator *= ( const double d );       // multiplication by a constant
  320 mat3& operator /= ( const double d );       // division by a constant
  321 vec3& operator [] ( int i);                 // indexing
  322 const vec3& operator [] ( int i) const;     // read-only indexing
  323 
  324 // special functions
  325 
  326 inline mat3 transpose() const;              // transpose
  327 mat3 inverse() const;               // inverse
  328 mat3& apply(V_FCT_PTR fct);         // apply a func. to each element
  329 
  330 // friends
  331 
  332 friend mat3 operator - (const mat3& a);                     // -m1
  333 friend mat3 operator + (const mat3& a, const mat3& b);      // m1 + m2
  334 friend mat3 operator - (const mat3& a, const mat3& b);      // m1 - m2
  335 friend mat3 operator * (const mat3& a, const mat3& b);      // m1 * m2
  336 friend mat3 operator * (const mat3& a, const double d);     // m1 * 3.0
  337 friend mat3 operator * (const double d, const mat3& a);     // 3.0 * m1
  338 friend mat3 operator / (const mat3& a, const double d);     // m1 / 3.0
  339 friend int operator == (const mat3& a, const mat3& b);      // m1 == m2 ?
  340 friend int operator != (const mat3& a, const mat3& b);      // m1 != m2 ?
  341 
  342 #ifdef ALGEBRA3IOSTREAMS
  343 friend ostream& operator << (ostream& s, const mat3& m);    // output to stream
  344 friend istream& operator >> (istream& s, mat3& m);      // input from strm.
  345 #endif //  ALGEBRA3IOSTREAMS
  346 
  347 friend void swap(mat3& a, mat3& b);             // swap m1 & m2
  348 
  349 // necessary friend declarations
  350 
  351 friend vec3 operator * (const mat3& a, const vec3& v);      // linear transform
  352 friend vec2 operator * (const mat3& a, const vec2& v);      // linear transform
  353 };
  354 
  355 /****************************************************************
  356 *                                                               *
  357 *              4x4 Matrix                                       *
  358 *                                                               *
  359 ****************************************************************/
  360 
  361 class mat4
  362 {
  363 protected:
  364 
  365  vec4 v[4];
  366 
  367 public:
  368 
  369 // Constructors
  370 
  371 mat4();
  372 mat4(const vec4& v0, const vec4& v1, const vec4& v2, const vec4& v3);
  373 mat4(const double d);
  374 mat4(const mat4& m);
  375 
  376 // Assignment operators
  377 
  378 mat4& operator  = ( const mat4& m );        // assignment of a mat4
  379 mat4& operator += ( const mat4& m );        // incrementation by a mat4
  380 mat4& operator -= ( const mat4& m );        // decrementation by a mat4
  381 mat4& operator *= ( const double d );       // multiplication by a constant
  382 mat4& operator /= ( const double d );       // division by a constant
  383 vec4& operator [] ( int i);                 // indexing
  384 const vec4& operator [] ( int i) const;     // read-only indexing
  385 
  386 // special functions
  387 
  388 inline mat4 transpose() const;                      // transpose
  389 mat4 inverse() const;                       // inverse
  390 mat4& apply(V_FCT_PTR fct);                 // apply a func. to each element
  391 
  392 // friends
  393 
  394 friend mat4 operator - (const mat4& a);                     // -m1
  395 friend mat4 operator + (const mat4& a, const mat4& b);      // m1 + m2
  396 friend mat4 operator - (const mat4& a, const mat4& b);      // m1 - m2
  397 friend mat4 operator * (const mat4& a, const mat4& b);      // m1 * m2
  398 friend mat4 operator * (const mat4& a, const double d);     // m1 * 4.0
  399 friend mat4 operator * (const double d, const mat4& a);     // 4.0 * m1
  400 friend mat4 operator / (const mat4& a, const double d);     // m1 / 3.0
  401 friend int operator == (const mat4& a, const mat4& b);      // m1 == m2 ?
  402 friend int operator != (const mat4& a, const mat4& b);      // m1 != m2 ?
  403 
  404 #ifdef ALGEBRA3IOSTREAMS
  405 friend ostream& operator << (ostream& s, const mat4& m);    // output to stream
  406 friend istream& operator >> (istream& s, mat4& m);          // input from strm.
  407 #endif //  ALGEBRA3IOSTREAMS
  408 
  409 friend void swap(mat4& a, mat4& b);                         // swap m1 & m2
  410 
  411 // necessary friend declarations
  412 
  413 inline friend vec4 operator * (const mat4& a, const vec4& v);       // linear transform
  414 friend vec3 operator * (const mat4& a, const vec3& v);      // linear transform
  415 };
  416 
  417 /****************************************************************
  418 *                                                               *
  419 *          2D functions and 3D functions                        *
  420 *                                                               *
  421 ****************************************************************/
  422 
  423 inline mat3 identity2D();                               // identity 2D
  424 mat3 translation2D(const vec2& v);              // translation 2D
  425 mat3 rotation2D(const vec2& Center, const double angleDeg); // rotation 2D
  426 mat3 scaling2D(const vec2& scaleVector);        // scaling 2D
  427 inline mat4 identity3D();                               // identity 3D
  428 mat4 translation3D(const vec3& v);              // translation 3D
  429 mat4 rotation3D(vec3 Axis, const double angleDeg);// rotation 3D
  430 mat4 scaling3D(const vec3& scaleVector);        // scaling 3D
  431 mat4 perspective3D(const double d);             // perspective 3D
  432 
  433 //
  434 //  Implementation
  435 //
  436 
  437 /****************************************************************
  438 *                                                               *
  439 *           vec2 Member functions                               *
  440 *                                                               *
  441 ****************************************************************/
  442 
  443 // CONSTRUCTORS
  444 
  445 inline vec2::vec2() {}
  446 
  447 inline vec2::vec2(const double x, const double y)
  448 { n[VX] = x; n[VY] = y; }
  449 
  450 inline vec2::vec2(const double d)
  451 { n[VX] = n[VY] = d; }
  452 
  453 inline vec2::vec2(const vec2& v)
  454 { n[VX] = v.n[VX]; n[VY] = v.n[VY]; }
  455 
  456 inline vec2::vec2(const vec3& v) // it is up to caller to avoid divide-by-zero
  457 { n[VX] = v.n[VX]/v.n[VZ]; n[VY] = v.n[VY]/v.n[VZ]; };
  458 
  459 inline vec2::vec2(const vec3& v, int dropAxis) {
  460     switch (dropAxis) {
  461     case VX: n[VX] = v.n[VY]; n[VY] = v.n[VZ]; break;
  462     case VY: n[VX] = v.n[VX]; n[VY] = v.n[VZ]; break;
  463     default: n[VX] = v.n[VX]; n[VY] = v.n[VY]; break;
  464     }
  465 }
  466 
  467 
  468 // ASSIGNMENT OPERATORS
  469 
  470 inline vec2& vec2::operator = (const vec2& v)
  471 { n[VX] = v.n[VX]; n[VY] = v.n[VY]; return *this; }
  472 
  473 inline vec2& vec2::operator += ( const vec2& v )
  474 { n[VX] += v.n[VX]; n[VY] += v.n[VY]; return *this; }
  475 
  476 inline vec2& vec2::operator -= ( const vec2& v )
  477 { n[VX] -= v.n[VX]; n[VY] -= v.n[VY]; return *this; }
  478 
  479 inline vec2& vec2::operator *= ( const double d )
  480 { n[VX] *= d; n[VY] *= d; return *this; }
  481 
  482 inline vec2& vec2::operator /= ( const double d )
  483 { double d_inv = 1./d; n[VX] *= d_inv; n[VY] *= d_inv; return *this; }
  484 
  485 inline double& vec2::operator [] ( int i) {
  486     assert(!(i < VX || i > VY));        // subscript check
  487     return n[i];
  488 }
  489 
  490 inline double vec2::operator [] ( int i) const {
  491     assert(!(i < VX || i > VY));
  492     return n[i];
  493 }
  494 
  495 
  496 // SPECIAL FUNCTIONS
  497 
  498 inline double vec2::length() const
  499 { return sqrt(length2()); }
  500 
  501 inline double vec2::length2() const
  502 { return n[VX]*n[VX] + n[VY]*n[VY]; }
  503 
  504 inline vec2& vec2::normalize() // it is up to caller to avoid divide-by-zero
  505 { *this /= length(); return *this; }
  506 
  507 inline vec2& vec2::apply(V_FCT_PTR fct)
  508 { n[VX] = (*fct)(n[VX]); n[VY] = (*fct)(n[VY]); return *this; }
  509 
  510 
  511 // FRIENDS
  512 
  513 inline vec2 operator - (const vec2& a)
  514 { return vec2(-a.n[VX],-a.n[VY]); }
  515 
  516 inline vec2 operator + (const vec2& a, const vec2& b)
  517 { return vec2(a.n[VX]+ b.n[VX], a.n[VY] + b.n[VY]); }
  518 
  519 inline vec2 operator - (const vec2& a, const vec2& b)
  520 { return vec2(a.n[VX]-b.n[VX], a.n[VY]-b.n[VY]); }
  521 
  522 inline vec2 operator * (const vec2& a, const double d)
  523 { return vec2(d*a.n[VX], d*a.n[VY]); }
  524 
  525 inline vec2 operator * (const double d, const vec2& a)
  526 { return a*d; }
  527 
  528 inline vec2 operator * (const mat3& a, const vec2& v) {
  529     vec3 av;
  530 
  531     av.n[VX] = a.v[0].n[VX]*v.n[VX] + a.v[0].n[VY]*v.n[VY] + a.v[0].n[VZ];
  532     av.n[VY] = a.v[1].n[VX]*v.n[VX] + a.v[1].n[VY]*v.n[VY] + a.v[1].n[VZ];
  533     av.n[VZ] = a.v[2].n[VX]*v.n[VX] + a.v[2].n[VY]*v.n[VY] + a.v[2].n[VZ];
  534     return av;
  535 }
  536 
  537 inline vec2 operator * (const vec2& v, const mat3& a) 
  538 { return a.transpose() * v; }
  539 
  540 inline double operator * (const vec2& a, const vec2& b)
  541 { return (a.n[VX]*b.n[VX] + a.n[VY]*b.n[VY]); }
  542 
  543 inline vec2 operator / (const vec2& a, const double d)
  544 { double d_inv = 1./d; return vec2(a.n[VX]*d_inv, a.n[VY]*d_inv); }
  545 
  546 inline vec3 operator ^ (const vec2& a, const vec2& b)
  547 { return vec3(0.0, 0.0, a.n[VX] * b.n[VY] - b.n[VX] * a.n[VY]); }
  548 
  549 inline int operator == (const vec2& a, const vec2& b)
  550 { return (a.n[VX] == b.n[VX]) && (a.n[VY] == b.n[VY]); }
  551 
  552 inline int operator != (const vec2& a, const vec2& b)
  553 { return !(a == b); }
  554 
  555 #ifdef ALGEBRA3IOSTREAMS
  556 inline ostream& operator << (ostream& s, const vec2& v)
  557 { return s << "| " << v.n[VX] << ' ' << v.n[VY] << " |"; }
  558 inline istream& operator >> (istream& s, vec2& v) {
  559     vec2    v_tmp;
  560     char    c = ' ';
  561 
  562     while (isspace(c))
  563     s >> c;
  564     // The vectors can be formatted either as x y or | x y |
  565     if (c == '|') {
  566     s >> v_tmp[VX] >> v_tmp[VY];
  567     while (s >> c && isspace(c)) ;
  568     if (c != '|')
  569         s.set(_bad);
  570     }
  571     else {
  572     s.putback(c);
  573     s >> v_tmp[VX] >> v_tmp[VY];
  574     }
  575     if (s)
  576     v = v_tmp;
  577     return s;
  578 }
  579 #endif // ALGEBRA3IOSTREAMS
  580 
  581 inline void swap(vec2& a, vec2& b)
  582 { vec2 tmp(a); a = b; b = tmp; }
  583 
  584 inline vec2 vec2min(const vec2& a, const vec2& b)
  585 { return vec2(MIN(a.n[VX], b.n[VX]), MIN(a.n[VY], b.n[VY])); }
  586 
  587 inline vec2 vec2max(const vec2& a, const vec2& b)
  588 { return vec2(MAX(a.n[VX], b.n[VX]), MAX(a.n[VY], b.n[VY])); }
  589 
  590 inline vec2 prod(const vec2& a, const vec2& b)
  591 { return vec2(a.n[VX] * b.n[VX], a.n[VY] * b.n[VY]); }
  592 
  593 /****************************************************************
  594 *                                                               *
  595 *           vec3 Member functions                               *
  596 *                                                               *
  597 ****************************************************************/
  598 
  599 // CONSTRUCTORS
  600 
  601 inline vec3::vec3() {}
  602 
  603 inline vec3::vec3(const double x, const double y, const double z)
  604 { n[VX] = x; n[VY] = y; n[VZ] = z; }
  605 
  606 inline vec3::vec3(const double d)
  607 { n[VX] = n[VY] = n[VZ] = d; }
  608 
  609 inline vec3::vec3(const vec3& v)
  610 { n[VX] = v.n[VX]; n[VY] = v.n[VY]; n[VZ] = v.n[VZ]; }
  611 
  612 inline vec3::vec3(const vec2& v)
  613 { n[VX] = v.n[VX]; n[VY] = v.n[VY]; n[VZ] = 1.0; }
  614 
  615 inline vec3::vec3(const vec2& v, double d)
  616 { n[VX] = v.n[VX]; n[VY] = v.n[VY]; n[VZ] = d; }
  617 
  618 inline vec3::vec3(const vec4& v) // it is up to caller to avoid divide-by-zero
  619 { n[VX] = v.n[VX] / v.n[VW]; n[VY] = v.n[VY] / v.n[VW];
  620   n[VZ] = v.n[VZ] / v.n[VW]; }
  621 
  622 inline vec3::vec3(const vec4& v, int dropAxis) {
  623     switch (dropAxis) {
  624     case VX: n[VX] = v.n[VY]; n[VY] = v.n[VZ]; n[VZ] = v.n[VW]; break;
  625     case VY: n[VX] = v.n[VX]; n[VY] = v.n[VZ]; n[VZ] = v.n[VW]; break;
  626     case VZ: n[VX] = v.n[VX]; n[VY] = v.n[VY]; n[VZ] = v.n[VW]; break;
  627     default: n[VX] = v.n[VX]; n[VY] = v.n[VY]; n[VZ] = v.n[VZ]; break;
  628     }
  629 }
  630 
  631 
  632 // ASSIGNMENT OPERATORS
  633 
  634 inline vec3& vec3::operator = (const vec3& v)
  635 { n[VX] = v.n[VX]; n[VY] = v.n[VY]; n[VZ] = v.n[VZ]; return *this; }
  636 
  637 inline vec3& vec3::operator += ( const vec3& v )
  638 { n[VX] += v.n[VX]; n[VY] += v.n[VY]; n[VZ] += v.n[VZ]; return *this; }
  639 
  640 inline vec3& vec3::operator -= ( const vec3& v )
  641 { n[VX] -= v.n[VX]; n[VY] -= v.n[VY]; n[VZ] -= v.n[VZ]; return *this; }
  642 
  643 inline vec3& vec3::operator *= ( const double d )
  644 { n[VX] *= d; n[VY] *= d; n[VZ] *= d; return *this; }
  645 
  646 inline vec3& vec3::operator /= ( const double d )
  647 { double d_inv = 1./d; n[VX] *= d_inv; n[VY] *= d_inv; n[VZ] *= d_inv;
  648   return *this; }
  649 
  650 inline double& vec3::operator [] ( int i) {
  651     assert(! (i < VX || i > VZ));
  652     return n[i];
  653 }
  654 
  655 inline double vec3::operator [] ( int i) const {
  656     assert(! (i < VX || i > VZ));
  657     return n[i];
  658 }
  659 
  660 
  661 // SPECIAL FUNCTIONS
  662 
  663 inline double vec3::length() const
  664 {  return sqrt(length2()); }
  665 
  666 inline double vec3::length2() const
  667 {  return n[VX]*n[VX] + n[VY]*n[VY] + n[VZ]*n[VZ]; }
  668 
  669 inline vec3& vec3::normalize() // it is up to caller to avoid divide-by-zero
  670 { *this /= length(); return *this; }
  671 
  672 inline vec3& vec3::apply(V_FCT_PTR fct)
  673 { n[VX] = (*fct)(n[VX]); n[VY] = (*fct)(n[VY]); n[VZ] = (*fct)(n[VZ]);
  674 return *this; }
  675 
  676 
  677 // FRIENDS
  678 
  679 inline vec3 operator - (const vec3& a)
  680 {  return vec3(-a.n[VX],-a.n[VY],-a.n[VZ]); }
  681 
  682 inline vec3 operator + (const vec3& a, const vec3& b)
  683 { return vec3(a.n[VX]+ b.n[VX], a.n[VY] + b.n[VY], a.n[VZ] + b.n[VZ]); }
  684 
  685 inline vec3 operator - (const vec3& a, const vec3& b)
  686 { return vec3(a.n[VX]-b.n[VX], a.n[VY]-b.n[VY], a.n[VZ]-b.n[VZ]); }
  687 
  688 inline vec3 operator * (const vec3& a, const double d)
  689 { return vec3(d*a.n[VX], d*a.n[VY], d*a.n[VZ]); }
  690 
  691 inline vec3 operator * (const double d, const vec3& a)
  692 { return a*d; }
  693 
  694 inline vec3 operator * (const mat4& a, const vec3& v)
  695 { return a * vec4(v); }
  696 
  697 inline vec3 operator * (const vec3& v, const mat4& a)
  698 { return a.transpose() * v; }
  699 
  700 inline double operator * (const vec3& a, const vec3& b)
  701 { return (a.n[VX]*b.n[VX] + a.n[VY]*b.n[VY] + a.n[VZ]*b.n[VZ]); }
  702 
  703 inline vec3 operator / (const vec3& a, const double d)
  704 { double d_inv = 1./d; return vec3(a.n[VX]*d_inv, a.n[VY]*d_inv,
  705   a.n[VZ]*d_inv); }
  706 
  707 inline vec3 operator ^ (const vec3& a, const vec3& b) {
  708     return vec3(a.n[VY]*b.n[VZ] - a.n[VZ]*b.n[VY],
  709         a.n[VZ]*b.n[VX] - a.n[VX]*b.n[VZ],
  710         a.n[VX]*b.n[VY] - a.n[VY]*b.n[VX]);
  711 }
  712 
  713 inline int operator == (const vec3& a, const vec3& b)
  714 { return (a.n[VX] == b.n[VX]) && (a.n[VY] == b.n[VY]) && (a.n[VZ] == b.n[VZ]);
  715 }
  716 
  717 inline int operator != (const vec3& a, const vec3& b)
  718 { return !(a == b); }
  719 
  720 #ifdef ALGEBRA3IOSTREAMS
  721 inline ostream& operator << (ostream& s, const vec3& v)
  722 { return s << "| " << v.n[VX] << ' ' << v.n[VY] << ' ' << v.n[VZ] << " |"; }
  723 
  724 inline istream& operator >> (istream& s, vec3& v) {
  725     vec3    v_tmp;
  726     char    c = ' ';
  727 
  728     while (isspace(c))
  729     s >> c;
  730     // The vectors can be formatted either as x y z or | x y z |
  731     if (c == '|') {
  732     s >> v_tmp[VX] >> v_tmp[VY] >> v_tmp[VZ];
  733     while (s >> c && isspace(c)) ;
  734     if (c != '|')
  735         s.set(_bad);
  736     }
  737     else {
  738     s.putback(c);
  739     s >> v_tmp[VX] >> v_tmp[VY] >> v_tmp[VZ];
  740     }
  741     if (s)
  742     v = v_tmp;
  743     return s;
  744 }
  745 #endif // ALGEBRA3IOSTREAMS
  746 
  747 inline void swap(vec3& a, vec3& b)
  748 { vec3 tmp(a); a = b; b = tmp; }
  749 
  750 inline vec3 vec3min(const vec3& a, const vec3& b)
  751 { return vec3(MIN(a.n[VX], b.n[VX]), MIN(a.n[VY], b.n[VY]), MIN(a.n[VZ],
  752   b.n[VZ])); }
  753 
  754 inline vec3 vec3max(const vec3& a, const vec3& b)
  755 { return vec3(MAX(a.n[VX], b.n[VX]), MAX(a.n[VY], b.n[VY]), MAX(a.n[VZ],
  756   b.n[VZ])); }
  757 
  758 inline vec3 prod(const vec3& a, const vec3& b)
  759 { return vec3(a.n[VX] * b.n[VX], a.n[VY] * b.n[VY], a.n[VZ] * b.n[VZ]); }
  760 
  761 
  762 /****************************************************************
  763 *                                                               *
  764 *           vec4 Member functions                               *
  765 *                                                               *
  766 ****************************************************************/
  767 
  768 // CONSTRUCTORS
  769 
  770 inline vec4::vec4() {}
  771 
  772 inline vec4::vec4(const double x, const double y, const double z, const double w)
  773 { n[VX] = x; n[VY] = y; n[VZ] = z; n[VW] = w; }
  774 
  775 inline vec4::vec4(const double d)
  776 {  n[VX] = n[VY] = n[VZ] = n[VW] = d; }
  777 
  778 inline vec4::vec4(const vec4& v)
  779 { n[VX] = v.n[VX]; n[VY] = v.n[VY]; n[VZ] = v.n[VZ]; n[VW] = v.n[VW]; }
  780 
  781 inline vec4::vec4(const vec3& v)
  782 { n[VX] = v.n[VX]; n[VY] = v.n[VY]; n[VZ] = v.n[VZ]; n[VW] = 1.0; }
  783 
  784 inline vec4::vec4(const vec3& v, const double d)
  785 { n[VX] = v.n[VX]; n[VY] = v.n[VY]; n[VZ] = v.n[VZ];  n[VW] = d; }
  786 
  787 
  788 // ASSIGNMENT OPERATORS
  789 
  790 inline vec4& vec4::operator = (const vec4& v)
  791 { n[VX] = v.n[VX]; n[VY] = v.n[VY]; n[VZ] = v.n[VZ]; n[VW] = v.n[VW];
  792 return *this; }
  793 
  794 inline vec4& vec4::operator += ( const vec4& v )
  795 { n[VX] += v.n[VX]; n[VY] += v.n[VY]; n[VZ] += v.n[VZ]; n[VW] += v.n[VW];
  796 return *this; }
  797 
  798 inline vec4& vec4::operator -= ( const vec4& v )
  799 { n[VX] -= v.n[VX]; n[VY] -= v.n[VY]; n[VZ] -= v.n[VZ]; n[VW] -= v.n[VW];
  800 return *this; }
  801 
  802 inline vec4& vec4::operator *= ( const double d )
  803 { n[VX] *= d; n[VY] *= d; n[VZ] *= d; n[VW] *= d; return *this; }
  804 
  805 inline vec4& vec4::operator /= ( const double d )
  806 { double d_inv = 1./d; n[VX] *= d_inv; n[VY] *= d_inv; n[VZ] *= d_inv;
  807   n[VW] *= d_inv; return *this; }
  808 
  809 inline double& vec4::operator [] ( int i) {
  810     assert(! (i < VX || i > VW));
  811     return n[i];
  812 }
  813 
  814 inline double vec4::operator [] ( int i) const {
  815     assert(! (i < VX || i > VW));
  816     return n[i];
  817 }
  818 
  819 // SPECIAL FUNCTIONS
  820 
  821 inline double vec4::length() const
  822 { return sqrt(length2()); }
  823 
  824 inline double vec4::length2() const
  825 { return n[VX]*n[VX] + n[VY]*n[VY] + n[VZ]*n[VZ] + n[VW]*n[VW]; }
  826 
  827 inline vec4& vec4::normalize() // it is up to caller to avoid divide-by-zero
  828 { *this /= length(); return *this; }
  829 
  830 inline vec4& vec4::apply(V_FCT_PTR fct)
  831 { n[VX] = (*fct)(n[VX]); n[VY] = (*fct)(n[VY]); n[VZ] = (*fct)(n[VZ]);
  832 n[VW] = (*fct)(n[VW]); return *this; }
  833 
  834 
  835 // FRIENDS
  836 
  837 inline vec4 operator - (const vec4& a)
  838 { return vec4(-a.n[VX],-a.n[VY],-a.n[VZ],-a.n[VW]); }
  839 
  840 inline vec4 operator + (const vec4& a, const vec4& b)
  841 { return vec4(a.n[VX] + b.n[VX], a.n[VY] + b.n[VY], a.n[VZ] + b.n[VZ],
  842   a.n[VW] + b.n[VW]); }
  843 
  844 inline vec4 operator - (const vec4& a, const vec4& b)
  845 {  return vec4(a.n[VX] - b.n[VX], a.n[VY] - b.n[VY], a.n[VZ] - b.n[VZ],
  846    a.n[VW] - b.n[VW]); }
  847 
  848 inline vec4 operator * (const vec4& a, const double d)
  849 { return vec4(d*a.n[VX], d*a.n[VY], d*a.n[VZ], d*a.n[VW] ); }
  850 
  851 inline vec4 operator * (const double d, const vec4& a)
  852 { return a*d; }
  853 
  854 inline vec4 operator * (const mat4& a, const vec4& v) {
  855 #define ROWCOL(i) a.v[i].n[0]*v.n[VX] + a.v[i].n[1]*v.n[VY] \
  856     + a.v[i].n[2]*v.n[VZ] + a.v[i].n[3]*v.n[VW]
  857     return vec4(ROWCOL(0), ROWCOL(1), ROWCOL(2), ROWCOL(3));
  858 #undef ROWCOL // (i)
  859 }
  860 
  861 inline vec4 operator * (const vec4& v, const mat4& a)
  862 { return a.transpose() * v; }
  863 
  864 inline double operator * (const vec4& a, const vec4& b)
  865 { return (a.n[VX]*b.n[VX] + a.n[VY]*b.n[VY] + a.n[VZ]*b.n[VZ] +
  866   a.n[VW]*b.n[VW]); }
  867 
  868 inline vec4 operator / (const vec4& a, const double d)
  869 { double d_inv = 1./d; return vec4(a.n[VX]*d_inv, a.n[VY]*d_inv, a.n[VZ]*d_inv,
  870   a.n[VW]*d_inv); }
  871 
  872 inline int operator == (const vec4& a, const vec4& b)
  873 { return (a.n[VX] == b.n[VX]) && (a.n[VY] == b.n[VY]) && (a.n[VZ] == b.n[VZ])
  874   && (a.n[VW] == b.n[VW]); }
  875 
  876 inline int operator != (const vec4& a, const vec4& b)
  877 { return !(a == b); }
  878 
  879 #ifdef ALGEBRA3IOSTREAMS
  880 inline ostream& operator << (ostream& s, const vec4& v)
  881 { return s << "| " << v.n[VX] << ' ' << v.n[VY] << ' ' << v.n[VZ] << ' '
  882   << v.n[VW] << " |"; }
  883 
  884 inline stream& operator >> (istream& s, vec4& v) {
  885     vec4    v_tmp;
  886     char    c = ' ';
  887 
  888     while (isspace(c))
  889     s >> c;
  890     // The vectors can be formatted either as x y z w or | x y z w |
  891     if (c == '|') {
  892     s >> v_tmp[VX] >> v_tmp[VY] >> v_tmp[VZ] >> v_tmp[VW];
  893     while (s >> c && isspace(c)) ;
  894     if (c != '|')
  895         s.set(_bad);
  896     }
  897     else {
  898     s.putback(c);
  899     s >> v_tmp[VX] >> v_tmp[VY] >> v_tmp[VZ] >> v_tmp[VW];
  900     }
  901     if (s)
  902     v = v_tmp;
  903     return s;
  904 }
  905 #endif // ALGEBRA3IOSTREAMS
  906 
  907 inline void swap(vec4& a, vec4& b)
  908 { vec4 tmp(a); a = b; b = tmp; }
  909 
  910 inline vec4 vec4min(const vec4& a, const vec4& b)
  911 { return vec4(MIN(a.n[VX], b.n[VX]), MIN(a.n[VY], b.n[VY]), MIN(a.n[VZ],
  912   b.n[VZ]), MIN(a.n[VW], b.n[VW])); }
  913 
  914 inline vec4 vec4max(const vec4& a, const vec4& b)
  915 { return vec4(MAX(a.n[VX], b.n[VX]), MAX(a.n[VY], b.n[VY]), MAX(a.n[VZ],
  916   b.n[VZ]), MAX(a.n[VW], b.n[VW])); }
  917 
  918 inline vec4 prod(const vec4& a, const vec4& b)
  919 { return vec4(a.n[VX] * b.n[VX], a.n[VY] * b.n[VY], a.n[VZ] * b.n[VZ],
  920   a.n[VW] * b.n[VW]); }
  921 
  922 
  923 /****************************************************************
  924 *                                                               *
  925 *           mat3 member functions                               *
  926 *                                                               *
  927 ****************************************************************/
  928 
  929 // CONSTRUCTORS
  930 
  931 inline mat3::mat3() {}
  932 
  933 inline mat3::mat3(const vec3& v0, const vec3& v1, const vec3& v2)
  934 { v[0] = v0; v[1] = v1; v[2] = v2; }
  935 
  936 inline mat3::mat3(const double d)
  937 { v[0] = v[1] = v[2] = vec3(d); }
  938 
  939 inline mat3::mat3(const mat3& m)
  940 { v[0] = m.v[0]; v[1] = m.v[1]; v[2] = m.v[2]; }
  941 
  942 
  943 // ASSIGNMENT OPERATORS
  944 
  945 inline mat3& mat3::operator = ( const mat3& m )
  946 { v[0] = m.v[0]; v[1] = m.v[1]; v[2] = m.v[2]; return *this; }
  947 
  948 inline mat3& mat3::operator += ( const mat3& m )
  949 { v[0] += m.v[0]; v[1] += m.v[1]; v[2] += m.v[2]; return *this; }
  950 
  951 inline mat3& mat3::operator -= ( const mat3& m )
  952 { v[0] -= m.v[0]; v[1] -= m.v[1]; v[2] -= m.v[2]; return *this; }
  953 
  954 inline mat3& mat3::operator *= ( const double d )
  955 { v[0] *= d; v[1] *= d; v[2] *= d; return *this; }
  956 
  957 inline mat3& mat3::operator /= ( const double d )
  958 { v[0] /= d; v[1] /= d; v[2] /= d; return *this; }
  959 
  960 inline vec3& mat3::operator [] ( int i) {
  961     assert(! (i < VX || i > VZ));
  962     return v[i];
  963 }
  964 
  965 inline const vec3& mat3::operator [] ( int i) const {
  966     assert(!(i < VX || i > VZ));
  967     return v[i];
  968 }
  969 
  970 // SPECIAL FUNCTIONS
  971 
  972 inline mat3 mat3::transpose() const {
  973     return mat3(vec3(v[0][0], v[1][0], v[2][0]),
  974         vec3(v[0][1], v[1][1], v[2][1]),
  975         vec3(v[0][2], v[1][2], v[2][2]));
  976 }
  977 
  978 inline mat3 mat3::inverse() const    // Gauss-Jordan elimination with partial pivoting
  979     {
  980     mat3 a(*this),      // As a evolves from original mat into identity
  981      b(identity2D());   // b evolves from identity into inverse(a)
  982     int  i, j, i1;
  983 
  984     // Loop over cols of a from left to right, eliminating above and below diag
  985     for (j=0; j<3; j++) {   // Find largest pivot in column j among rows j..2
  986     i1 = j;         // Row with largest pivot candidate
  987     for (i=j+1; i<3; i++)
  988     if (fabs(a.v[i].n[j]) > fabs(a.v[i1].n[j]))
  989         i1 = i;
  990 
  991     // Swap rows i1 and j in a and b to put pivot on diagonal
  992     swap(a.v[i1], a.v[j]);
  993     swap(b.v[i1], b.v[j]);
  994 
  995     // Scale row j to have a unit diagonal
  996     if (a.v[j].n[j]==0.)
  997     ALGEBRA_ERROR("mat3::inverse: singular matrix; can't invert\n")
  998     b.v[j] /= a.v[j].n[j];
  999     a.v[j] /= a.v[j].n[j];
 1000 
 1001     // Eliminate off-diagonal elems in col j of a, doing identical ops to b
 1002     for (i=0; i<3; i++)
 1003     if (i!=j) {
 1004     b.v[i] -= a.v[i].n[j]*b.v[j];
 1005     a.v[i] -= a.v[i].n[j]*a.v[j];
 1006     }
 1007     }
 1008     return b;
 1009 }
 1010 
 1011 inline mat3& mat3::apply(V_FCT_PTR fct) {
 1012     v[VX].apply(fct);
 1013     v[VY].apply(fct);
 1014     v[VZ].apply(fct);
 1015     return *this;
 1016 }
 1017 
 1018 
 1019 // FRIENDS
 1020 
 1021 inline mat3 operator - (const mat3& a)
 1022 { return mat3(-a.v[0], -a.v[1], -a.v[2]); }
 1023 
 1024 inline mat3 operator + (const mat3& a, const mat3& b)
 1025 { return mat3(a.v[0] + b.v[0], a.v[1] + b.v[1], a.v[2] + b.v[2]); }
 1026 
 1027 inline mat3 operator - (const mat3& a, const mat3& b)
 1028 { return mat3(a.v[0] - b.v[0], a.v[1] - b.v[1], a.v[2] - b.v[2]); }
 1029 
 1030 inline mat3 operator * (const mat3& a, const mat3& b) {
 1031     #define ROWCOL(i, j) \
 1032     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]
 1033     return mat3(vec3(ROWCOL(0,0), ROWCOL(0,1), ROWCOL(0,2)),
 1034         vec3(ROWCOL(1,0), ROWCOL(1,1), ROWCOL(1,2)),
 1035         vec3(ROWCOL(2,0), ROWCOL(2,1), ROWCOL(2,2)));
 1036     #undef ROWCOL // (i, j)
 1037 }
 1038 
 1039 inline mat3 operator * (const mat3& a, const double d)
 1040 { return mat3(a.v[0] * d, a.v[1] * d, a.v[2] * d); }
 1041 
 1042 inline mat3 operator * (const double d, const mat3& a)
 1043 { return a*d; }
 1044 
 1045 inline mat3 operator / (const mat3& a, const double d)
 1046 { return mat3(a.v[0] / d, a.v[1] / d, a.v[2] / d); }
 1047 
 1048 inline int operator == (const mat3& a, const mat3& b)
 1049 { return (a.v[0] == b.v[0]) && (a.v[1] == b.v[1]) && (a.v[2] == b.v[2]); }
 1050 
 1051 inline int operator != (const mat3& a, const mat3& b)
 1052 { return !(a == b); }
 1053 
 1054 #ifdef ALGEBRA3IOSTREAMS
 1055 inline ostream& operator << (ostream& s, const mat3& m)
 1056 { return s << m.v[VX] << '\n' << m.v[VY] << '\n' << m.v[VZ]; }
 1057 
 1058 inline stream& operator >> (istream& s, mat3& m) {
 1059     mat3    m_tmp;
 1060 
 1061     s >> m_tmp[VX] >> m_tmp[VY] >> m_tmp[VZ];
 1062     if (s)
 1063     m = m_tmp;
 1064     return s;
 1065 }
 1066 #endif // ALGEBRA3IOSTREAMS
 1067 
 1068 inline void swap(mat3& a, mat3& b)
 1069 { mat3 tmp(a); a = b; b = tmp; }
 1070 
 1071 
 1072 /****************************************************************
 1073 *                                                               *
 1074 *           mat4 member functions                               *
 1075 *                                                               *
 1076 ****************************************************************/
 1077 
 1078 // CONSTRUCTORS
 1079 
 1080 inline mat4::mat4() {}
 1081 
 1082 inline mat4::mat4(const vec4& v0, const vec4& v1, const vec4& v2, const vec4& v3)
 1083 { v[0] = v0; v[1] = v1; v[2] = v2; v[3] = v3; }
 1084 
 1085 inline mat4::mat4(const double d)
 1086 { v[0] = v[1] = v[2] = v[3] = vec4(d); }
 1087 
 1088 inline mat4::mat4(const mat4& m)
 1089 { v[0] = m.v[0]; v[1] = m.v[1]; v[2] = m.v[2]; v[3] = m.v[3]; }
 1090 
 1091 
 1092 // ASSIGNMENT OPERATORS
 1093 
 1094 inline mat4& mat4::operator = ( const mat4& m )
 1095 { v[0] = m.v[0]; v[1] = m.v[1]; v[2] = m.v[2]; v[3] = m.v[3];
 1096 return *this; }
 1097 
 1098 inline mat4& mat4::operator += ( const mat4& m )
 1099 { v[0] += m.v[0]; v[1] += m.v[1]; v[2] += m.v[2]; v[3] += m.v[3];
 1100 return *this; }
 1101 
 1102 inline mat4& mat4::operator -= ( const mat4& m )
 1103 { v[0] -= m.v[0]; v[1] -= m.v[1]; v[2] -= m.v[2]; v[3] -= m.v[3];
 1104 return *this; }
 1105 
 1106 inline mat4& mat4::operator *= ( const double d )
 1107 { v[0] *= d; v[1] *= d; v[2] *= d; v[3] *= d; return *this; }
 1108 
 1109 inline mat4& mat4::operator /= ( const double d )
 1110 { v[0] /= d; v[1] /= d; v[2] /= d; v[3] /= d; return *this; }
 1111 
 1112 inline vec4& mat4::operator [] ( int i) {
 1113     assert(! (i < VX || i > VW));
 1114     return v[i];
 1115 }
 1116 
 1117 inline const vec4& mat4::operator [] ( int i) const {
 1118     assert(! (i < VX || i > VW));
 1119     return v[i];
 1120 }
 1121 
 1122 // SPECIAL FUNCTIONS;
 1123 
 1124 inline mat4 mat4::transpose() const{
 1125     return mat4(vec4(v[0][0], v[1][0], v[2][0], v[3][0]),
 1126         vec4(v[0][1], v[1][1], v[2][1], v[3][1]),
 1127         vec4(v[0][2], v[1][2], v[2][2], v[3][2]),
 1128         vec4(v[0][3], v[1][3], v[2][3], v[3][3]));
 1129 }
 1130 
 1131 inline mat4 mat4::inverse() const    // Gauss-Jordan elimination with partial pivoting
 1132 {
 1133     mat4 a(*this),      // As a evolves from original mat into identity
 1134      b(identity3D());   // b evolves from identity into inverse(a)
 1135     int i, j, i1;
 1136 
 1137     // Loop over cols of a from left to right, eliminating above and below diag
 1138     for (j=0; j<4; j++) {   // Find largest pivot in column j among rows j..3
 1139     i1 = j;         // Row with largest pivot candidate
 1140     for (i=j+1; i<4; i++)
 1141     if (fabs(a.v[i].n[j]) > fabs(a.v[i1].n[j]))
 1142     i1 = i;
 1143 
 1144     // Swap rows i1 and j in a and b to put pivot on diagonal
 1145     swap(a.v[i1], a.v[j]);
 1146     swap(b.v[i1], b.v[j]);
 1147 
 1148     // Scale row j to have a unit diagonal
 1149     if (a.v[j].n[j]==0.)
 1150     ALGEBRA_ERROR("mat4::inverse: singular matrix; can't invert\n");
 1151     b.v[j] /= a.v[j].n[j];
 1152     a.v[j] /= a.v[j].n[j];
 1153 
 1154     // Eliminate off-diagonal elems in col j of a, doing identical ops to b
 1155     for (i=0; i<4; i++)
 1156     if (i!=j) {
 1157     b.v[i] -= a.v[i].n[j]*b.v[j];
 1158     a.v[i] -= a.v[i].n[j]*a.v[j];
 1159     }
 1160     }
 1161     return b;
 1162 }
 1163 
 1164 inline mat4& mat4::apply(V_FCT_PTR fct)
 1165 { v[VX].apply(fct); v[VY].apply(fct); v[VZ].apply(fct); v[VW].apply(fct);
 1166 return *this; }
 1167 
 1168 
 1169 // FRIENDS
 1170 
 1171 inline mat4 operator - (const mat4& a)
 1172 { return mat4(-a.v[0], -a.v[1], -a.v[2], -a.v[3]); }
 1173 
 1174 inline mat4 operator + (const mat4& a, const mat4& b)
 1175 { return mat4(a.v[0] + b.v[0], a.v[1] + b.v[1], a.v[2] + b.v[2],
 1176   a.v[3] + b.v[3]);
 1177 }
 1178 
 1179 inline mat4 operator - (const mat4& a, const mat4& b)
 1180 { return mat4(a.v[0] - b.v[0], a.v[1] - b.v[1], a.v[2] - b.v[2], a.v[3] - b.v[3]); }
 1181 
 1182 inline mat4 operator * (const mat4& a, const mat4& b) {
 1183     #define ROWCOL(i, j) a.v[i].n[0]*b.v[0][j] + a.v[i].n[1]*b.v[1][j] + \
 1184     a.v[i].n[2]*b.v[2][j] + a.v[i].n[3]*b.v[3][j]
 1185     return mat4(
 1186     vec4(ROWCOL(0,0), ROWCOL(0,1), ROWCOL(0,2), ROWCOL(0,3)),
 1187     vec4(ROWCOL(1,0), ROWCOL(1,1), ROWCOL(1,2), ROWCOL(1,3)),
 1188     vec4(ROWCOL(2,0), ROWCOL(2,1), ROWCOL(2,2), ROWCOL(2,3)),
 1189     vec4(ROWCOL(3,0), ROWCOL(3,1), ROWCOL(3,2), ROWCOL(3,3))
 1190     );
 1191 }
 1192 
 1193 inline mat4 operator * (const mat4& a, const double d)
 1194 { return mat4(a.v[0] * d, a.v[1] * d, a.v[2] * d, a.v[3] * d); }
 1195 
 1196 inline mat4 operator * (const double d, const mat4& a)
 1197 { return a*d; }
 1198 
 1199 inline mat4 operator / (const mat4& a, const double d)
 1200 { return mat4(a.v[0] / d, a.v[1] / d, a.v[2] / d, a.v[3] / d); }
 1201 
 1202 inline int operator == (const mat4& a, const mat4& b)
 1203 { return ((a.v[0] == b.v[0]) && (a.v[1] == b.v[1]) && (a.v[2] == b.v[2]) &&
 1204   (a.v[3] == b.v[3])); }
 1205 
 1206 inline int operator != (const mat4& a, const mat4& b)
 1207 { return !(a == b); }
 1208 
 1209 #ifdef ALGEBRA3IOSTREAMS
 1210 inline ostream& operator << (ostream& s, const mat4& m)
 1211 { return s << m.v[VX] << '\n' << m.v[VY] << '\n' << m.v[VZ] << '\n' << m.v[VW]; }
 1212 
 1213 inline istream& operator >> (istream& s, mat4& m)
 1214 {
 1215     mat4    m_tmp;
 1216 
 1217     s >> m_tmp[VX] >> m_tmp[VY] >> m_tmp[VZ] >> m_tmp[VW];
 1218     if (s)
 1219     m = m_tmp;
 1220     return s;
 1221 }
 1222 #endif // ALGEBRA3IOSTREAMS
 1223 
 1224 inline void swap(mat4& a, mat4& b)
 1225 { mat4 tmp(a); a = b; b = tmp; }
 1226 
 1227 
 1228 /****************************************************************
 1229 *                                                               *
 1230 *          2D functions and 3D functions                        *
 1231 *                                                               *
 1232 ****************************************************************/
 1233 
 1234 inline mat3 identity2D()
 1235 {   return mat3(vec3(1.0, 0.0, 0.0),
 1236         vec3(0.0, 1.0, 0.0),
 1237         vec3(0.0, 0.0, 1.0)); }
 1238 
 1239 inline mat3 translation2D(const vec2& v)
 1240 {   return mat3(vec3(1.0, 0.0, v[VX]),
 1241         vec3(0.0, 1.0, v[VY]),
 1242         vec3(0.0, 0.0, 1.0)); }
 1243 
 1244 inline mat3 rotation2D(const vec2& Center, const double angleDeg) {
 1245     double  angleRad = radians(angleDeg),
 1246         c = cos(angleRad),
 1247         s = sin(angleRad);
 1248 
 1249     return mat3(vec3(c, -s, Center[VX] * (1.0-c) + Center[VY] * s),
 1250         vec3(s, c, Center[VY] * (1.0-c) - Center[VX] * s),
 1251         vec3(0.0, 0.0, 1.0));
 1252 }
 1253 
 1254 inline mat3 scaling2D(const vec2& scaleVector)
 1255 {   return mat3(vec3(scaleVector[VX], 0.0, 0.0),
 1256         vec3(0.0, scaleVector[VY], 0.0),
 1257         vec3(0.0, 0.0, 1.0)); }
 1258 
 1259 inline mat4 identity3D()
 1260 {   return mat4(vec4(1.0, 0.0, 0.0, 0.0),
 1261         vec4(0.0, 1.0, 0.0, 0.0),
 1262         vec4(0.0, 0.0, 1.0, 0.0),
 1263         vec4(0.0, 0.0, 0.0, 1.0)); }
 1264 
 1265 inline mat4 translation3D(const vec3& v)
 1266 {   return mat4(vec4(1.0, 0.0, 0.0, v[VX]),
 1267         vec4(0.0, 1.0, 0.0, v[VY]),
 1268         vec4(0.0, 0.0, 1.0, v[VZ]),
 1269         vec4(0.0, 0.0, 0.0, 1.0)); }
 1270 
 1271 inline mat4 rotation3D(vec3 Axis, const double angleDeg) {
 1272     double  angleRad = radians(angleDeg),
 1273         c = cos(angleRad),
 1274         s = sin(angleRad),
 1275         t = 1.0 - c;
 1276 
 1277     Axis.normalize();
 1278     return mat4(vec4(t * Axis[VX] * Axis[VX] + c,
 1279              t * Axis[VX] * Axis[VY] - s * Axis[VZ],
 1280              t * Axis[VX] * Axis[VZ] + s * Axis[VY],
 1281              0.0),
 1282         vec4(t * Axis[VX] * Axis[VY] + s * Axis[VZ],
 1283              t * Axis[VY] * Axis[VY] + c,
 1284              t * Axis[VY] * Axis[VZ] - s * Axis[VX],
 1285              0.0),
 1286         vec4(t * Axis[VX] * Axis[VZ] - s * Axis[VY],
 1287              t * Axis[VY] * Axis[VZ] + s * Axis[VX],
 1288              t * Axis[VZ] * Axis[VZ] + c,
 1289              0.0),
 1290         vec4(0.0, 0.0, 0.0, 1.0));
 1291 }
 1292 
 1293 inline mat4 scaling3D(const vec3& scaleVector)
 1294 {   return mat4(vec4(scaleVector[VX], 0.0, 0.0, 0.0),
 1295         vec4(0.0, scaleVector[VY], 0.0, 0.0),
 1296         vec4(0.0, 0.0, scaleVector[VZ], 0.0),
 1297         vec4(0.0, 0.0, 0.0, 1.0)); }
 1298 
 1299 inline mat4 perspective3D(const double d)
 1300 {   return mat4(vec4(1.0, 0.0, 0.0, 0.0),
 1301         vec4(0.0, 1.0, 0.0, 0.0),
 1302         vec4(0.0, 0.0, 1.0, 0.0),
 1303         vec4(0.0, 0.0, 1.0/d, 0.0)); }
 1304 
 1305 
 1306 #endif // ALGEBRA3H