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

//矩阵求逆的快速算法

//算法介绍

//矩阵求逆在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];

}