矩阵求逆的快速算法(转)

//矩阵求逆的快速算法
//算法介绍
//矩阵求逆在3D程序中很常见，主要应用于求Billboard矩阵。按照定义的计算方法乘法运算，严重影响了性能。在需要大量Billboard矩阵运算时，矩阵求逆的优化能极大提高性能。这里要介绍的矩阵求逆算法称为全选主元高斯-约旦法。

//高斯-约旦法（全选主元）求逆的步骤如下：

//首先，对于 k 从 0 到 n - 1 作如下几步：

//从第 k 行、第 k 列开始的右下角子阵中选取绝对值最大的元素，并记住次元素所在的行号和列号，在通过行交换和列交换将它交换到主元素位置上。这一步称为全选主元。
//m(k, k) = 1 / m(k, k)
//m(k, j) = m(k, j) * m(k, k)，j = 0, 1, ..., n-1；j != k
//m(i, j) = m(i, j) - m(i, k) * m(k, j)，i, j = 0, 1, ..., n-1；i, j != k
//m(i, k) = -m(i, k) * m(k, k)，i = 0, 1, ..., n-1；i != k
//最后，根据在全选主元过程中所记录的行、列交换的信息进行恢复，恢复的原则如下：在全选主元过程中，先交换的行（列）后进行恢复；原来的行（列）交换用列（行）交换来恢复。

//实现(4阶矩阵)
float Inverse(CLAYMATRIX& mOut, const CLAYMATRIX& rhs)
{
CLAYMATRIX m(rhs);
DWORD is[4];
DWORD js[4];
float fDet = 1.0f;
int f = 1;

for (int k = 0; k < 4; k ++)
{
// 第一步，全选主元
float fMax = 0.0f;
for (DWORD i = k; i < 4; i ++)
{
for (DWORD j = k; j < 4; j ++)
{
const float f = Abs(m(i, j));
if (f > fMax)
{
fMax = f;
is[k] = i;
js[k] = j;
}
}
}
if (Abs(fMax) < 0.0001f)
return 0;

if (is[k] != k)
{
f = -f;
swap(m(k, 0), m(is[k], 0));
swap(m(k, 1), m(is[k], 1));
swap(m(k, 2), m(is[k], 2));
swap(m(k, 3), m(is[k], 3));
}
if (js[k] != k)
{
f = -f;
swap(m(0, k), m(0, js[k]));
swap(m(1, k), m(1, js[k]));
swap(m(2, k), m(2, js[k]));
swap(m(3, k), m(3, js[k]));
}

// 计算行列值
fDet *= m(k, k);

// 计算逆矩阵

// 第二步
m(k, k) = 1.0f / m(k, k);
// 第三步
for (DWORD j = 0; j < 4; j ++)
{
if (j != k)
m(k, j) *= m(k, k);
}
// 第四步
for (DWORD i = 0; i < 4; i ++)
{
if (i != k)
{
for (j = 0; j < 4; j ++)
{
if (j != k)
m(i, j) = m(i, j) - m(i, k) * m(k, j);
}
}
}
// 第五步
for (i = 0; i < 4; i ++)
{
if (i != k)
m(i, k) *= -m(k, k);
}
}

for (k = 3; k >= 0; k --)
{
if (js[k] != k)
{
swap(m(k, 0), m(js[k], 0));
swap(m(k, 1), m(js[k], 1));
swap(m(k, 2), m(js[k], 2));
swap(m(k, 3), m(js[k], 3));
}
if (is[k] != k)
{
swap(m(0, k), m(0, is[k]));
swap(m(1, k), m(1, is[k]));
swap(m(2, k), m(2, is[k]));
swap(m(3, k), m(3, is[k]));
}
}

mOut = m;
return fDet * f;
}


//比较
//原算法 原算法(经过高度优化) 新算法
//加法次数 103 61 39
//乘法次数 170 116 69
//需要额外空间 16 * sizeof(float) 34 * sizeof(float) 25 * sizeof(float)

//结果不言而喻吧。

//1、算法介绍
//　　矩阵相乘在进行3D变换的时候是经常用到的。在应用中常用矩阵相乘的定义算法对其进行计算。这个算法用到了大量的循环和相乘运算，这使得算法效率不高。而矩阵相乘的计算效率很大程度上的影响了整个程序的运行速度，所以对矩阵相乘算法进行一些改进是必要的。
//　　这里要介绍的矩阵算法称为斯特拉森方法，它是由v.斯特拉森在1969年提出的一个方法。
//　　我们先讨论二阶矩阵的计算方法。
//　　对于二阶矩阵：
//A = | a11 a12 |
//| a21 a22 |
//B = | b11 b12 |
//| b21 b22 |
//
//
//　　先计算下面7个量（1）：

x1 = (a11 + a22) * (b11 + b22);
x2 = (a21 + a22) * b11;
x3 = a11 * (b12 - b22);
x4 = a22 * (b21 - b11);
x5 = (a11 + a12) * b22;
x6 = (a21 - a11) * (b11 + b12);
x7 = (a12 - a22) * (b21 + b22);

　　//再设C = AB。根据矩阵相乘的规则，C的各元素为（2）：

c11 = a11 * b11 + a12 * b21
c12 = a11 * b12 + a12 * b22
c21 = a21 * b11 + a22 * b21
c22 = a21 * b12 + a22 * b22

　　//比较（1）（2），C的各元素可以表示为（3）：

c11 = x1 + x4 - x5 + x7
c12 = x3 + x5
c21 = x2 + x4
c22 = x1 + x3 - x2 + x6

　　//根据以上的方法，我们就可以计算4阶矩阵了，先将4阶矩阵A和B划分成四块2阶矩阵，分别利用公式计算它们的乘积，再使用（1）（3 ）来计算出最后结果。

A4 = | ma11 ma12 |
| ma21 ma22 |
B4 = | mb11 mb12 |
| mb21 mb22 |


　　//其中：

ma11 = | a11 a12 |
| a21 a22 |
ma12 = | a13 a14 |
| a23 a24 |
mb11 = | b11 b12 |
| b21 b22 |
mb12 = | b13 b14 |
| b23 b24 |


ma21 = | a31 a32 |
| a41 a42 |
ma22 = | a33 a34 |
| a43 a44 |
mb21 = | b31 b32 |
| b41 b42 |
mb22 = | b33 b34 |
| b43 b44 |


//2、实现

// 计算2X2矩阵
void Multiply2X2(float& fOut_11, float& fOut_12, float& fOut_21, float& fOut_22,
　　　　float f1_11, float f1_12, float f1_21, float f1_22,
　　　　float f2_11, float f2_12, float f2_21, float f2_22)
{
　　const float x1((f1_11 + f1_22) * (f2_11 + f2_22));
　　const float x2((f1_21 + f1_22) * f2_11);
　　const float x3(f1_11 * (f2_12 - f2_22));
　　const float x4(f1_22 * (f2_21 - f2_11));
　　const float x5((f1_11 + f1_12) * f2_22);
　　const float x6((f1_21 - f1_11) * (f2_11 + f2_12));
　　const float x7((f1_12 - f1_22) * (f2_21 + f2_22));

　　fOut_11 = x1 + x4 - x5 + x7;
　　fOut_12 = x3 + x5;
　　fOut_21 = x2 + x4;
　　fOut_22 = x1 - x2 + x3 + x6;
}

// 计算4X4矩阵
void Multiply(CLAYMATRIX& mOut, const CLAYMATRIX& m1, const CLAYMATRIX& m2)
{
　　float fTmp[7][4];

　　// (ma11 + ma22) * (mb11 + mb22)
　　Multiply2X2(fTmp[0][0], fTmp[0][1], fTmp[0][2], fTmp[0][3],
　　　　　　m1._11 + m1._33, m1._12 + m1._34, m1._21 + m1._43, m1._22 + m1._44,
　　　　　　m2._11 + m2._33, m2._12 + m2._34, m2._21 + m2._43, m2._22 + m2._44);

　　// (ma21 + ma22) * mb11
　　Multiply2X2(fTmp[1][0], fTmp[1][1], fTmp[1][2], fTmp[1][3],
　　　　　　m1._31 + m1._33, m1._32 + m1._34, m1._41 + m1._43, m1._42 + m1._44,
　　　　　　m2._11, m2._12, m2._21, m2._22);

　　// ma11 * (mb12 - mb22)
　　Multiply2X2(fTmp[2][0], fTmp[2][1], fTmp[2][2], fTmp[2][3],
　　　　　　m1._11, m1._12, m1._21, m1._22,
　　　　　　m2._13 - m2._33, m2._14 - m2._34, m2._23 - m2._43, m2._24 - m2._44);

　　// ma22 * (mb21 - mb11)
　　Multiply2X2(fTmp[3][0], fTmp[3][1], fTmp[3][2], fTmp[3][3],
　　　　　　m1._33, m1._34, m1._43, m1._44,
　　　　　　m2._31 - m2._11, m2._32 - m2._12, m2._41 - m2._21, m2._42 - m2._22);

　　// (ma11 + ma12) * mb22
　　Multiply2X2(fTmp[4][0], fTmp[4][1], fTmp[4][2], fTmp[4][3],
　　　　　　m1._11 + m1._13, m1._12 + m1._14, m1._21 + m1._23, m1._22 + m1._24,
　　　　　　m2._33, m2._34, m2._43, m2._44);

　　// (ma21 - ma11) * (mb11 + mb12)
　　Multiply2X2(fTmp[5][0], fTmp[5][1], fTmp[5][2], fTmp[5][3],
　　　　　　m1._31 - m1._11, m1._32 - m1._12, m1._41 - m1._21, m1._42 - m1._22,
　　　　　　m2._11 + m2._13, m2._12 + m2._14, m2._21 + m2._23, m2._22 + m2._24);

　　// (ma12 - ma22) * (mb21 + mb22)
　　Multiply2X2(fTmp[6][0], fTmp[6][1], fTmp[6][2], fTmp[6][3],
　　　　　　m1._13 - m1._33, m1._14 - m1._34, m1._23 - m1._43, m1._24 - m1._44,
　　　　　　m2._31 + m2._33, m2._32 + m2._34, m2._41 + m2._43, m2._42 + m2._44);

　　// 第一块
　　mOut._11 = fTmp[0][0] + fTmp[3][0] - fTmp[4][0] + fTmp[6][0];
　　mOut._12 = fTmp[0][1] + fTmp[3][1] - fTmp[4][1] + fTmp[6][1];
　　mOut._21 = fTmp[0][2] + fTmp[3][2] - fTmp[4][2] + fTmp[6][2];
　　mOut._22 = fTmp[0][3] + fTmp[3][3] - fTmp[4][3] + fTmp[6][3];

　　// 第二块
　　mOut._13 = fTmp[2][0] + fTmp[4][0];
　　mOut._14 = fTmp[2][1] + fTmp[4][1];
　　mOut._23 = fTmp[2][2] + fTmp[4][2];
　　mOut._24 = fTmp[2][3] + fTmp[4][3];

　　// 第三块
　　mOut._31 = fTmp[1][0] + fTmp[3][0];
　　mOut._32 = fTmp[1][1] + fTmp[3][1];
　　mOut._41 = fTmp[1][2] + fTmp[3][2];
　　mOut._42 = fTmp[1][3] + fTmp[3][3];

　　// 第四块
　　mOut._33 = fTmp[0][0] - fTmp[1][0] + fTmp[2][0] + fTmp[5][0];
　　mOut._34 = fTmp[0][1] - fTmp[1][1] + fTmp[2][1] + fTmp[5][1];
　　mOut._43 = fTmp[0][2] - fTmp[1][2] + fTmp[2][2] + fTmp[5][2];
　　mOut._44 = fTmp[0][3] - fTmp[1][3] + fTmp[2][3] + fTmp[5][3];
}