#include "math.h"
int N=4;
int M=4;
float a[4][4]={
{1,0,0,0},
{1,0.5,0,0},
{1,0,1,0},
{1,0,0,1},
};
float **b = new float *[4]; // 拷贝a
int matrixInversion(float **a, int n)
{
int *is = new int[n];
int *js = new int[n];
int i,j,k;
double d,p;
for ( k = 0; k < n; k++)
{
d = 0.0;
for (i=k; i<=n-1; i++)
for (j=k; j<=n-1; j++)
{
p=fabs(a[i][j]);
if (p>d) { d=p; is[k]=i; js[k]=j;}
}
if ( 0.0 == d )
{
free(is); free(js); Serial.println("err**not inv\n");
return(0);
}
if (is[k]!=k)
for (j=0; j<=n-1; j++)
{
p=a[k][j];
a[k][j]=a[is[k]][j];
a[is[k]][j]=p;
}
if (js[k]!=k)
for (i=0; i<=n-1; i++)
{
p=a[i][k];
a[i][k]=a[i][js[k]];
a[i][js[k]]=p;
}
a[k][k] = 1.0/a[k][k];
for (j=0; j<=n-1; j++)
if (j!=k)
{
a[k][j] *= a[k][k];
}
for (i=0; i<=n-1; i++)
if (i!=k)
for (j=0; j<=n-1; j++)
if (j!=k)
{
a[i][j] -= a[i][k]*a[k][j];
}
for (i=0; i<=n-1; i++)
if (i!=k)
{
a[i][k] = -a[i][k]*a[k][k];
}
}
for ( k = n-1; k >= 0; k--)
{
if (js[k]!=k)
for (j=0; j<=n-1; j++)
{
p = a[k][j];
a[k][j] = a[js[k]][j];
a[js[k]][j]=p;
}
if (is[k]!=k)
for (i=0; i<=n-1; i++)
{
p = a[i][k];
a[i][k]=a[i][is[k]];
a[i][is[k]] = p;
}
}
free(is); free(js);
return(1);
}
void matrix_result(int){
int i,j;
for (i=0; i< 4; i++)
{
b[i] = new float[4];
for (j=0; j< 4; j++)
b[i][j]=a[i][j]; // 拷贝a
}
i=matrixInversion(b,4); // 计算逆矩阵，结果在b中
if (i!=0)
{
Serial.print("\nMAT A IS:");
for (i=0; i<=3; i++)
{ Serial.println();
for (j=0; j<=3; j++)
{ Serial.print(a[i][j]);Serial.print(" , ");}
}
Serial.print("\nMAT A- IS:");
for (i=0; i<=3; i++)
{
Serial.println("");
for (j=0; j<=3; j++)
{ Serial.print(b[i][j]);Serial.print(" , ");}
}
}
}
void setup() {
Serial.begin(115200);
matrix_result();
}
void loop() {
}