//矩阵乘法
void pbgMatricMul(zdouble_t C[DN * DN], double A[DN * DN],double B[DN * DN])
{
for (int32_t i = 0; i < DN; i++)
{
for (int32_t j = 0; j < DN; j++)
{
for (int32_t k = 0; k < DN; k++)
{
C[i * DN + j] += A[ i * DN + k] * B[k * DN + j];
}
}
}
//若绝对值小于10^-5,则置为0
for (int32_t i = 0; i < DN * DN ; i++)
{
if (abs(C[i]) < 1e-5)
{
C[i] = 0;
}
}
}
//LUP分解
void pbgLupDescomposition(zdouble_t A[DN * DN], zdouble_t L[DN * DN], zdouble_t U[DN * DN], int32_t P[DN])
{
int32_t row = 0;
for (int32_t i = 0;i < DN; i++)
{
P[i] = i;
}
for (int32_t i = 0; i < DN - 1;i++)
{
zdouble_t p = 0.0;
for (int32_t j = i; j < DN; j++)
{
if (abs( A[j * DN + i]) > p)
{
p = abs(A[j * DN + i]);
row = j;
}
}
if(0 == p)
{
//cout<< "矩阵奇异,无法计算逆" <<endl;
return ;
}
//交换P[i]和P[row]
int32_t tmp = P[i];
P[i] = P[row];
P[row] = tmp;
zdouble_t tmp2 = 0.0;
for (int32_t j = 0; j < DN; j++)
{
//交换A[i][j]和 A[row][j]
tmp2 = A[i * DN + j];
A[i * DN + j] = A[row * DN + j];
A[row * DN + j] = tmp2;
}
//以下同LU分解
zdouble_t u = A[i * DN + i], l = 0.0;
for (int32_t j = i + 1; j < DN; j++)
{
l = A[j * DN + i] / u;
A[j * DN + i] = l;
for (int32_t k = i + 1; k < DN; k ++)
{
A[j * DN + k] = A[j * DN + k] - A[i * DN + k] * l;
}
}
}
//构造L和U
for (int32_t i = 0; i < DN; i++)
{
for (int32_t j = 0; j <= i; j++)
{
if (i != j)
{
L[i * DN + j] = A[i * DN + j];
}
else
{
L[i * DN + j] = 1;
}
}
for (int32_t k = i; k < DN; k++)
{
U[i * DN + k] = A[i * DN + k];
}
}
}
//LUP求解方程
zdouble_t * pbgLupSolve(zdouble_t L[DN * DN], zdouble_t U[DN * DN], int P[DN], zdouble_t b[DN])
{
zdouble_t *x=new double[DN]();
zdouble_t *y=new double[DN]();
//正向替换
for (int32_t i = 0;i < DN;i++)
{
y[i] = b[P[i]];
for (int32_t j = 0;j < i;j++)
{
y[i] = y[i] - L[i * DN + j] * y[j];
}
}
//反向替换
for (int32_t i = DN-1;i >= 0; i--)
{
x[i] = y[i];
for (int32_t j = DN-1;j > i;j--)
{
x[i] = x[i] - U[i * DN + j] * x[j];
}
x[i] /= U[i * DN + i];
}
return x;
}
/*****************矩阵原地转置BEGIN********************/
/* 后继 */
int32_t pbgGetNext(int32_t i, int32_t m, int32_t n)
{
return (i % n) * m + i / n;
}
/* 前驱 */
int32_t pbgGetPre(int32_t i, int32_t m, int32_t n)
{
return (i % m) * n + i / m;
}
/* 处理以下标i为起点的环 */
void pbgMovedata(zdouble_t *mtx, int32_t i, int32_t m, int32_t n)
{
zdouble_t temp = mtx[i]; // 暂存
int32_t cur = i; // 当前下标
int32_t pre = pbgGetPre(cur, m, n);
while(pre != i)
{
mtx[cur] = mtx[pre];
cur = pre;
pre = pbgGetPre(cur, m, n);
}
mtx[cur] = temp;
}
/* 转置,即循环处理所有环 */
void pbgTranspose(zdouble_t *mtx, int32_t m, int32_t n)
{
for (int32_t i =0; i < m * n; ++i)
{
int32_t next = pbgGetNext(i, m, n);
while(next > i) // 若存在后继小于i说明重复,就不进行下去了(只有不重复时进入while循环)
{
next = pbgGetNext(next, m, n);
}
if(next == i) // 处理当前环
{
pbgMovedata(mtx, i, m, n);
}
}
}
/*****************矩阵原地转置END********************/
//LUP求逆(将每列b求出的各列x进行组装)
void pbgLupSolveInverse(zdouble_t inv_A[DN * DN], zdouble_t A[DN * DN])
{
//创建矩阵A的副本,注意不能直接用A计算,因为LUP分解算法已将其改变
zdouble_t *A_mirror = new zdouble_t[DN * DN]();
//zdouble_t *inv_A = new zdouble_t[DN * DN]();//最终的逆矩阵(还需要转置)
zdouble_t *inv_A_each = new zdouble_t[DN]();//矩阵逆的各列
//double *B =new double[DN*DN]();
zdouble_t *b =new zdouble_t[DN]();//b阵为B阵的列矩阵分量
for(int32_t i = 0; i < DN; i++)
{
zdouble_t *L = new zdouble_t[DN * DN]();
zdouble_t *U = new zdouble_t[DN * DN]();
int32_t *P = new int32_t[DN]();
//构造单位阵的每一列
for(int32_t j = 0; j < DN; j++)
{
b[j] = 0;
}
b[i] = 1;
//每次都需要重新将A复制一份
for(int32_t j = 0; j < DN * DN; j++)
{
A_mirror[j] = A[j];
}
pbgLupDescomposition(A_mirror, L, U, P);
inv_A_each = pbgLupSolve (L, U, P, b);
memcpy(inv_A + i * DN, inv_A_each, DN * sizeof(zdouble_t));//将各列拼接起来
}
pbgTranspose(inv_A, DN, DN);//由于现在根据每列b算出的x按行存储,因此需转置
// return inv_A;
}