矩阵运算概要

/* 矩阵定义 - 4*4齐次矩阵 用于openGL
linolzhang, 2010.2.10
*/

#ifndef MATRIX4_H
#define MATRIX4_H

#include "Tuple4.h"
#include "BaseOperation.h"

template <typename T>
class Matrix4
{
public:
T  m[16]; // 一维数组，与openGL矩阵一致 - 按列优先

// [ m0, m4, m8, m12 ]
// [ m1, m5, m9, m13 ]
// [ m2, m6, m10,m14 ]
// [ m3, m4, m11,m15 ]

public:
Matrix4() // 默认构造函数，设置单位矩阵
{
setIdentity();
}

Matrix4( T  m0, T  m1, T  m2, T  m3,
T  m4, T  m5, T  m6, T  m7,
T  m8, T  m9, T m10, T m11,
T m12, T m13, T m14, T m15 ) // 构造函数，指定值
{
m[ 0] =  m0; m[ 1] =  m1; m[ 2] =  m2; m[ 3] =  m3;
m[ 4] =  m4; m[ 5] =  m5; m[ 6] =  m6; m[ 7] =  m7;
m[ 8] =  m8; m[ 9] =  m9; m[10] = m10; m[11] = m11;
m[12] = m12; m[13] = m13; m[14] = m14; m[15] = m15;
}

Matrix4(const T* elements) // 构造函数，指针指定
{
memcpy(m, elements, sizeof(T)*16);
}

Matrix4(const Matrix4 &mat)  // 拷贝构造函数
{
memcpy(m,mat.m, sizeof(T)*16);
}
const Matrix4 &operator  = (const Matrix4 &mat) // 赋值操作符
{
memcpy(m,mat.m, sizeof(T)*16);
return *this;
}

// --------------------------------------------------------
// 构造函数 - 由其它转换
Matrix4(const Tuple3<T> &rAxis, float rAngle)  // 由 旋转轴向量，旋转角 得到
{
float sinAngle = fastSin(rAngle),
cosAngle = fastCos(rAngle),
oneMinusCosAngle = 1.0f - cosAngle;

m[ 0] = (rAxis.x)*(rAxis.x) + cosAngle*(1-(rAxis.x)*(rAxis.x));
m[ 4] = (rAxis.x)*(rAxis.y)*(oneMinusCosAngle) - sinAngle*rAxis.z;
m[ 8] = (rAxis.x)*(rAxis.z)*(oneMinusCosAngle) + sinAngle*rAxis.y;

m[ 1] = (rAxis.x)*(rAxis.y)*(oneMinusCosAngle) + sinAngle*rAxis.z;
m[ 5] = (rAxis.y)*(rAxis.y) + cosAngle*(1-(rAxis.y)*(rAxis.y));
m[ 9] = (rAxis.y)*(rAxis.z)*(oneMinusCosAngle) - sinAngle*rAxis.x;

m[ 2] = (rAxis.x)*(rAxis.z)*(oneMinusCosAngle) - sinAngle*rAxis.y;
m[ 6] = (rAxis.y)*(rAxis.z)*(oneMinusCosAngle) + sinAngle*rAxis.x;
m[10] = (rAxis.z)*(rAxis.z) + cosAngle*(1-(rAxis.z)*(rAxis.z));

m[3] = m[7] = m[11] = 0;
m[12] = m[13] = m[14] = 0;
m[15] = 1;
}

// --------------------------------------------------------
// 重载 +=, -=, *=矩阵, *=常量
void operator +=(const Matrix4 &mat)
{
for(int i = 0; i < 16; i++)
m[i] += mat.m[i];
}

void operator -=(const Matrix4 &mat)
{
for(int i = 0; i < 16; i++)
m[i] -= mat.m[i];
}
void operator *= (const Matrix4 &mat)
{
setElements( m[0]*mat.m[ 0]+m[4]*mat.m[ 1]+m[ 8]*mat.m[ 2]+ m[12]*mat.m[ 3],
m[1]*mat.m[ 0]+m[5]*mat.m[ 1]+m[ 9]*mat.m[ 2]+ m[13]*mat.m[ 3],
m[2]*mat.m[ 0]+m[6]*mat.m[ 1]+m[10]*mat.m[ 2]+ m[14]*mat.m[ 3],
m[3]*mat.m[ 0]+m[7]*mat.m[ 1]+m[11]*mat.m[ 2]+ m[15]*mat.m[ 3],
m[0]*mat.m[ 4]+m[4]*mat.m[ 5]+m[ 8]*mat.m[ 6]+ m[12]*mat.m[ 7],
m[1]*mat.m[ 4]+m[5]*mat.m[ 5]+m[ 9]*mat.m[ 6]+ m[13]*mat.m[ 7],
m[2]*mat.m[ 4]+m[6]*mat.m[ 5]+m[10]*mat.m[ 6]+ m[14]*mat.m[ 7],
m[3]*mat.m[ 4]+m[7]*mat.m[ 5]+m[11]*mat.m[ 6]+ m[15]*mat.m[ 7],
m[0]*mat.m[ 8]+m[4]*mat.m[ 9]+m[ 8]*mat.m[10]+ m[12]*mat.m[11],
m[1]*mat.m[ 8]+m[5]*mat.m[ 9]+m[ 9]*mat.m[10]+ m[13]*mat.m[11],
m[2]*mat.m[ 8]+m[6]*mat.m[ 9]+m[10]*mat.m[10]+ m[14]*mat.m[11],
m[3]*mat.m[ 8]+m[7]*mat.m[ 9]+m[11]*mat.m[10]+ m[15]*mat.m[11],
m[0]*mat.m[12]+m[4]*mat.m[13]+m[ 8]*mat.m[14]+ m[12]*mat.m[15],
m[1]*mat.m[12]+m[5]*mat.m[13]+m[ 9]*mat.m[14]+ m[13]*mat.m[15],
m[2]*mat.m[12]+m[6]*mat.m[13]+m[10]*mat.m[14]+ m[14]*mat.m[15],
m[3]*mat.m[12]+m[7]*mat.m[13]+m[11]*mat.m[14]+ m[15]*mat.m[15] );
}
void operator *= (T scale) // 乘常量
{
for(int i=0; i<16; i++)
m[i] *= scale;
}

// ----------------------------------
// ==, !=
bool operator == (const Matrix4 &mat)
{
return memcmp(m, mat.m, sizeof(T)*16) == 0;
}

bool operator != (const Matrix4 &mat)
{
return memcmp(m, mat.m, sizeof(T)*16) != 0;
}

////////////////////////////////////////////////////////////////////////////////////////////////////////
// --------------------------------------------------------
// 矩阵求解
const T getDeterminant()  // 得到行列式的值（行列式 !=0 可逆）
{
T det;
det =  m[0] * m[5] * m[10];
det += m[4] * m[9] * m[2];
det += m[8] * m[1] * m[6];
det -= m[8] * m[5] * m[2];
det -= m[4] * m[1] * m[10];
det -= m[0] * m[9] * m[6];
return det;
}

////////////////////////////////////////////////////////////////////////////////////////////////////////
// --------------------------------------------------------
// 矩阵变换
void setElements(const T* elements) // 重新设置矩阵元素
{
memcpy(m, elements, sizeof(T)*16);
}

void setElements( T  m0, T  m1, T  m2, T  m3,
T  m4, T  m5, T  m6, T  m7,
T  m8, T  m9, T m10, T m11,
T m12, T m13, T m14, T m15 ) // 重新设置矩阵元素
{
m[ 0] =  m0; m[ 1] =  m1; m[ 2] =  m2; m[ 3] =  m3;
m[ 4] =  m4; m[ 5] =  m5; m[ 6] =  m6; m[ 7] =  m7;
m[ 8] =  m8; m[ 9] =  m9; m[10] = m10; m[11] = m11;
m[12] = m12; m[13] = m13; m[14] = m14; m[15] = m15;
}

void setElements(const Matrix4  &matrix) // 重新设置矩阵元素
{
setElements(matrix.m);
}

// ----------------------------------
void setZero() // 设置为0矩阵
{
for(int i = 0; i < 16; i++)
m[i] = 0;
}

void setIdentity() // 设置为单位矩阵
{
m[ 0] = 1; m[ 1] = 0; m[ 2] = 0; m[ 3] = 0;
m[ 4] = 0; m[ 5] = 1; m[ 6] = 0; m[ 7] = 0;
m[ 8] = 0; m[ 9] = 0; m[10] = 1; m[11] = 0;
m[12] = 0; m[13] = 0; m[14] = 0; m[15] = 1;
}

// ----------------------------------
void setTranspose() // 矩阵转置
{
T temp = 0;
temp  = m[4];  m[4]  = m[1];
m[1]  = temp;  temp  = m[8];
m[8]  = m[2];  m[2]  = temp;
temp  = m[12]; m[12] = m[3];
m[3]  = temp;  temp  = m[9];
m[9]  = m[6];  m[6]  = temp;
temp  = m[13]; m[13] = m[7];
m[7]  = temp;  temp  = m[14];
m[14] = m[11]; m[11] = temp;
}

void setNegate() // 矩阵取反
{
for(int i = 0; i < 16; i++)
m[i] =-m[i];
}

// --------------------------------------------------------
bool setInverse() // 求逆矩阵
{
int i, j, k, swap;
T temp[16], t;

memcpy(temp, m, 16*sizeof(T)); // 当前矩阵拷贝到暂存temp
setIdentity();

for (i = 0; i < 4; i++)
{
swap = i;
for (j = i + 1; j < 4; j++)
{
if (fabs(temp[j*4 + i]) >  fabs(temp[i*4 + i]))
{
swap = j;
}
}

if (swap != i)
{
for (k = 0; k < 4; k++)
{
t                = temp[i*4 + k];
temp[i*4 + k]    = temp[swap*4 + k];
temp[swap*4 + k] = t;

t             = m[i*4 + k];
m[i*4 + k]    = m[swap*4 + k];
m[swap*4 + k] = t;
}
}

if(!temp[i*4 + i])
return false;

t = temp[i*4 + i];

for (k = 0; k < 4; k++)
{
temp[i*4 + k] /= t;
m[i*4 + k]     = m[i*4 + k] / t;
}

for (j = 0; j < 4; j++)
{
if (j != i)
{
t = temp[j*4 + i];
for (k = 0; k < 4; k++)
{
temp[j*4 + k] -= temp[i*4 + k] * t;
m[j*4 + k]     = m[j*4 + k] - m[i*4 + k] * t;
}
}
}
}
return true;
}

////////////////////////////////////////////////////////////////////////////////////////////////////////
// --------------------------------------------------------
// 矩阵转化，由其它表达方式转换得到
void setTranslate(const Tuple3<T> &t) // 设置为平移矩阵 - 由三元组指定平移量
{
setIdentity();
m[12] =  t.x;
m[13] =  t.y;
m[14] =  t.z;
}

void setTranslate(T x,T y,T z) // 设置为平移矩阵 - 由x,y,z指定平移量
{
setIdentity();
m[12] =  x;
m[13] =  y;
m[14] =  z;
}

void setRotationX(float xAngle) // 设置为旋转矩阵 - 指定绕 X 旋转角度 - 欧拉角
{
setIdentity();
m[ 5] =  fastCos(xAngle);
m[ 6] =  fastSin(xAngle);
m[ 9] = -m[6];
m[10] =  m[5];
}

void setRotationY(float yAngle) // 设置为旋转矩阵 - 指定绕 Y 旋转角度 - 欧拉角
{
setIdentity();
m[ 0] =  fastCos(yAngle);
m[ 2] =  fastSin(yAngle);
m[ 8] = -m[2];
m[10] =  m[0];
}

void setRotationZ(float zAngle)  // 设置为旋转矩阵 - 指定绕 Z 旋转角度 - 欧拉角
{
setIdentity();
m[0] =  fastCos(zAngle);
m[1] =  fastSin(zAngle);
m[4] = -m[1];
m[5] =  m[0];
}

void setRotationXYZ(float x,float y,float z) // 设置为旋转矩阵 - 分别指定绕 X,Y,Z 旋转角度 - 欧拉角
{
float cosX = fastCos(x), sinX = fastSin(x),
cosY = fastCos(y), sinY = fastSin(y),
cosZ = fastCos(z), sinZ = fastSin(z);

setElements( cosY * cosZ + sinX * sinY * sinZ,   -cosX * sinZ,    sinX * cosY * sinZ - sinY * cosZ,  0,
cosY * sinZ - sinX * sinY * cosZ,    cosX * cosZ,   -sinY * sinZ - sinX * cosY * cosZ,  0,
cosX * sinY,                         sinX,           cosX * cosY,                       0,
0,                                   0,              0,                                 1  );
}
void setRotateAxis(const Tuple3<T> &axis, float angle) // 设置为旋转矩阵 - 由旋转轴向量，旋转角指定
{
float sinAngle = fastSin(angle),
cosAngle = fastCos(angle),
oneMinusCosAngle = 1.0f - cosAngle;

m[ 0] = (axis.x)*(axis.x) + cosAngle*(1-(axis.x)*(axis.x));
m[ 4] = (axis.x)*(axis.y)*(oneMinusCosAngle) - sinAngle*axis.z;
m[ 8] = (axis.x)*(axis.z)*(oneMinusCosAngle) + sinAngle*axis.y;

m[ 1] = (axis.x)*(axis.y)*(oneMinusCosAngle) + sinAngle*axis.z;
m[ 5] = (axis.y)*(axis.y) + cosAngle*(1-(axis.y)*(axis.y));
m[ 9] = (axis.y)*(axis.z)*(oneMinusCosAngle) - sinAngle*axis.x;

m[ 2] = (axis.x)*(axis.z)*(oneMinusCosAngle) - sinAngle*axis.y;
m[ 6] = (axis.y)*(axis.z)*(oneMinusCosAngle) + sinAngle*axis.x;
m[10] = (axis.z)*(axis.z) + cosAngle*(1-(axis.z)*(axis.z));

m[3] = m[7] = m[11] = 0;
m[12] = m[13] = m[14] = 0;
m[15] = 1;
}

void setScale(T x,T y,T z)  // 设置为缩放矩阵，由 x,y,z 指定缩放量
{
setIdentity();
m[ 0] = x;
m[ 5] = y;
m[10] = z;
}

////////////////////////////////////////////////////////////////////////////////////////////////////////
// --------------------------------------------------------
// 定义矩阵运算 - 左乘
void multByMatrix(const Matrix4<T> &mat)
{
setElements(    mat.m[0]*m[ 0] + mat.m[4]*m[ 1] + mat.m[ 8]*m[ 2] + mat.m[12]*m[ 3],
mat.m[1]*m[ 0] + mat.m[5]*m[ 1] + mat.m[ 9]*m[ 2] + mat.m[13]*m[ 3],
mat.m[2]*m[ 0] + mat.m[6]*m[ 1] + mat.m[10]*m[ 2] + mat.m[14]*m[ 3],
mat.m[3]*m[ 0] + mat.m[7]*m[ 1] + mat.m[11]*m[ 2] + mat.m[15]*m[ 3],
mat.m[0]*m[ 4] + mat.m[4]*m[ 5] + mat.m[ 8]*m[ 6] + mat.m[12]*m[ 7],
mat.m[1]*m[ 4] + mat.m[5]*m[ 5] + mat.m[ 9]*m[ 6] + mat.m[13]*m[ 7],
mat.m[2]*m[ 4] + mat.m[6]*m[ 5] + mat.m[10]*m[ 6] + mat.m[14]*m[ 7],
mat.m[3]*m[ 4] + mat.m[7]*m[ 5] + mat.m[11]*m[ 6] + mat.m[15]*m[ 7],
mat.m[0]*m[ 8] + mat.m[4]*m[ 9] + mat.m[ 8]*m[10] + mat.m[12]*m[11],
mat.m[1]*m[ 8] + mat.m[5]*m[ 9] + mat.m[ 9]*m[10] + mat.m[13]*m[11],
mat.m[2]*m[ 8] + mat.m[6]*m[ 9] + mat.m[10]*m[10] + mat.m[14]*m[11],
mat.m[3]*m[ 8] + mat.m[7]*m[ 9] + mat.m[11]*m[10] + mat.m[15]*m[11],
mat.m[0]*m[12] + mat.m[4]*m[13] + mat.m[ 8]*m[14] + mat.m[12]*m[15],
mat.m[1]*m[12] + mat.m[5]*m[13] + mat.m[ 9]*m[14] + mat.m[13]*m[15],
mat.m[2]*m[12] + mat.m[6]*m[13] + mat.m[10]*m[14] + mat.m[14]*m[15],
mat.m[3]*m[12] + mat.m[7]*m[13] + mat.m[11]*m[14] + mat.m[15]*m[15] );
}
};

////////////////////////////////////////////////////////////////////////////////////////////////////////
// --------------------------------------------------------
// 外部函数 - 矩阵运算 +, -, * ( Matrix4 vs Matrix4)
template<typename T>
inline const Matrix4<T> operator + (const Matrix4<T> &mat1,const Matrix4<T> &mat2)
{
return Matrix4<T>( mat1.m[ 0] + mat2.m[ 0], mat1.m[ 1] + mat2.m[ 1], mat1.m[ 2] + mat2.m[ 2], mat1.m[ 3] + mat2.m[ 3],
mat1.m[ 4] + mat2.m[ 4], mat1.m[ 5] + mat2.m[ 5], mat1.m[ 6] + mat2.m[ 6], mat1.m[ 7] + mat2.m[ 7],
mat1.m[ 8] + mat2.m[ 8], mat1.m[ 9] + mat2.m[ 9], mat1.m[10] + mat2.m[10], mat1.m[11] + mat2.m[11],
mat1.m[12] + mat2.m[12], mat1.m[13] + mat2.m[13], mat1.m[14] + mat2.m[14], mat1.m[15] + mat2.m[15] );
}

template<typename T>
inline const Matrix4<T> operator - (const Matrix4<T> &mat1,const Matrix4<T> &mat2)
{
return Matrix4<T>( mat1.m[ 0] - mat2.m[ 0], mat1.m[ 1] - mat2.m[ 1], mat1.m[ 2] - mat2.m[ 2], mat1.m[ 3] - mat2.m[ 3],
mat1.m[ 4] - mat2.m[ 4], mat1.m[ 5] - mat2.m[ 5], mat1.m[ 6] - mat2.m[ 6], mat1.m[ 7] - mat2.m[ 7],
mat1.m[ 8] - mat2.m[ 8], mat1.m[ 9] - mat2.m[ 9], mat1.m[10] - mat2.m[10], mat1.m[11] - mat2.m[11],
mat1.m[12] - mat2.m[12], mat1.m[13] - mat2.m[13], mat1.m[14] - mat2.m[14], mat1.m[15] - mat2.m[15]);
}

template<typename T>
inline const Matrix4<T> operator * (const Matrix4<T> &mat1,const Matrix4<T> &mat2)
{
return Matrix4<T>( mat1.m[0]*mat2.m[ 0] + mat1.m[4]*mat2.m[ 1] + mat1.m[ 8]*mat2.m[ 2] + mat1.m[12]*mat2.m[ 3],
mat1.m[1]*mat2.m[ 0] + mat1.m[5]*mat2.m[ 1] + mat1.m[ 9]*mat2.m[ 2] + mat1.m[13]*mat2.m[ 3],
mat1.m[2]*mat2.m[ 0] + mat1.m[6]*mat2.m[ 1] + mat1.m[10]*mat2.m[ 2] + mat1.m[14]*mat2.m[ 3],
mat1.m[3]*mat2.m[ 0] + mat1.m[7]*mat2.m[ 1] + mat1.m[11]*mat2.m[ 2] + mat1.m[15]*mat2.m[ 3],
mat1.m[0]*mat2.m[ 4] + mat1.m[4]*mat2.m[ 5] + mat1.m[ 8]*mat2.m[ 6] + mat1.m[12]*mat2.m[ 7],
mat1.m[1]*mat2.m[ 4] + mat1.m[5]*mat2.m[ 5] + mat1.m[ 9]*mat2.m[ 6] + mat1.m[13]*mat2.m[ 7],
mat1.m[2]*mat2.m[ 4] + mat1.m[6]*mat2.m[ 5] + mat1.m[10]*mat2.m[ 6] + mat1.m[14]*mat2.m[ 7],
mat1.m[3]*mat2.m[ 4] + mat1.m[7]*mat2.m[ 5] + mat1.m[11]*mat2.m[ 6] + mat1.m[15]*mat2.m[ 7],
mat1.m[0]*mat2.m[ 8] + mat1.m[4]*mat2.m[ 9] + mat1.m[ 8]*mat2.m[10] + mat1.m[12]*mat2.m[11],
mat1.m[1]*mat2.m[ 8] + mat1.m[5]*mat2.m[ 9] + mat1.m[ 9]*mat2.m[10] + mat1.m[13]*mat2.m[11],
mat1.m[2]*mat2.m[ 8] + mat1.m[6]*mat2.m[ 9] + mat1.m[10]*mat2.m[10] + mat1.m[14]*mat2.m[11],
mat1.m[3]*mat2.m[ 8] + mat1.m[7]*mat2.m[ 9] + mat1.m[11]*mat2.m[10] + mat1.m[15]*mat2.m[11],
mat1.m[0]*mat2.m[12] + mat1.m[4]*mat2.m[13] + mat1.m[ 8]*mat2.m[14] + mat1.m[12]*mat2.m[15],
mat1.m[1]*mat2.m[12] + mat1.m[5]*mat2.m[13] + mat1.m[ 9]*mat2.m[14] + mat1.m[13]*mat2.m[15],
mat1.m[2]*mat2.m[12] + mat1.m[6]*mat2.m[13] + mat1.m[10]*mat2.m[14] + mat1.m[14]*mat2.m[15],
mat1.m[3]*mat2.m[12] + mat1.m[7]*mat2.m[13] + mat1.m[11]*mat2.m[14] + mat1.m[15]*mat2.m[15] );
}

// --------------------------------------------------------
// 外部函数 - 矩阵运算( Matrix4 vs Others)
template<typename T>
inline const Tuple3<T> operator * (const Tuple3<T> &t,const Matrix4<T> &mat)
{
return Tuple3<T>( mat.m[ 0]*t.x + mat.m[ 4]*t.y + mat.m[ 8]*t.z + mat.m[12],
mat.m[ 1]*t.x + mat.m[ 5]*t.y + mat.m[ 9]*t.z + mat.m[13],
mat.m[ 2]*t.x + mat.m[ 6]*t.y + mat.m[10]*t.z + mat.m[14] );
}

template<typename T>
inline const Tuple3<T> operator * (const Matrix4<T> &mat,const Tuple3<T> &t)
{
return Tuple3<T>( mat.m[ 0]*t.x + mat.m[ 4]*t.y + mat.m[ 8]*t.z + mat.m[12],
mat.m[ 1]*t.x + mat.m[ 5]*t.y + mat.m[ 9]*t.z + mat.m[13],
mat.m[ 2]*t.x + mat.m[ 6]*t.y + mat.m[10]*t.z + mat.m[14] );
}

template<typename T>
inline const Tuple4<T> operator * (const Matrix4<T> &mat, const Tuple4<T> &t)
{
return Tuple4<T>( mat.m[ 0]*t.x + mat.m[ 4]*t.y + mat.m[ 8]*t.z + mat.m[12]*t.w,
mat.m[ 1]*t.x + mat.m[ 5]*t.y + mat.m[ 9]*t.z + mat.m[13]*t.w,
mat.m[ 2]*t.x + mat.m[ 6]*t.y + mat.m[10]*t.z + mat.m[14]*t.w,
mat.m[ 3]*t.x + mat.m[ 7]*t.y + mat.m[11]*t.z + mat.m[15]*t.w  );
}

template<typename T>
inline const Tuple4<T> operator * (const Tuple4<T> &t,const Matrix4<T> &mat)
{
return Tuple4<T>( mat.m[ 0]*t.x + mat.m[ 4]*t.y + mat.m[ 8]*t.z + mat.m[12]*t.w,
mat.m[ 1]*t.x + mat.m[ 5]*t.y + mat.m[ 9]*t.z + mat.m[13]*t.w,
mat.m[ 2]*t.x + mat.m[ 6]*t.y + mat.m[10]*t.z + mat.m[14]*t.w,
mat.m[ 3]*t.x + mat.m[ 7]*t.y + mat.m[11]*t.z + mat.m[15]*t.w  );
}

// -------------------------------------
typedef Matrix4<int>    Matrix4i;
typedef Matrix4<float>  Matrix4f;
typedef Matrix4<double> Matrix4d;

#endif

// 利用克莱姆法则求方程解
// a11 a12 a13   x1   b1
// a21 a22 a23 * x2 = b2
// a31 a32 a33   x3   b3
// 或者写成矩阵形式为Ax=b，其中A为n*n方阵，x为n个变量构成列向量，b为n个常数项构成列向量。
bool CramerSolver(double a11,double a12,double a13,double a21,double a22,double a23,double a31,double a32,double a33,double b1,double b2,double b3,double& x1,double& x2,double& x3)
{
// 首先计算行列式的值
double D = Determinant( a11, a12, a13,
a21, a22, a23,
a31, a32, a33 );
if(0==D)
return false;

// 当它的系数矩阵可逆，或者说对应的行列式|A|不等于0的时候，它有唯一解xi=|Ai|/|A|，其中Ai〔i = 1，2，……，n〕是矩阵A中第i列的a 1i，a 2i，……a ni （即第i列）依次换成b1，b2，……bn所得的矩阵。
//D1=|b1 a12 a13|
//	 |b2 a22 a23|
//	 |b3 a32 a33|
double D1 = Determinant( b1, a12, a13,
b2, a22, a23,
b3, a32, a33 );
double D2 = Determinant( a11, b1, a13,
a21, b2, a23,
a31, b3, a33 );
double D3 = Determinant( a11, a12, b1,
a21, a22, b2,
a31, a32, b3 );

x1 = D1/D;
x2 = D2/D;
x3 = D3/D;

return true;
}