mtxlib.cpp

游戏编程精华01－含有几十个游戏编程例子

/* 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;