t3dlib4.cpp

3D游戏编程大师技巧第八章的源代码

{
// this function multiplies two 2x2 matrices together and 
// and stores the result in mprod
mprod->M00 = ma->M00*mb->M00 + ma->M01*mb->M10;
mprod->M01 = ma->M00*mb->M01 + ma->M01*mb->M11;

mprod->M10 = ma->M10*mb->M00 + ma->M11*mb->M10;
mprod->M11 = ma->M10*mb->M01 + ma->M11*mb->M11;

} // end Mat_Mul_2X2

/////////////////////////////////////////////////////////////////

int Mat_Inverse_2X2(MATRIX2X2_PTR m, MATRIX2X2_PTR mi)
{
// this function computes the inverse of a 2x2 matrix
// and stores the result in mi

// compute determinate
float det = (m->M00*m->M11 - m->M01*m->M10);

// if determinate is 0 then inverse doesn't exist
if (fabs(det) < EPSILON_E5)
   return(0);

float det_inv = 1.0/det;

// fill in inverse by formula
mi->M00 =  m->M11*det_inv;
mi->M01 = -m->M01*det_inv;
mi->M10 = -m->M10*det_inv;
mi->M11 =  m->M00*det_inv;

// return sucess
return(1);

} // end Mat_Inverse_2X2

/////////////////////////////////////////////////////////////////

void Print_Mat_2X2(MATRIX2X2_PTR ma, char *name="M")
{
// prints out a 3x3 matrix
Write_Error("\n%s=\n",name);

for (int r=0; r < 2; r++, Write_Error("\n"))
    for (int c=0; c < 2; c++)
        Write_Error("%f ",ma->M[r][c]);        

} // end Print_Mat_2X2

//////////////////////////////////////////////////////////////////

float Mat_Det_2X2(MATRIX2X2_PTR m)
{
// computes the determinate of a 2x2 matrix

return(m->M00*m->M11 - m->M01*m->M10);
} // end Mat_Det_2X2

///////////////////////////////////////////////////////////////

int Solve_2X2_System(MATRIX2X2_PTR A, MATRIX1X2_PTR X, MATRIX1X2_PTR B)
{
// solves the system AX=B and computes X=A(-1)*B
// by using cramers rule and determinates

// step 1: compute determinate of A
float det_A = Mat_Det_2X2(A);

// test if det(a) is zero, if so then there is no solution
if (fabs(det_A) < EPSILON_E5)
   return(0);

// step 2: create x,y numerator matrices by taking A and
// replacing each column of it with B(transpose) and solve
MATRIX2X2 work_mat; // working matrix

// solve for x /////////////////

// copy A into working matrix
MAT_COPY_2X2(A, &work_mat);

// swap out column 0 (x column)
MAT_COLUMN_SWAP_2X2(&work_mat, 0, B);

// compute determinate of A with B swapped into x column
float det_ABx = Mat_Det_2X2(&work_mat);

// now solve for X00
X->M00 = det_ABx/det_A;

// solve for y /////////////////

// copy A into working matrix
MAT_COPY_2X2(A, &work_mat);

// swap out column 1 (y column)
MAT_COLUMN_SWAP_2X2(&work_mat, 1, B);

// compute determinate of A with B swapped into y column
float det_ABy = Mat_Det_2X2(&work_mat);

// now solve for X01
X->M01 = det_ABy/det_A;

// return success
return(1);

} // end Solve_2X2_System

///////////////////////////////////////////////////////////////

void Mat_Add_3X3(MATRIX3X3_PTR ma, 
                 MATRIX3X3_PTR mb,
                 MATRIX3X3_PTR msum)
{
// this function adds two 3x3 matrices together and 
// and stores the result

for (int row=0; row<3; row++)
    {
    for (int col=0; col<3; col++)
        {
        // insert resulting row,col element
        msum->M[row][col] = ma->M[row][col] + mb->M[row][col];
        } // end for col

    } // end for row

} // end Mat_Add_3X3

////////////////////////////////////////////////////////////////////

void Mat_Mul_VECTOR3D_3X3(VECTOR3D_PTR  va, 
                          MATRIX3X3_PTR mb,
                          VECTOR3D_PTR  vprod)
{
// this function multiplies a VECTOR3D against a 
// 3x3 matrix - ma*mb and stores the result in mprod

    for (int col=0; col < 3; col++)
        {
        // compute dot product from row of ma 
        // and column of mb
        float sum = 0; // used to hold result

        for (int row=0; row<3; row++)
             {
             // add in next product pair
             sum+=(va->M[row]*mb->M[row][col]);
             } // end for index

        // insert resulting col element
        vprod->M[col] = sum;

        } // end for col

} // end Mat_Mul_VECTOR3D_3X3

////////////////////////////////////////////////////////////////////

void Mat_Init_3X3(MATRIX3X3_PTR ma, 
                  float m00, float m01, float m02,
                  float m10, float m11, float m12,
                  float m20, float m21, float m22)
{
// this function fills a 3x3 matrix with the sent data in row major form

ma->M00 = m00; ma->M01 = m01; ma->M02 = m02;
ma->M10 = m10; ma->M11 = m11; ma->M12 = m12;
ma->M20 = m20; ma->M21 = m21; ma->M22 = m22;

} // end Mat_Init_3X3

/////////////////////////////////////////////////////////////////

 int Mat_Inverse_3X3(MATRIX3X3_PTR m, MATRIX3X3_PTR mi)
{
// this function computes the inverse of a 3x3

// first compute the determinate to see if there is 
// an inverse
float det = m->M00*(m->M11*m->M22 - m->M21*m->M12) - 
            m->M01*(m->M10*m->M22 - m->M20*m->M12) + 
            m->M02*(m->M10*m->M21 - m->M20*m->M11);

if (fabs(det) < EPSILON_E5)
   return(0);

// compute inverse to save divides
float det_inv = 1.0/det;

// compute inverse using m-1 = adjoint(m)/det(m)
mi->M00 =  det_inv*(m->M11*m->M22 - m->M21*m->M12);
mi->M10 = -det_inv*(m->M10*m->M22 - m->M20*m->M12);
mi->M20 =  det_inv*(m->M10*m->M21 - m->M20*m->M11);

mi->M01 = -det_inv*(m->M01*m->M22 - m->M21*m->M02);
mi->M11 =  det_inv*(m->M00*m->M22 - m->M20*m->M02);
mi->M21 = -det_inv*(m->M00*m->M21 - m->M20*m->M01);

mi->M02 =  det_inv*(m->M01*m->M12 - m->M11*m->M02);
mi->M12 = -det_inv*(m->M00*m->M12 - m->M10*m->M02);
mi->M22 =  det_inv*(m->M00*m->M11 - m->M10*m->M01);

// return success
return(1);

} // end Mat_Inverse_3X3

/////////////////////////////////////////////////////////////////

void Print_Mat_3X3(MATRIX3X3_PTR ma, char *name="M")
{
// prints out a 3x3 matrix
Write_Error("\n%s=\n",name);

for (int r=0; r < 3; r++, Write_Error("\n"))
    for (int c=0; c < 3; c++)
        Write_Error("%f ",ma->M[r][c]);        

} // end Print_Mat_3X3

/////////////////////////////////////////////////////////////////

float Mat_Det_3X3(MATRIX3X3_PTR m)
{
// computes the determinate of a 3x3 matrix using co-factor
// expansion

return(m->M00*(m->M11*m->M22 - m->M21*m->M12) - 
       m->M01*(m->M10*m->M22 - m->M20*m->M12) + 
       m->M02*(m->M10*m->M21 - m->M20*m->M11) );

} // end Mat_Det_3X3

///////////////////////////////////////////////////////////////

int Solve_3X3_System(MATRIX3X3_PTR A, MATRIX1X3_PTR X, MATRIX1X3_PTR B)
{
// solves the system AX=B and computes X=A(-1)*B
// by using cramers rule and determinates

// step 1: compute determinate of A
float det_A = Mat_Det_3X3(A);

// test if det(a) is zero, if so then there is no solution
if (fabs(det_A) < EPSILON_E5)
   return(0);

// step 2: create x,y,z numerator matrices by taking A and
// replacing each column of it with B(transpose) and solve
MATRIX3X3 work_mat; // working matrix

// solve for x /////////////////

// copy A into working matrix
MAT_COPY_3X3(A, &work_mat);

// swap out column 0 (x column)
MAT_COLUMN_SWAP_3X3(&work_mat, 0, B);

// compute determinate of A with B swapped into x column
float det_ABx = Mat_Det_3X3(&work_mat);

// now solve for X00
X->M00 = det_ABx/det_A;

// solve for y /////////////////

// copy A into working matrix
MAT_COPY_3X3(A, &work_mat);

// swap out column 1 (y column)
MAT_COLUMN_SWAP_3X3(&work_mat, 1, B);

// compute determinate of A with B swapped into y column
float det_ABy = Mat_Det_3X3(&work_mat);

// now solve for X01
X->M01 = det_ABy/det_A;

// solve for z /////////////////

// copy A into working matrix
MAT_COPY_3X3(A, &work_mat);

// swap out column 2 (z column)
MAT_COLUMN_SWAP_3X3(&work_mat, 2, B);

// compute determinate of A with B swapped into z column
float det_ABz = Mat_Det_3X3(&work_mat);

// now solve for X02
X->M02 = det_ABz/det_A;

// return success
return(1);

} // end Solve_3X3_System

///////////////////////////////////////////////////////////////

void Mat_Add_4X4(MATRIX4X4_PTR ma, 
                 MATRIX4X4_PTR mb,
                 MATRIX4X4_PTR msum)
{
// this function adds two 4x4 matrices together and 
// and stores the result

for (int row=0; row<4; row++)
    {
    for (int col=0; col<4; col++)
        {
        // insert resulting row,col element
        msum->M[row][col] = ma->M[row][col] + mb->M[row][col];
        } // end for col

    } // end for row

} // end Mat_Add_4X4

///////////////////////////////////////////////////////////////

void Mat_Mul_4X4(MATRIX4X4_PTR ma, 
                 MATRIX4X4_PTR mb,
                 MATRIX4X4_PTR mprod)
{
// this function multiplies two 4x4 matrices together and 
// and stores the result in mprod
// note later we will take advantage of the fact that we know
// that w=1 always, and that the last column of a 4x4 is
// always 0

for (int row=0; row<4; row++)
    {
    for (int col=0; col<4; col++)
        {
        // compute dot product from row of ma 
        // and column of mb

        float sum = 0; // used to hold result

        for (int index=0; index<4; index++)
             {
             // add in next product pair
             sum+=(ma->M[row][index]*mb->M[index][col]);
             } // end for index

        // insert resulting row,col element
        mprod->M[row][col] = sum;

        } // end for col

    } // end for row

} // end Mat_Mul_4X4

////////////////////////////////////////////////////////////////

void Mat_Mul_1X4_4X4(MATRIX1X4_PTR ma, 
                     MATRIX4X4_PTR mb,
                     MATRIX1X4_PTR mprod)
{
// this function multiplies a 1x4 matrix against a 
// 4x4 matrix - ma*mb and stores the result
// no tricks or assumptions here, just a straight multiply

    for (int col=0; col<4; col++)
        {
        // compute dot product from row of ma 
        // and column of mb
        float sum = 0; // used to hold result

        for (int row=0; row<4; row++)
             {
             // add in next product pair
             sum+=(ma->M[row] * mb->M[row][col]);
             } // end for index

        // insert resulting col element
        mprod->M[col] = sum;

        } // end for col

} // end Mat_Mul_1X4_4X4

////////////////////////////////////////////////////////////////////

void Mat_Mul_VECTOR3D_4X4(VECTOR3D_PTR  va, 
                          MATRIX4X4_PTR mb,
                          VECTOR3D_PTR  vprod)
{
// this function multiplies a VECTOR3D against a 
// 4x4 matrix - ma*mb and stores the result in mprod
// the function assumes that the vector refers to a 
// 4D homogenous vector, thus the function assumes that
// w=1 to carry out the multiply, also the function
// does not carry out the last column multiply since
// we are assuming w=1, there is no point

    for (int col=0; col < 3; col++)
        {
        // compute dot product from row of ma 
        // and column of mb
        float sum = 0; // used to hold result

        for (int row=0; row<3; row++)
             {
             // add in next product pair
             sum+=(va->M[row]*mb->M[row][col]);
             } // end for index

        // add in last element in column or w*mb[3][col]
        sum+=mb->M[row][col];    
 
        // insert resulting col element
        vprod->M[col] = sum;

        } // end for col

} // end Mat_Mul_VECTOR3D_4X4

///////////////////////////////////////////////////////////////

void Mat_Mul_VECTOR3D_4X3(VECTOR3D_PTR  va, 
                          MATRIX4X3_PTR mb,
                          VECTOR3D_PTR  vprod)
{
// this function multiplies a VECTOR3D against a 
// 4x3 matrix - ma*mb and stores the result in mprod
// the function assumes that the vector refers to a 
// 4D homogenous vector, thus the function assumes that
// w=1 to carry out the multiply, also the function
// does not carry out the last column multiply since
// we are assuming w=1, there is no point

    for (int col=0; col < 3; col++)
        {
        // compute dot product from row of ma 
        // and column of mb
        float sum = 0; // used to hold result

        for (int row=0; row<3; row++)
             {
             // add in next product pair