计算方阵的逆，利用高斯约旦消元方法，全程基于数组操作，简单易懂

#include "AllHeaders.h" #include "GlobalVariables.h" // 注意，仅在进行运动学逆解时需要进行行列式的判断，以对奇异点进行判断，故行列式的判断仅需6自由度即可 // 在进行驻留函数的逆解计算时，不需要进行行列式的判断，因其必定为超定方程 // 维度小于6x6的行列式计算 //n阶方阵求逆运算函数，采用 void Inverse_cal(float* A, float* M_temp, float* Inv_A, int n) { float temp; float temp1; float temp2; //第一步，将A矩阵赋值到M_temp矩阵中，并形成增广矩阵 for (int i = 0; i < n; i++) { for (int j = 0; j < n; j++) { temp = *(A + i * n + j); *(M_temp + i * n * 2 + j) = temp; } } for (int i = 0; i < n; i++) { *(M_temp + i * n * 2 + n + i) = 1; } //进行列主元消元操作，将其转化为一个上三角式 for (int i = 0; i < n; i++) { int k = i; for (int j = i + 1; j < n; j++) { if (fabs(*(M_temp + j * n * 2 + i)) > fabs(*(M_temp + k * n * 2 + i))) { k = j; } } if (fabs(*(M_temp + k * n * 2 + i)) < EPS) { return; } if (k != i) { /* temp = *(M_temp + k * 2 * n + 0); temp1 = *(M_temp + i * 2 * n + 0); *(M_temp + i * 2 * n + 0) = temp; *(M_temp + k * 2 * n + 0) = temp1; */ //交换第k行和第i行的值 for (int ii = 0; ii < 2 * n; ii++) { temp = *(M_temp + k * 2 * n + ii); temp1 = *(M_temp + i * 2 * n + ii); *(M_temp + i * 2 * n + ii) = temp; *(M_temp + k * 2 * n + ii) = temp1; } } temp = *(M_temp + i * 2 * n + i); for (int j = 0; j < 2 * n; j++) { temp1 = (*(M_temp + i * 2 * n + j)) / temp; *(M_temp + i * 2 * n + j) = temp1; } for (int j = i + 1; j < n; j++) { temp2 = *(M_temp + j * 2 * n + i); for (int k = 0; k < 2 * n; k++) { temp = temp2 * (*(M_temp + i * 2 * n + k)); temp1 = (*(M_temp + j * 2 * n + k)) - temp; *(M_temp + j * 2 * n + k) = temp1; } } } //对M反向带入，将其化为一个对角线形式 for (int i = n - 1; i >= 0; i--) { for (int j = i - 1; j >= 0; j--) { temp2 = *(M_temp + j * 2 * n + i); for (int k = 0; k < 2 * n; k++) { temp1 = temp2 * (*(M_temp + i * 2 * n + k)); temp = (*(M_temp + j * 2 * n + k)) - temp1; *(M_temp + j * 2 * n + k) = temp; } } } //将M的右半部分取出 for (int i = 0; i < n; i++) { for (int j = 0; j < n; j++) { temp = *(M_temp + i * 2 * n + n + j); *(Inv_A + i * n + j) = temp; } } }