实作ADTvector2 vector3 vector4 matrix44 matrix33

http://hi.baidu.com/zyb_debug/blog/item/b837e41ffd64f7fd1ad5761b.html
以前一直有个想法就是写标题上的ADT，但一直担心自己写的效率不高。

最近看了一段源码，终于圆了我的梦想。

话不多说，国外高手写的matrix，想法果然不一样，直接看代码吧。

来自 《游戏编程精粹1》

mtxlib.h

/*  Copyright (C) Dante Treglia II and Mark A. DeLoura, 2000. 
 * All rights reserved worldwide.
 *
 * This software is provided "as is" without express or implied
 * warranties. You may freely copy and compile this source into
 * applications you distribute provided that the copyright text
 * below is included in the resulting source code, for example:
 * "Portions Copyright (C) Dante Treglia II and Mark A. DeLoura, 2000"
  */
// ==========================================================
//  C++ Matrix Library
//  Version: 2.6
//  Date: May 19, 2000
//  Authors: Dante Treglia II and Mark A. DeLoura
//  Thanks to: Miguel Gomez, Stan Melax, Pete Isensee, 
//    Gabor Nagy, Scott Bilas, James Boer, Eric Lengyel
// ==========================================================

#ifndef _MTXLIB_H
#define  _MTXLIB_H
#include  < cstdio >
#include  < cmath >
#include  < cassert >

static  inline  float  DegToRad( float  a) {  return  a * 0.01745329252f ;};
static  inline  float  RadToDeg( float  a) {  return  a * 57.29577951f ;};

class  vector2;
class  vector3;
class  vector4;
class  matrix33;
class  matrix44;

////////////////////////////////////////////////////////////
//  vector2 class
//

class  vector2 
{
public :
   //  Members
   float  x, y;

public :
   //  Constructors
  vector2() {};
   //  Constructor with initializing float values
  vector2( float  inX,  float  inY): x(inX), y(inY) {}
   //  Constructor with initializing vector2
  vector2( const  vector2  & v): x(v.x), y(v.y) {}

public :
   //  Array indexing
   float                   & operator  [] (unsigned  int  i) {
    assert (i < 2 );
     return   * ( & x + i);
  }
   //  Array indexing
   const   float             & operator  [] (unsigned  int  i)  const  {
    assert (i < 2 );
     return   * ( & x + i);
  }
   //  Add a vector2 to this one
  vector2                & operator   +=  ( const  vector2  & v) {
    x  +=  v.x;
    y  +=  v.y;
     return   * this ;
  }
   //  Subtract a vector2 from this one
  vector2                & operator   -=  ( const  vector2  & v) {
    x  -=  v.x;
    y  -=  v.y;
     return   * this ;
  }
   //  Multiply the vector2 by a float
  vector2                & operator   *=  ( float  f) {
    x  *=  f;
    y  *=  f;
     return   * this ;
  }
   //  Divide the vector2 by a float
  vector2                & operator   /=  ( float  f) {
    x  /=  f;
    y  /=  f;
     return   * this ;
  }
   //  Are these two vector2's equal?
  friend  bool             operator   ==  ( const  vector2  & a,  const  vector2  & b) {
     return ((a.x  ==  b.x)  &&  (a.y  ==  b.y));
  }
   //  Are these two vector2's not equal?
  friend  bool             operator   !=  ( const  vector2  & a,  const  vector2  & b) {
     return ((a.x  !=  b.x)  ||  (a.y  !=  b.y));
  }
   //  Negate this vector
  friend vector2         operator   -  ( const  vector2  & a) {
     return  vector2( - a.x,  - a.y);
  }
   //  Add two vector2's
  friend vector2         operator   +  ( const  vector2  & a,  const  vector2  & b) {
    vector2 ret(a);
    ret  +=  b;
     return  ret;
  }
   //  Subtract one vector2 from another
  friend vector2         operator   -  ( const  vector2  & a,  const  vector2  & b) {
    vector2 ret(a);
    ret  -=  b;
     return  ret;
  }
   //  Multiply vector2 by a float
  friend vector2         operator   *  ( const  vector2  & v,  float  f) {
     return  vector2(f  *  v.x, f  *  v.y);
  }
   //  Multiply vector2 by a float
  friend vector2         operator   *  ( float  f,  const  vector2  & v) {
     return  vector2(f  *  v.x, f  *  v.y);
  }
   //  Divide vector2 by a float
  friend vector2         operator   /  ( const  vector2  & v,  float  f) {
     return  vector2(v.x  /  f, v.y  /  f);
  }

public :
   //  Methods
   //  Set Values
   void                    set ( float  xIn,  float  yIn) {
    x  =  xIn;
    y  =  yIn;
  }
   //  Get length of a vector2
   float                  length()  const  {
     return ( float ) sqrt(x * x  +  y * y);
  }
   //  Get squared length of a vector2
   float                  lengthSqr()  const  {
     return (x * x  +  y * y);
  }
   //  Does vector2 equal (0, 0)?
   bool                   isZero()  const  {
     return ((x  ==   0.0F )  &&  (y  ==   0.0F ));
  }
   //  Normalize a vector2
  vector2                & normalize() {
     float  m  =  length();

     if  (m  >   0.0F )
      m  =   1.0F   /  m;
     else
      m  =   0.0F ;
    x  *=  m;
    y  *=  m;

     return   * this ;
  }

   //  Debug
   void                   fprint(FILE *  file,  char *  str)  const ;
};


////////////////////////////////////////////////////////////
//  vector3 class
//

class  vector3 
{
public :
   //  Members
   float  x, y, z;

public :
   //  Constructors
  vector3() {};
   //  Constructor with initializing float values
  vector3( float  inX,  float  inY,  float  inZ): x(inX), y(inY), z(inZ) {}
   //  Constructor with initializing vector3
  vector3( const  vector3  & v): x(v.x), y(v.y), z(v.z) {}
   //  Constructor with initializing vector2
   explicit  vector3( const  vector2  & v): x(v.x), y(v.y), z( 0.0F ) {}
   //  Constructor with initializing vector4
   //  TODO
   explicit  vector3( const  vector4  & v);

public :
   //  Operators
   //  Array indexing
   float                   & operator  [] (unsigned  int  i) {
    assert (i < 3 );
     return   * ( & x + i);
  }
   //  Array indexing
   const   float             & operator  [] (unsigned  int  i)  const  {
    assert (i < 3 );
     return   * ( & x + i);
  }
   //  Assign from a vector2
  vector3                & operator   =   ( const  vector2  & v) {
    x  =  v.x;
    y  =  v.y;
    z  =   0.0F ;
     return   * this ;
  }
   //  Add a vector3 to this one
  vector3                & operator   +=  ( const  vector3  & v) {
    x  +=  v.x;
    y  +=  v.y;
    z  +=  v.z;
     return   * this ;
  }
   //  Subtract a vector3 from this one
  vector3                & operator   -=  ( const  vector3  & v) {
    x  -=  v.x;
    y  -=  v.y;
    z  -=  v.z;
     return   * this ;
  }
   //  Multiply the vector3 by a float
  vector3                & operator   *=  ( float  f) {
    x  *=  f;
    y  *=  f;
    z  *=  f;
     return   * this ;
  }
   //  Divide the vector3 by a float
  vector3                & operator   /=  ( float  f) {
    x  /=  f;
    y  /=  f;
    z  /=  f;
     return   * this ;
  }
   //  Are these two vector3's equal?
  friend  bool             operator   ==  ( const  vector3  & a,  const  vector3  & b) {
     return ((a.x  ==  b.x)  &&  (a.y  ==  b.y)  &&  (a.z  ==  b.z));
  }
   //  Are these two vector3's not equal?
  friend  bool             operator   !=  ( const  vector3  & a,  const  vector3  & b) {
     return ((a.x  !=  b.x)  ||  (a.y  !=  b.y)  ||  (a.z  !=  b.z));
  }
   //  Negate a vector3
  friend vector3         operator   -  ( const  vector3  & a) {
     return  vector3( - a.x,  - a.y,  - a.z);
  }
   //  Add two vector3's
  friend vector3         operator   +  ( const  vector3  & a,  const  vector3  & b) {
    vector3 ret(a);
    ret  +=  b;
     return  ret;
  }
   //  Subtract one vector3 from another
  friend vector3         operator   -  ( const  vector3  & a,  const  vector3  & b) {
    vector3 ret(a);
    ret  -=  b;
     return  ret;
  }
   //  Multiply vector3 by a float
  friend vector3         operator   *  ( const  vector3  & v,  float  f) {
     return  vector3(f  *  v.x, f  *  v.y, f  *  v.z);
  }
   //  Multiply vector3 by a float
  friend vector3         operator   *  ( float  f,  const  vector3  & v) {
     return  vector3(f  *  v.x, f  *  v.y, f  *  v.z);
  }
   //  Divide vector3 by a float
  friend vector3         operator   /  ( const  vector3  & v,  float  f) {
     return  vector3(v.x  /  f, v.y  /  f, v.z  /  f);
  }

public :
   //  Methods
   //  Set Values
   void                    set ( float  xIn,  float  yIn,  float  zIn) {
    x  =  xIn;
    y  =  yIn;
    z  =  zIn;
  }
   //  Get length of a vector3
   float                  length()  const  {
     return ( float ) sqrt(x * x  +  y * y  +  z * z);
  }
   //  Get squared length of a vector3
   float                  lengthSqr()  const  {
     return (x * x  +  y * y  +  z * z);
  }
   //  Does vector3 equal (0, 0, 0)?
   bool                   isZero()  const  {
     return ((x  ==   0.0F )  &&  (y  ==   0.0F )  &&  (z  ==   0.0F ));
  }
   //  Normalize a vector3
  vector3                & normalize() {
     float  m  =  length();
     if  (m  >   0.0F )
      m  =   1.0F   /  m;
     else
      m  =   0.0F ;
    x  *=  m;
    y  *=  m;
    z  *=  m;
     return   * this ;
  }

   //  Debug
   void                   fprint(FILE *  file,  char *  str)  const ;
};


////////////////////////////////////////////////////////////
//  vector4 class
//

class  vector4 
{
public :
   //  Members
   float  x, y, z, w;

public :
   //  Constructors
   //  vector4(): x(0), y(0), z(0), w(0) {};
  vector4() {};
   //  Constructor with initializing float values
  vector4( float  inX,  float  inY,  float  inZ,  float  inW): x(inX), y(inY), z(inZ), w(inW) {};
   //  Constructor with initializing vector4
  vector4( const  vector4  & v): x(v.x), y(v.y), z(v.z), w(v.w) {};
   //  Constructor with initializing vector3
   explicit  vector4( const  vector3  & v): x(v.x), y(v.y), z(v.z), w( 0.0F ) {};
   //  Constructor with initializing vector2
   explicit  vector4( const  vector2  & v): x(v.x), y(v.y), z( 0.0F ), w( 0.0F ) {};

public :
   //  Operators
   //  Array indexing
   float                   & operator  [] (unsigned  int  i) {
    assert (i < 4 );
     // return *(&x+i);
     return (i  ==   0 )  ?  x : (i  ==   1 )  ?  y : (i  ==   2 )  ?  z : w;
  }
   //  Array indexing
   const   float             & operator  [] (unsigned  int  i)  const  {
    assert (i < 4 );
     // return *(&x+i);
     return (i  ==   0 )  ?  x : (i  ==   1 )  ?  y : (i  ==   2 )  ?  z : w;
  }
   //  Assign from a vector3
  vector4                & operator   =   ( const  vector3  & v) { 
    x  =  v.x;
    y  =  v.y;
    z  =  v.z;
    w  =   0.0F ;
     return   * this ;
  }
   //  Assign from a vector2
  vector4                & operator   =   ( const  vector2  & v) {
    x  =  v.x;
    y  =  v.y;
    z  =   0.0F ;
    w  =   0.0F ;
     return   * this ;
  }
   //  Add a vector4 to this one
  vector4                & operator   +=  ( const  vector4  & v) {
    x  +=  v.x;
    y  +=  v.y;
    z  +=  v.z;
    w  +=  v.w;
     return   * this ;
  }
   //  Subtract a vector4 from this one
  vector4                & operator   -=  ( const  vector4  & v) {
    x  -=  v.x;
    y  -=  v.y;
    z  -=  v.z;
    w  -=  v.w;
     return   * this ;
  }
   //  Multiply the vector4 by a float
  vector4                & operator   *=  ( float  f) {
    x  *=  f;
    y  *=  f;
    z  *=  f;
    w  *=  f;
     return   * this ;
  }
   //  Divide the vector4 by a float
  vector4                & operator   /=  ( float  f) {
    x  /=  f;
    y  /=  f;
    z  /=  f;
    w  /=  f;
     return   * this ;
  }
   //  Are these two vector4's equal?
  friend  bool             operator   ==  ( const  vector4  & a,  const  vector4  & b) {
     return ((a.x  ==  b.x)  &&  (a.y  ==  b.y)  &&
        (a.z  ==  b.z)  &&  (a.w  ==  b.w));
  }
   //  Are these two vector4's not equal?
  friend  bool             operator   !=  ( const  vector4  & a,  const  vector4  & b) {
     return ((a.x  !=  b.x)  ||  (a.y  !=  b.y)  ||
        (a.z  !=  b.z)  ||  (a.w  !=  b.w));
  }
   //  Negate a vector4
  friend vector4         operator   -  ( const  vector4  & a) {
     return  vector4( - a.x,  - a.y,  - a.z,  - a.w);
  }
   //  Add two vector4's
  friend vector4         operator   +  ( const  vector4  & a,  const  vector4  & b) {
    vector4 ret(a);
    ret  +=  b;
     return  ret;
  }
   //  Subtract one vector4 from another
  friend vector4         operator   -  ( const  vector4  & a,  const  vector4  & b) {
    vector4 ret(a);
    ret  -=  b;
     return  ret;
  }
   //  Multiply vector4 by a float
  friend vector4         operator   *  ( const  vector4  & v,  float  f) {
     return  vector4(f  *  v.x, f  *  v.y, f  *  v.z, f  *  v.w);
  }
   //  Multiply vector4 by a float
  friend vector4         operator   *  ( float  f,  const  vector4  & v) {
     return  vector4(f  *  v.x, f  *  v.y, f  *  v.z, f  *  v.w);
  }
   //  Divide vector4 by a float
  friend vector4         operator   /  ( const  vector4  & v,  float  f) {
     return  vector4(v.x  /  f, v.y  /  f, v.z  /  f, v.w  /  f);
  }

public :
   //  Methods
   //  Set Values
   void                    set ( float  xIn,  float  yIn,  float  zIn,  float  wIn) {
    x  =  xIn;
    y  =  yIn;
    z  =  zIn;
    w  =  wIn;
  }
   //  Get length of a vector4
   float                  length()  const  {
     return ( float ) sqrt(x * x  +  y * y  +  z * z  +  w * w);
  }
   //  Get squared length of a vector4
   float                  lengthSqr()  const  {
     return (x * x  +  y * y  +  z * z  +  w * w);
  }
   //  Does vector4 equal (0, 0, 0, 0)?
   bool                   isZero()  const  {
     return ((x  ==   0.0F )  &&  (y  ==   0.0F )  &&  (z  ==   0.0F )  &&  (w  ==   0.0F ));
  }
   //  Normalize a vector4
  vector4                & normalize() {
     float  m  =  length();
     if  (m  >   0.0F )
      m  =   1.0F   /  m;
     else
      m  =   0.0F ;
    x  *=  m;
    y  *=  m;
    z  *=  m;
    w  *=  m;
     return   * this ;
  }

   //  Debug
   void                   fprint(FILE *  file,  char *  str)  const ;
};


////////////////////////////////////////////////////////////
//  Miscellaneous vector functions
//

vector2 Normalized( const  vector2  & a);
vector3 Normalized( const  vector3  & a);
vector4 Normalized( const  vector4  & a);
float  DotProduct( const  vector2  & a,  const  vector2  & b);
float  DotProduct( const  vector3  & a,  const  vector3  & b);
float  DotProduct( const  vector4  & a,  const  vector4  & b);
void  SwapVec(vector2  & a, vector2  & b);
void  SwapVec(vector3  & a, vector3  & b);
void  SwapVec(vector4  & a, vector4  & b);
vector3 CrossProduct( const  vector3  & a,  const  vector3  & b);
bool  NearlyEquals( const  vector2  & a,  const  vector2  & b,  float  r);
bool  NearlyEquals( const  vector3  & a,  const  vector3  & b,  float  r);
bool  NearlyEquals( const  vector4  & a,  const  vector4  & b,  float  r);


////////////////////////////////////////////////////////////
//  matrix33 class
//

class  matrix33 
{
public :
   //  Members
  vector3       col[ 3 ];

public :
   //  Constructors
  matrix33() {};
   //  Constructor with initializing value
  matrix33( float  v) {
    col[ 0 ]. set (v, v, v);
    col[ 1 ]. set (v, v, v);
    col[ 2 ]. set (v, v, v);
  }
   //  Constructor with initializing matrix33
  matrix33( const  matrix33  & m) {
    col[ 0 ]  =  m[ 0 ];
    col[ 1 ]  =  m[ 1 ];
    col[ 2 ]  =  m[ 2 ];
  }
   //  Constructor with initializing vector3's
  matrix33( const  vector3  & v0,  const  vector3  & v1,  const  vector3  & v2) {
    col[ 0 ]  =  v0;
    col[ 1 ]  =  v1;
    col[ 2 ]  =  v2;
  }

public :
   //  Operators
   //  Array indexing
  vector3        & operator  [] (unsigned  int  i) {
    assert (i < 3 );
     return (vector3 & )col[i];
  }
   //  Array indexing
   const  vector3      & operator  [] (unsigned  int  i)  const  {
    assert (i < 3 );
     return (vector3 & )col[i];
  }
   //  Assign
  matrix33       & operator   =   ( const  matrix33  & m) {
    col[ 0 ]  =  m[ 0 ];
    col[ 1 ]  =  m[ 1 ];
    col[ 2 ]  =  m[ 2 ];
     return   * this ;
  }
   //  Add a matrix33 to this one
  matrix33       & operator   +=  ( const  matrix33  & m) {
    col[ 0 ]  +=  m[ 0 ];
    col[ 1 ]  +=  m[ 1 ];
    col[ 2 ]  +=  m[ 2 ];
     return   * this ;
  }
   //  Subtract a matrix33 from this one
  matrix33       & operator   -=  ( const  matrix33  & m) {
    col[ 0 ]  -=  m[ 0 ];
    col[ 1 ]  -=  m[ 1 ];
    col[ 2 ]  -=  m[ 2 ];
     return   * this ;
  }
   //  Multiply the matrix33 by another matrix33
  matrix33       & operator   *=  ( const  matrix33  & m);
   //  Multiply the matrix33 by a float
  matrix33       & operator   *=  ( float  f) {
    col[ 0 ]  *=  f;
    col[ 1 ]  *=  f;
    col[ 2 ]  *=  f;
     return   * this ;
  }
   //  Are these two matrix33's equal?
  friend  bool         operator   ==  ( const  matrix33  & a,  const  matrix33  & b) {
     return ((a[ 0 ]  ==  b[ 0 ])  &&  (a[ 1 ]  ==  b[ 1 ])  &&  (a[ 2 ]  ==  b[ 2 ]));
  }
   //  Are these two matrix33's not equal?
  friend  bool         operator   !=  ( const  matrix33  & a,  const  matrix33  & b) {
     return ((a[ 0 ]  !=  b[ 0 ])  ||  (a[ 1 ]  !=  b[ 1 ])  ||  (a[ 2 ]  !=  b[ 2 ]));
  }
   //  Add two matrix33's
  friend matrix33    operator   +  ( const  matrix33  & a,  const  matrix33  & b) {
    matrix33 ret(a);
    ret  +=  b;
     return  ret;
  }
   //  Subtract one matrix33 from another
  friend matrix33    operator   -  ( const  matrix33  & a,  const  matrix33  & b) {
    matrix33 ret(a);
    ret  -=  b;
     return  ret;
  }
   //  Multiply matrix33 by another matrix33
  friend matrix33    operator   *  ( const  matrix33  & a,  const  matrix33  & b) {
    matrix33 ret(a);
    ret  *=  b;
     return  ret;
  }
   //  Multiply a vector3 by this matrix33
  friend vector3     operator   *  ( const  matrix33  & m,  const  vector3  & v) {
    vector3 ret;
    ret.x  =  v.x  *  m[ 0 ][ 0 ]  +  v.y  *  m[ 1 ][ 0 ]  +  v.z  *  m[ 2 ][ 0 ];
    ret.y  =  v.x  *  m[ 0 ][ 1 ]  +  v.y  *  m[ 1 ][ 1 ]  +  v.z  *  m[ 2 ][ 1 ];
    ret.z  =  v.x  *  m[ 0 ][ 2 ]  +  v.y  *  m[ 1 ][ 2 ]  +  v.z  *  m[ 2 ][ 2 ];
     return  ret;
  }
   //  Multiply a vector3 by this matrix33
  friend vector3     operator   *  ( const  vector3  & v,  const  matrix33  & m) {
    vector3 ret;
    ret.x  =  DotProduct(m[ 0 ], v);
    ret.y  =  DotProduct(m[ 1 ], v);
    ret.z  =  DotProduct(m[ 2 ], v);
     return  ret;
  }
   //  Multiply matrix33 by a float
  friend matrix33    operator   *  ( const  matrix33  & m,  float  f) {
    matrix33 ret(m);
    ret  *=  f;
     return  ret;
  }
   //  Multiply matrix33 by a float
  friend matrix33    operator   *  ( float  f,  const  matrix33  & m) {
    matrix33 ret(m);
    ret  *=  f;
     return  ret;
  }

public :
   //  Methods
   //  Set matrix33 to the identity matrix
  matrix33       & identity() {
    col[ 0 ]. set ( 1.0 ,  0.0 ,  0.0 );
    col[ 1 ]. set ( 0.0 ,  1.0 ,  0.0 );
    col[ 2 ]. set ( 0.0 ,  0.0 ,  1.0 );
     return   * this ;
  }
   //  Transpose the matrix33
  matrix33       & transpose();
   //  Invert the matrix33
  matrix33       & invert();

   //  Debug
   void           fprint(FILE *  file,  char *  str)  const ;
};

matrix33    IdentityMatrix33();
matrix33    TransposeMatrix33( const  matrix33  & m);
matrix33    InvertMatrix33( const  matrix33  & m);
matrix33    RotateRadMatrix33( float  rad);
matrix33    TranslateMatrix33( float  x,  float  y);
matrix33    ScaleMatrix33( float  x,  float  y,  float  z  =   1.0 );


////////////////////////////////////////////////////////////
//  matrix44 class
//

class  matrix44 
{
public :
   //  Members
  vector4   col[ 4 ];

public :
   //  Constructors
  matrix44() {};
   //  Constructor with initializing value
  matrix44( float  v) {
    col[ 0 ]. set (v, v, v, v);
    col[ 1 ]. set (v, v, v, v);
    col[ 2 ]. set (v, v, v, v);
    col[ 3 ]. set (v, v, v, v);
  }
   //  Constructor with initializing matrix44
  matrix44( const  matrix44  & m) {
    col[ 0 ]  =  m[ 0 ];
    col[ 1 ]  =  m[ 1 ];
    col[ 2 ]  =  m[ 2 ];
    col[ 3 ]  =  m[ 3 ];
  }
   //  Constructor with initializing vector4's
  matrix44( const  vector4  & v0,  const  vector4  & v1, 
       const  vector4  & v2,  const  vector4  & v3) {
    col[ 0 ]  =  v0;
    col[ 1 ]  =  v1;
    col[ 2 ]  =  v2;
    col[ 3 ]  =  v3;
  }
   //  Constructor with initializing matrix33
   explicit  matrix44( const  matrix33  & m) {
    col[ 0 ]  =  m[ 0 ];
    col[ 1 ]  =  m[ 1 ];
    col[ 2 ]  =  m[ 2 ];
    col[ 3 ]. set ( 0.0 ,  0.0 ,  0.0 ,  1.0 );
  }

public :
   //  Operators
   //  Array indexing
  vector4        & operator  [] (unsigned  int  i) {
    assert (i < 4 );
     return  col[i];
  }
   //  Array indexing
   const  vector4      & operator  [] (unsigned  int  i)  const  {
    assert (i < 4 );
     return  col[i];
  }
   //  Assign
  matrix44       & operator   =   ( const  matrix44  & m) {
    col[ 0 ]  =  m[ 0 ];
    col[ 1 ]  =  m[ 1 ];
    col[ 2 ]  =  m[ 2 ];
    col[ 3 ]  =  m[ 3 ];
     return   * this ;
  }
   //  Assign a matrix33 to the matrix44
  matrix44       & operator   =   ( const  matrix33  & m) {
    col[ 0 ]  =  m[ 0 ];
    col[ 1 ]  =  m[ 1 ];
    col[ 2 ]  =  m[ 2 ];
    col[ 3 ]. set ( 0.0 ,  0.0 ,  0.0 ,  1.0 );
     return   * this ;
  }
   //  Add a matrix44 to this one
  matrix44       & operator   +=  ( const  matrix44  & m) {
    col[ 0 ]  +=  m[ 0 ];
    col[ 1 ]  +=  m[ 1 ];
    col[ 2 ]  +=  m[ 2 ];
    col[ 3 ]  +=  m[ 3 ];
     return   * this ;
  }
   //  Subtract a matrix44 from this one
  matrix44       & operator   -=  ( const  matrix44  & m) {
    col[ 0 ]  -=  m[ 0 ];
    col[ 1 ]  -=  m[ 1 ];
    col[ 2 ]  -=  m[ 2 ];
    col[ 3 ]  -=  m[ 3 ];
     return   * this ;
  }
   //  Multiply the matrix44 by another matrix44
  matrix44       & operator   *=  ( const  matrix44  & m);
   //  Multiply the matrix44 by a float
  matrix44       & operator   *=  ( float  f) {
    col[ 0 ]  *=  f;
    col[ 1 ]  *=  f;
    col[ 2 ]  *=  f;
    col[ 3 ]  *=  f;
     return   * this ;
  }
   // matrix44      &operator /= (float f) {}
   //  Are these two matrix44's equal?
  friend  bool         operator   ==  ( const  matrix44  & a,  const  matrix44  & b) {
     return ((a[ 0 ]  ==  b[ 0 ])  &&  (a[ 1 ]  ==  b[ 1 ])  &&
        (a[ 2 ]  ==  b[ 2 ])  &&  (a[ 3 ]  ==  b[ 3 ]));
  }
   //  Are these two matrix44's not equal?
  friend  bool         operator   !=  ( const  matrix44  & a,  const  matrix44  & b) {
     return ((a[ 0 ]  !=  b[ 0 ])  ||  (a[ 1 ]  !=  b[ 1 ])  ||
        (a[ 2 ]  !=  b[ 2 ])  ||  (a[ 3 ]  !=  b[ 3 ]));
  }
   //  Add two matrix44's
  friend matrix44    operator   +  ( const  matrix44  & a,  const  matrix44  & b) {
    matrix44 ret(a);
    ret  +=  b;
     return  ret;
  }
   //  Subtract one matrix44 from another
  friend matrix44    operator   -  ( const  matrix44  & a,  const  matrix44  & b) {
    matrix44 ret(a);
    ret  -=  b;
     return  ret;
  }
   //  Multiply matrix44 by another matrix44
  friend matrix44    operator   *  ( const  matrix44  & a,  const  matrix44  & b) {
    matrix44 ret(a);
    ret  *=  b;
     return  ret;
  }
   //  Multiply a vector3 by this matrix44
  friend vector3     operator   *  ( const  matrix44  & m,  const  vector3  & v) {
    vector4 ret(v);
    ret  =  m  *  ret;
     return  vector3(ret.x, ret.y, ret.z);
  }
   //  Multiply a vector3 by this matrix44
  friend vector3     operator   *  ( const  vector3  & v,  const  matrix44  & m) {
    vector4 ret(v);
    ret  =  ret  *  m;
     return  vector3(ret.x, ret.y, ret.z);
  }
   //  Multiply a vector4 by this matrix44
  friend vector4     operator   *  ( const  matrix44  & m,  const  vector4  & v) {
    vector4 ret;
    ret.x  =  v.x  *  m[ 0 ][ 0 ]  +  v.y  *  m[ 1 ][ 0 ]  +  v.z  *  m[ 2 ][ 0 ]  +  v.w  *  m[ 3 ][ 0 ];
    ret.y  =  v.x  *  m[ 0 ][ 1 ]  +  v.y  *  m[ 1 ][ 1 ]  +  v.z  *  m[ 2 ][ 1 ]  +  v.w  *  m[ 3 ][ 1 ];
    ret.z  =  v.x  *  m[ 0 ][ 2 ]  +  v.y  *  m[ 1 ][ 2 ]  +  v.z  *  m[ 2 ][ 2 ]  +  v.w  *  m[ 3 ][ 2 ];
    ret.w  =  v.x  *  m[ 0 ][ 3 ]  +  v.y  *  m[ 1 ][ 3 ]  +  v.z  *  m[ 2 ][ 3 ]  +  v.w  *  m[ 3 ][ 3 ];
     return  ret;
  }
   //  Multiply a vector4 by this matrix44
  friend vector4     operator   *  ( const  vector4  & v,  const  matrix44  & m) {
    vector4 ret;
    ret.x  =  DotProduct(m[ 0 ], v);
    ret.y  =  DotProduct(m[ 1 ], v);
    ret.z  =  DotProduct(m[ 2 ], v);
    ret.w  =  DotProduct(m[ 3 ], v);
     return  ret;
  }
   //  Multiply matrix44 by a float
  friend matrix44    operator   *  ( const  matrix44  & m,  float  f) {
    matrix44 ret(m);
    ret  *=  f;
     return  ret;
  }
   //  Set matrix44 to the identity matrix
  friend matrix44    operator   *  ( float  f,  const  matrix44  & m) {
    matrix44 ret(m);
    ret  *=  f;
     return  ret;
  }

public :
   //  Methods
   //  Set matrix44 to the identity matrix
  matrix44       & identity() {
    col[ 0 ]. set ( 1.0 ,  0.0 ,  0.0 ,  0.0 );
    col[ 1 ]. set ( 0.0 ,  1.0 ,  0.0 ,  0.0 );
    col[ 2 ]. set ( 0.0 ,  0.0 ,  1.0 ,  0.0 );
    col[ 3 ]. set ( 0.0 ,  0.0 ,  0.0 ,  1.0 );
     return   * this ;
  }
   //  Transpose the matrix44
  matrix44       & transpose();
   //  Invert the matrix44
  matrix44       & invert();

   //  Debug
   void           fprint(FILE *  file,  char *  str)  const ;
};

matrix44    IdentityMatrix44();
matrix44    TransposeMatrix44( const  matrix44  & m);
matrix44    InvertMatrix44( const  matrix44  & m);
matrix44    RotateRadMatrix44( char  axis,  float  rad);
matrix44    RotateRadMatrix44( const  vector3  & axis,  float  rad);
matrix44    TranslateMatrix44( float  x,  float  y,  float  z);
matrix44    ScaleMatrix44( float  x,  float  y,  float  z,  float  w  =   1.0 );
matrix44    LookAtMatrix44( const  vector3  & camPos,  const  vector3  & camUp, 
     const  vector3  & target );
matrix44    FrustumMatrix44( float  l,  float  r,  float  b,  float  t, 
     float  n,  float  f);
matrix44    PerspectiveMatrix44( float  fovY,  float  aspect, 
     float  n,  float  f);
matrix44    OrthoMatrix44( float  l,  float  r,  float  b,  float  t, 
     float  n,  float  f);
matrix44    OrthoNormalMatrix44( const  vector3  & xdir, 
     const  vector3  & ydir,  const  vector3  & zdir);

#endif

mtxlib.cpp

/*  Copyright (C) Dante Treglia II and Mark A. DeLoura, 2000. 
 * All rights reserved worldwide.
 *
 * This software is provided "as is" without express or implied
 * warranties. You may freely copy and compile this source into
 * applications you distribute provided that the copyright text
 * below is included in the resulting source code, for example:
 * "Portions Copyright (C) Dante Treglia II and Mark A. DeLoura, 2000"
  */
// ==========================================================
//  C++ Matrix Library
//  Version: 2.6
//  Date: May 19, 2000
//  Authors: Dante Treglia II and Mark A. DeLoura
//  Thanks to: Miguel Gomez, Stan Melax, Pete Isensee, 
//    Gabor Nagy, Scott Bilas, James Boer, Eric Lengyel
// ==========================================================
#include  " mtxlib.h "
#include  < cmath >
#include  < cassert >

////////////////////////////////////////////////////////////
//  Miscellaneous vector functions
//

//  Return Normal of vector2's
vector2 Normalized( const  vector2  & a)
{
  vector2 ret(a);
   return  ret.normalize();
}
//  Return Normal of vector3's
vector3 Normalized( const  vector3  & a)
{
  vector3 ret(a);
   return  ret.normalize();
}
//  Return Normal of vector4's
vector4 Normalized( const  vector4  & a)
{
  vector4 ret(a);
   return  ret.normalize();
}

//  Dot product of two vector2's
float  DotProduct( const  vector2  & a,  const  vector2  & b) 
{
   return  a.x * b.x  +  a.y * b.y;
}

//  Dot product of two vector3's
float  DotProduct( const  vector3  & a,  const  vector3  & b) 
{
   return  a.x * b.x  +  a.y * b.y  +  a.z * b.z;
}

//  Dot product of two vector4's
float  DotProduct( const  vector4  & a,  const  vector4  & b) 
{
   return  a.x * b.x  +  a.y * b.y  +  a.z * b.z  +  a.w * b.w;
}

//  Swap two vector2's
void  SwapVec(vector2  & a, vector2  & b) 
{
  vector2 tmp(a);

  a  =  b;
  b  =  tmp;
}

//  Swap two vector3's
void  SwapVec(vector3  & a, vector3  & b) 
{
  vector3 tmp(a);

  a  =  b;
  b  =  tmp;
}

//  Swap two vector4's
void  SwapVec(vector4  & a, vector4  & b) 
{
  vector4 tmp(a);

  a  =  b;
  b  =  tmp;
}

//  Cross product of two vector3's
vector3 CrossProduct( const  vector3  & a,  const  vector3  & b) 
{
   return  vector3(a.y * b.z  -  a.z * b.y,
      a.z * b.x  -  a.x * b.z,
      a.x * b.y  -  a.y * b.x);
}

//  Are these two vector2's nearly equal?
bool  NearlyEquals(  const  vector2 &  a,  const  vector2 &  b,  float  r ) 
{
  vector2 diff  =  a  -  b;   //  difference

   return (DotProduct(diff, diff)  <  r * r);   //  radius
}

//  Are these two vector3's nearly equal?
bool  NearlyEquals(  const  vector3 &  a,  const  vector3 &  b,  float  r ) 
{
  vector3 diff  =  a  -  b;   //  difference

   return (DotProduct(diff, diff)  <  r * r);   //  radius
}

//  Are these two vector4's nearly equal?
bool  NearlyEquals(  const  vector4 &  a,  const  vector4 &  b,  float  r ) 
{
  vector4 diff  =  a  -  b;   //  difference

   return (DotProduct(diff, diff)  <  r * r);   //  radius
}


////////////////////////////////////////////////////////////
//  matrix33 class
//

//  Multiply the matrix33 by another matrix33
matrix33  & matrix33:: operator   *=  ( const  matrix33  & m) 
{
  matrix33 t;

   for  (unsigned  int  r  =   0 ; r  <   3 ; r ++ )
  {
   for  (unsigned  int  c  =   0 ; c  <   3 ; c ++ )
  {
     float  f  =   0 ;

    f  +=  col[ 0 ][r]  *  m[c][ 0 ];
    f  +=  col[ 1 ][r]  *  m[c][ 1 ];
    f  +=  col[ 2 ][r]  *  m[c][ 2 ];

    t[c][r]  =  f;
  }
  }

   * this   =  t;

   return   * this ;
}

//  Transpose the matrix33
matrix33  & matrix33::transpose() 
{
   float  t;

   for  (unsigned  int  c  =   0 ; c  <   3 ; c ++ )
  {
   for  (unsigned  int  r  =  c  +   1 ; r  <   3 ; r ++ )
  {
    t  =  col[c][r];
    col[c][r]  =  col[r][c];
    col[r][c]  =  t;
  } 
  }

   return   * this ;
}

//  Invert the matrix33
matrix33  & matrix33::invert() 
{
  matrix33 a( * this );
  matrix33 b(IdentityMatrix33());

  unsigned  int  c, r;
  unsigned  int  cc;
  unsigned  int  rowMax;  //  Points to max abs value row in this column
  unsigned  int  row;
   float  tmp;

   //  Go through columns
   for  (c = 0 ; c < 3 ; c ++ )
  {
   //  Find the row with max value in this column
  rowMax  =  c;
   for  (r = c + 1 ; r < 3 ; r ++ )
  {
     if  (fabs(a[c][r])  >  fabs(a[c][rowMax]))
    {
    rowMax  =  r;
    }
  }

   //  If the max value here is 0, we can't invert.  Return identity.
   if  (a[rowMax][c]  ==   0.0F )
     return (identity());

   //  Swap row "rowMax" with row "c"
   for  (cc = 0 ; cc < 3 ; cc ++ )
  {
    tmp  =  a[cc][c];
    a[cc][c]  =  a[cc][rowMax];
    a[cc][rowMax]  =  tmp;
    tmp  =  b[cc][c];
    b[cc][c]  =  b[cc][rowMax];
    b[cc][rowMax]  =  tmp;
  }

   //  Now everything we do is on row "c".
   //  Set the max cell to 1 by dividing the entire row by that value
  tmp  =  a[c][c];
   for  (cc = 0 ; cc < 3 ; cc ++ )
  {
    a[cc][c]  /=  tmp;
    b[cc][c]  /=  tmp;
  }

   //  Now do the other rows, so that this column only has a 1 and 0's
   for  (row  =   0 ; row  <   3 ; row ++ )
  {
     if  (row  !=  c)
    {
    tmp  =  a[c][row];
     for  (cc = 0 ; cc < 3 ; cc ++ )
    {
      a[cc][row]  -=  a[cc][c]  *  tmp;
      b[cc][row]  -=  b[cc][c]  *  tmp;
    }
    }
  }

  }

   * this   =  b;

   return   * this ;
}

//  Return a matrix33 set to the identity matrix
matrix33 IdentityMatrix33() 
{
  matrix33 ret;

   return  ret.identity();
}

//  Return the transpose of the matrix33
matrix33 TransposeMatrix33( const  matrix33  & m) 
{
  matrix33 ret(m);

   return  ret.transpose();
}

//  Return the inverted matrix33
matrix33 InvertMatrix33( const  matrix33  & m) 
{
  matrix33 ret(m);

   return  ret.invert();
}

//  Return a 2D rotation matrix33
matrix33 RotateRadMatrix33( float  rad) 
{
  matrix33 ret;
   float  sinA, cosA;

  sinA  =  ( float )sin(rad);
  cosA  =  ( float )cos(rad);

  ret[ 0 ][ 0 ]  =  cosA; ret[ 1 ][ 0 ]  =   - sinA; ret[ 2 ][ 0 ]  =   0.0F ;
  ret[ 0 ][ 1 ]  =  sinA; ret[ 1 ][ 1 ]  =   cosA; ret[ 2 ][ 1 ]  =   0.0F ;
  ret[ 0 ][ 2 ]  =   0.0F ; ret[ 1 ][ 2 ]  =    0.0F ; ret[ 2 ][ 2 ]  =   1.0F ;

   return  ret;
}

//  Return a 2D translation matrix33
matrix33 TranslateMatrix33( float  x,  float  y) 
{
  matrix33 ret;

  ret.identity();
  ret[ 2 ][ 0 ]  =  x;
  ret[ 2 ][ 1 ]  =  y;

   return  ret;
}

//  Return a 2D/3D scale matrix33
matrix33 ScaleMatrix33( float  x,  float  y,  float  z) 
{
  matrix33 ret;

  ret.identity();
  ret[ 0 ][ 0 ]  =  x;
  ret[ 1 ][ 1 ]  =  y;
  ret[ 2 ][ 2 ]  =  z;

   return  ret;
}


////////////////////////////////////////////////////////////
//  matrix44 class
//

//  Multiply the matrix44 by another matrix44
matrix44  & matrix44:: operator   *=  ( const  matrix44  & m) 
{
  matrix44 t;
   for  (unsigned  int  r  =   0 ; r  <   4 ; r ++ )
  {
   for  (unsigned  int  c  =   0 ; c  <   4 ; c ++ )
  {
     float  f  =   0 ;

    f  +=  (col[ 0 ][r]  *  m[c][ 0 ]);
    f  +=  (col[ 1 ][r]  *  m[c][ 1 ]);
    f  +=  (col[ 2 ][r]  *  m[c][ 2 ]);
    f  +=  (col[ 3 ][r]  *  m[c][ 3 ]);

    t[c][r]  =  f;
  }
  }
   * this   =  t;
   return   * this ;
}


//  Transpose the matrix44
matrix44  & matrix44::transpose() 
{
   float  t;

   for  (unsigned  int  c  =   0 ; c  <   4 ; c ++ )
  {
   for  (unsigned  int  r  =  c  +   1 ; r  <   4 ; r ++ )
  {
    t  =  col[c][r];
    col[c][r]  =  col[r][c];
    col[r][c]  =  t;
  } 
  } 

   return   * this ;
}

//  Invert the matrix44
matrix44  & matrix44::invert() 
{
  matrix44 a( * this );
  matrix44 b(IdentityMatrix44());

  unsigned  int  r, c;
  unsigned  int  cc;
  unsigned  int  rowMax;  //  Points to max abs value row in this column
  unsigned  int  row;
   float  tmp;

   //  Go through columns
   for  (c = 0 ; c < 4 ; c ++ )
  {

   //  Find the row with max value in this column
  rowMax  =  c;
   for  (r = c + 1 ; r < 4 ; r ++ )
  {
     if  (fabs(a[c][r])  >  fabs(a[c][rowMax]))
    {
    rowMax  =  r;
    }
  }

   //  If the max value here is 0, we can't invert.  Return identity.
   if  (a[rowMax][c]  ==   0.0F )
     return (identity());

   //  Swap row "rowMax" with row "c"
   for  (cc = 0 ; cc < 4 ; cc ++ )
  {
    tmp  =  a[cc][c];
    a[cc][c]  =  a[cc][rowMax];
    a[cc][rowMax]  =  tmp;
    tmp  =  b[cc][c];
    b[cc][c]  =  b[cc][rowMax];
    b[cc][rowMax]  =  tmp;
  }

   //  Now everything we do is on row "c".
   //  Set the max cell to 1 by dividing the entire row by that value
  tmp  =  a[c][c];
   for  (cc = 0 ; cc < 4 ; cc ++ )
  {
    a[cc][c]  /=  tmp;
    b[cc][c]  /=  tmp;
  }

   //  Now do the other rows, so that this column only has a 1 and 0's
   for  (row  =   0 ; row  <   4 ; row ++ )
  {
     if  (row  !=  c)
    {
    tmp  =  a[c][row];
     for  (cc = 0 ; cc < 4 ; cc ++ )
    {
      a[cc][row]  -=  a[cc][c]  *  tmp;
      b[cc][row]  -=  b[cc][c]  *  tmp;
    }
    }
  }

  }

   * this   =  b;

   return   * this ;
}

//  Return a matrix44 set to the identity matrix
matrix44 IdentityMatrix44() 
{
  matrix44 ret;

   return  ret.identity();
}

//  Return the transpose of the matrix44
matrix44 TransposeMatrix44( const  matrix44  & m) 
{
  matrix44 ret(m);

   return  ret.transpose();
}

//  Return the inverted matrix44
matrix44 InvertMatrix44( const  matrix44  & m) 
{
  matrix44 ret(m);

   return  ret.invert();
}

//  Return a 3D axis-rotation matrix44
//  Pass in 'x', 'y', or 'z' for the axis.
matrix44 RotateRadMatrix44( char  axis,  float  rad) 
{
  matrix44 ret;
   float  sinA, cosA;

  sinA  =  ( float )sin(rad);
  cosA  =  ( float )cos(rad);

   switch  (axis)
  {
   case   ' x ' :
   case   ' X ' :
    ret[ 0 ][ 0 ]  =    1.0F ; ret[ 1 ][ 0 ]  =    0.0F ; ret[ 2 ][ 0 ]  =    0.0F ;
    ret[ 0 ][ 1 ]  =    0.0F ; ret[ 1 ][ 1 ]  =   cosA; ret[ 2 ][ 1 ]  =   - sinA;
    ret[ 0 ][ 2 ]  =    0.0F ; ret[ 1 ][ 2 ]  =   sinA; ret[ 2 ][ 2 ]  =   cosA;
     break ;

   case   ' y ' :
   case   ' Y ' :
    ret[ 0 ][ 0 ]  =   cosA; ret[ 1 ][ 0 ]  =    0.0F ; ret[ 2 ][ 0 ]  =   sinA;
    ret[ 0 ][ 1 ]  =    0.0F ; ret[ 1 ][ 1 ]  =    1.0F ; ret[ 2 ][ 1 ]  =    0.0F ;
    ret[ 0 ][ 2 ]  =   - sinA; ret[ 1 ][ 2 ]  =    0.0F ; ret[ 2 ][ 2 ]  =   cosA;
     break ;

   case   ' z ' :
   case   ' Z ' :
    ret[ 0 ][ 0 ]  =   cosA; ret[ 1 ][ 0 ]  =   - sinA; ret[ 2 ][ 0 ]  =    0.0F ;
    ret[ 0 ][ 1 ]  =   sinA; ret[ 1 ][ 1 ]  =   cosA; ret[ 2 ][ 1 ]  =    0.0F ;
    ret[ 0 ][ 2 ]  =    0.0F ; ret[ 1 ][ 2 ]  =    0.0F ; ret[ 2 ][ 2 ]  =    1.0F ;
     break ;
  }

  ret[ 0 ][ 3 ]  =   0.0F ; ret[ 1 ][ 3 ]  =   0.0F ; ret[ 2 ][ 3 ]  =   0.0F ;
  ret[ 3 ][ 0 ]  =   0.0F ;
  ret[ 3 ][ 1 ]  =   0.0F ;
  ret[ 3 ][ 2 ]  =   0.0F ;
  ret[ 3 ][ 3 ]  =   1.0F ;

   return  ret;
}

//  Return a 3D axis-rotation matrix44
//  Pass in an arbitrary vector3 axis.
matrix44 RotateRadMatrix44( const  vector3  & axis,  float  rad) 
{
  matrix44 ret;
   float  sinA, cosA;
   float  invCosA;
  vector3 nrm  =  axis;
   float  x, y, z;
   float  xSq, ySq, zSq;

  nrm.normalize();
  sinA  =  ( float )sin(rad);
  cosA  =  ( float )cos(rad);
  invCosA  =   1.0F   -  cosA;

  x  =  nrm.x;
  y  =  nrm.y;
  z  =  nrm.z;

  xSq  =  x  *  x;
  ySq  =  y  *  y;
  zSq  =  z  *  z;

  ret[ 0 ][ 0 ]  =  (invCosA  *  xSq)  +  (cosA);
  ret[ 1 ][ 0 ]  =  (invCosA  *  x  *  y)  -  (sinA  *  z );
  ret[ 2 ][ 0 ]  =  (invCosA  *  x  *  z)  +  (sinA  *  y );
  ret[ 3 ][ 0 ]  =   0.0F ;

  ret[ 0 ][ 1 ]  =  (invCosA  *  x  *  y)  +  (sinA  *  z);
  ret[ 1 ][ 1 ]  =  (invCosA  *  ySq)  +  (cosA);
  ret[ 2 ][ 1 ]  =  (invCosA  *  y  *  z)  -  (sinA  *  x);
  ret[ 3 ][ 1 ]  =   0.0F ;

  ret[ 0 ][ 2 ]  =  (invCosA  *  x  *  z)  -  (sinA  *  y);
  ret[ 1 ][ 2 ]  =  (invCosA  *  y  *  z)  +  (sinA  *  x);
  ret[ 2 ][ 2 ]  =  (invCosA  *  zSq)  +  (cosA);
  ret[ 3 ][ 2 ]  =   0.0F ;

  ret[ 0 ][ 3 ]  =   0.0F ;
  ret[ 1 ][ 3 ]  =   0.0F ;
  ret[ 2 ][ 3 ]  =   0.0F ;
  ret[ 3 ][ 3 ]  =   1.0F ;

   return  ret;
}

//  Return a 3D translation matrix44
matrix44 TranslateMatrix44( float  x,  float  y,  float  z) 
{
  matrix44 ret;

  ret.identity();
  ret[ 3 ][ 0 ]  =  x;
  ret[ 3 ][ 1 ]  =  y;
  ret[ 3 ][ 2 ]  =  z;

   return  ret;
}

//  Return a 3D/4D scale matrix44
matrix44 ScaleMatrix44( float  x,  float  y,  float  z,  float  w) 
{
  matrix44 ret;

  ret.identity();
  ret[ 0 ][ 0 ]  =  x;
  ret[ 1 ][ 1 ]  =  y;
  ret[ 2 ][ 2 ]  =  z;
  ret[ 3 ][ 3 ]  =  w;

   return  ret;
}

//  Return a "lookat" matrix44 given the current camera position (vector3),
//    camera-up vector3, and camera-target vector3.
matrix44 LookAtMatrix44( const  vector3  & camPos,  const  vector3  & target, 
     const  vector3  & camUp ) 
{
  matrix44 ret;

  vector3 F  =  target  -  camPos;
  F.normalize();

  vector3 S  =  CrossProduct(F, Normalized(camUp));
  S.normalize();

  vector3 U  =  CrossProduct(S, F);
  U.normalize();

  ret[ 0 ][ 0 ]  =  S.x;
  ret[ 1 ][ 0 ]  =  S.y;
  ret[ 2 ][ 0 ]  =  S.z;
  ret[ 3 ][ 0 ]  =   0.0 ;

  ret[ 0 ][ 1 ]  =  U.x;
  ret[ 1 ][ 1 ]  =  U.y;
  ret[ 2 ][ 1 ]  =  U.z;
  ret[ 3 ][ 1 ]  =   0.0 ;

  ret[ 0 ][ 2 ]  =   - F.x;
  ret[ 1 ][ 2 ]  =   - F.y;
  ret[ 2 ][ 2 ]  =   - F.z;
  ret[ 3 ][ 2 ]  =   0.0 ;

  ret[ 0 ][ 3 ]  =   0.0F ;
  ret[ 1 ][ 3 ]  =   0.0F ;
  ret[ 2 ][ 3 ]  =   0.0F ;
  ret[ 3 ][ 3 ]  =   1.0F ;

  ret  *=  TranslateMatrix44( - camPos.x,  - camPos.y,  - camPos.z);

   return  ret;
}

//  Return a frustum matrix44 given the left, right, bottom, top,
//    near, and far values for the frustum boundaries.
matrix44 FrustumMatrix44( float  l,  float  r, 
     float  b,  float  t,  float  n,  float  f) 
{
  matrix44 ret;
   float  width  =  r - l;
   float  height  =  t - b;
   float  depth  =  f - n;

  ret[ 0 ][ 0 ]  =  ( 2 * n)  /  width;
  ret[ 0 ][ 1 ]  =   0.0F ;
  ret[ 0 ][ 2 ]  =   0.0F ;
  ret[ 0 ][ 3 ]  =   0.0F ;

  ret[ 1 ][ 0 ]  =   0.0F ;
  ret[ 1 ][ 1 ]  =  ( 2 * n)  /  height;
  ret[ 1 ][ 2 ]  =   0.0F ;
  ret[ 1 ][ 3 ]  =   0.0F ;

  ret[ 2 ][ 0 ]  =  (r  +  l)  /  width;
  ret[ 2 ][ 1 ]  =  (t  +  b)  /  height;
  ret[ 2 ][ 2 ]  =   - (f  +  n)  /  depth;
  ret[ 2 ][ 3 ]  =   - 1.0F ;

  ret[ 3 ][ 0 ]  =   0.0F ;
  ret[ 3 ][ 1 ]  =   0.0F ;
  ret[ 3 ][ 2 ]  =   - ( 2 * f * n)  /  depth;
  ret[ 3 ][ 3 ]  =   0.0F ;

   return  ret;
}

//  Return a perspective matrix44 given the field-of-view in the Y
//    direction in degrees, the aspect ratio of Y/X, and near and
//    far plane distances.
matrix44 PerspectiveMatrix44( float  fovY,  float  aspect,  float  n,  float  f) 
{
  matrix44 ret;
   float  angle;
   float  cot;

  angle  =  fovY  /   2.0F ;
  angle  =  DegToRad( angle );

  cot  =  ( float ) cos(angle)  /  ( float ) sin(angle);

  ret[ 0 ][ 0 ]  =  cot  /  aspect;
  ret[ 0 ][ 1 ]  =   0.0F ;
  ret[ 0 ][ 2 ]  =   0.0F ;
  ret[ 0 ][ 3 ]  =   0.0F ;

  ret[ 1 ][ 0 ]  =   0.0F ;
  ret[ 1 ][ 1 ]  =  cot;
  ret[ 1 ][ 2 ]  =   0.0F ;
  ret[ 1 ][ 3 ]  =   0.0F ;

  ret[ 2 ][ 0 ]  =   0.0F ;
  ret[ 2 ][ 1 ]  =   0.0F ;
  ret[ 2 ][ 2 ]  =   - (f  +  n)  /  (f  -  n);
  ret[ 2 ][ 3 ]  =   - 1.0F ;


  ret[ 3 ][ 0 ]  =   0.0F ;
  ret[ 3 ][ 1 ]  =   0.0F ;
  ret[ 3 ][ 2 ]  =   - ( 2 * f * n)  /  (f  -  n);
  ret[ 3 ][ 3 ]  =   0.0F ;

   return  ret;
}

//  Return an orthographic matrix44 given the left, right, bottom, top,
//    near, and far values for the frustum boundaries.
matrix44 OrthoMatrix44( float  l,  float  r, 
     float  b,  float  t,  float  n,  float  f) 
{
  matrix44 ret;
   float  width  =  r - l;
   float  height  =  t - b;
   float  depth  =  f - n;

  ret[ 0 ][ 0 ]  =   2.0F   /  width;
  ret[ 0 ][ 1 ]  =   0.0F ;
  ret[ 0 ][ 2 ]  =   0.0F ;
  ret[ 0 ][ 3 ]  =   0.0F ;

  ret[ 1 ][ 0 ]  =   0.0F ;
  ret[ 1 ][ 1 ]  =   2.0F   /  height;
  ret[ 1 ][ 2 ]  =   0.0F ;
  ret[ 1 ][ 3 ]  =   0.0F ;

  ret[ 2 ][ 0 ]  =   0.0F ;
  ret[ 2 ][ 1 ]  =   0.0F ;
  ret[ 2 ][ 2 ]  =   - ( 2.0F )  /  depth;
  ret[ 2 ][ 3 ]  =   0.0F ;

  ret[ 3 ][ 0 ]  =   - (r  +  l)  /  width;
  ret[ 1 ][ 3 ]  =   - (t  +  b)  /  height;
  ret[ 3 ][ 2 ]  =   - (f  +  n)  /  depth;
  ret[ 3 ][ 3 ]  =   1.0F ;

   return  ret;
}

//  Return an orientation matrix using 3 basis normalized vectors
matrix44    OrthoNormalMatrix44( const  vector3  & xdir, 
     const  vector3  & ydir,  const  vector3  & zdir)
{
  matrix44 ret;

  ret[ 0 ]  =  (vector4)xdir;
  ret[ 1 ]  =  (vector4)ydir;
  ret[ 2 ]  =  (vector4)zdir;
  ret[ 3 ]. set ( 0.0 ,  0.0 ,  0.0 ,  1.0 );

   return  ret;
}


////////////////////////////////////////////////////////////
//  Debug functions
//

//  Print a vector2 to a file
void  vector2::fprint(FILE *  file,  char *  str)  const  
{
  fprintf(file,  " %svector2: <%f, %f>\n " , str, x, y);
}

//  Print a vector3 to a file
void  vector3::fprint(FILE *  file,  char *  str)  const  
{
  fprintf(file,  " %svector3: <%f, %f, %f>\n " , str, x, y, z);
}

//  Print a vector4 to a file
void  vector4::fprint(FILE *  file,  char *  str)  const  
{
  fprintf(file,  " %svector4: <%f, %f, %f, %f>\n " , str, x, y, z, w);
}

//  Print a matrix33 to a file
void  matrix33::fprint(FILE *  file,  char   *  str)  const  
{
  fprintf(file,  " %smatrix33:\n " , str);
  vector3 row0(col[ 0 ][ 0 ], col[ 1 ][ 0 ], col[ 2 ][ 0 ]);
  row0.fprint(file,  " \t " );
  vector3 row1(col[ 0 ][ 1 ], col[ 1 ][ 1 ], col[ 2 ][ 1 ]);
  row1.fprint(file,  " \t " );
  vector3 row2(col[ 0 ][ 2 ], col[ 1 ][ 2 ], col[ 2 ][ 2 ]);
  row2.fprint(file,  " \t " );
}

//  Print a matrix44 to a file
void  matrix44::fprint(FILE *  file,  char *  str)  const  
{
  fprintf(file,  " %smatrix44:\n " , str);
  vector4 row0(col[ 0 ][ 0 ], col[ 1 ][ 0 ], col[ 2 ][ 0 ], col[ 3 ][ 0 ]);
  row0.fprint(file,  " \t " );
  vector4 row1(col[ 0 ][ 1 ], col[ 1 ][ 1 ], col[ 2 ][ 1 ], col[ 3 ][ 1 ]);
  row1.fprint(file,  " \t " );
  vector4 row2(col[ 0 ][ 2 ], col[ 1 ][ 2 ], col[ 2 ][ 2 ], col[ 3 ][ 2 ]);
  row2.fprint(file,  " \t " );
  vector4 row3(col[ 0 ][ 3 ], col[ 1 ][ 3 ], col[ 2 ][ 3 ], col[ 3 ][ 3 ]);
  row3.fprint(file,  " \t " );
}

Posted on 2009-09-08 10:46 zyb_debug 阅读(1348) 评论(4)   编辑  收藏 引用