矩阵运算
声明
bool VecPlus(const int n1, const double Vec1[], const int n2, const double Vec2[], double *result);
bool VecAdd(const int n1, const double Vec1[], const int n2, const double Vec2[], double result[]);
bool VecSub(const int n1, const double Vec1[], const int n2, const double Vec2[], double result[]);
bool VecCrossPlus(const int n1, const double Vec1[], const int n2, const double Vec2[], double result[]);
bool MatInit(const int row, const int col, double mat[]);
bool MatAdd(const int row1, const int col1, const int row2, const int col2, const double A[], const double B[], double output[]);
bool MatSub(const int row1, const int col1, const int row2, const int col2, const double A[], const double B[], double output[]);
bool MatMul_Num(const double k, const int row, const int col, const double A[], double output[]);
bool MatMul_Mat(const int row1, const int col1, const int row2, const int col2, const double A[], const double B[], double output[]);
bool MatMul_Poi(const int row1, const int col1, const int row2, const int col2, const double A[], const double B[], double output[]);
bool MatTrans(const int row, const int col, const double A[], double output[]);
double Calculate_Det(const double arr[], const int n);
bool MatInver(int n, double a[], double b[]);
bool MatBTQB(const int row1, const int col1, const int row2, const int col2, const double B[], const double Q[], double output[]);
bool MatEye(int n, double eye[]);
void PrintfMat(const int row, const int col, const double Mat[]);
实现
bool VecPlus(const int n1, const double Vec1[], const int n2, const double Vec2[], double *result)
{
int i =0 ;
*result = 0;
if (n1!=n2)
{
printf("Vector's dimensions must agree\n");
return false;
}
for (i; i < n1;i++)
{
*result = Vec2[i] * Vec1[i] + *result;
}
return true;
}
bool VecAdd(const int n1, const double Vec1[], const int n2, const double Vec2[], double result[])
{
int i = 0;
if (n1!=n2)
{
printf("Vector's dimensions must agree\n");
return false;
}
for (i; i < n1;i++)
{
result[i] = Vec1[i] + Vec2[i];
}
return true;
}
bool VecSub(const int n1, const double Vec1[], const int n2, const double Vec2[], double result[])
{
int i = 0;
if (n1 != n2)
{
printf("Vector's dimensions must agree\n");
return false;
}
for (i; i < n1; i++)
{
result[i] = Vec1[i] - Vec2[i];
}
return true;
}
bool VecCrossPlus(const int n1, const double Vec1[], const int n2, const double Vec2[], double result[])
{
int i = 0;
if (n1 != n2)
{
printf("Vector's dimensions must agree\n");
return false;
}
result[0] = Vec1[1] * Vec2[2] - Vec2[1] * Vec1[2];
result[1] = Vec1[2] * Vec2[0] - Vec1[0] * Vec2[2];
result[2] = Vec1[0] * Vec2[1] - Vec2[0] * Vec1[1];
return true;
}
bool MatInit(const int row,const int col,double mat[] )
{
if (row<0||col<0)
{
printf("Row and Col must be 0+\n");
return false;
}
for (int i = 0; i < row*col;i++)
{
mat[i] = 0;
}
return true;
}
bool MatAdd(const int row1, const int col1, const int row2, const int col2, const double A[], const double B[], double output[])
{
int i = 0;
if (row1 != row2 || col1 != col2)
{
printf("MatAdd:Inner matrix dimensions must agree.\n");
return false;
}
else
{
for ( i = 0; i < row1*col1; i++)
{
output[i] = A[i] + B[i];
}
return true;
}
}
bool MatSub(const int row1, const int col1, const int row2, const int col2, const double A[], const double B[], double output[])
{
int i = 0;
if (row1 != row2 || col1 != col2)
{
printf("MatAdd:Inner matrix dimensions must agree.\n");
return false;
}
else
{
for (int i = 0; i < row1*col1; i++)
{
output[i] = A[i] - B[i];
}
return true;
}
}
bool MatMul_Num(const double k, const int row, const int col,const double A[], double output[])
{
int i = 0;
for ( i = 0; i < row*col;i++)
{
output[i] = k*A[i];
}
return true;
}
bool MatMul_Mat(const int row1, const int col1, const int row2, const int col2, const double A[], const double B[], double output[])
{
int i, j, l;
if (col1!=row2)
{
printf("MatAdd:Inner matrix dimensions must agree.\n");
return false;
}
else
{
int u = 0;
for ( i = 0; i <= row1 - 1; i++)
for ( j = 0; j <= col2 - 1; j++)
{
u = i*col2 + j;
output[u] = 0.0;
for ( l = 0; l <= col1 - 1; l++)
{
output[u] = output[u] + A[i*col1 + l] * B[l*col2 + j];
}
}
return true;
}
}
bool MatMul_Poi(const int row1, const int col1, const int row2, const int col2, const double A[], const double B[], double output[])
{
int i;
if (row1!=row2||col1!=col2)
{
printf("MatAdd:Inner matrix dimensions must agree.\n");
return false;
}
else
{
for (i = 0; i < row1*col1; i++)
{
output[i] = A[i] * B[i];
}
return ture;
}
}
bool MatTrans(const int row, const int col, const double A[], double output[])
{
for (int i = 0; i < row; i++)
{
for (int j = 0; j < col;j++)
{
output[j*row + i] = A[i*col + j];
}
}
return true;
}
bool MatInver(int n, double a[], double b[])
{
int i, j, k, l, u, v, is[10], js[10];
double d, p;
if (n <= 0)
{
printf("Error dimension in MatrixInv!\n");
return false;
}
for (i = 0; i < n; i++)
{
for (j = 0; j < n; j++)
{
b[i*n + j] = a[i*n + j];
}
}
for (k = 0; k < n; k++)
{
d = 0.0;
for (i = k; i < n; i++)
{
for (j = k; j<n; j++)
{
l = n*i + j;
p = fabs(b[l]);
if (p>d)
{
d = p;
is[k] = i;
js[k] = j;
}
}
}
if (d < 2.2204460492503131e-16)
{
printf("Divided by 0 in MatrixInv!\n");
return false;
}
if (is[k] != k)
{
for (j = 0; j < n; j++)
{
u = k*n + j;
v = is[k] * n + j;
p = b[u];
b[u] = b[v];
b[v] = p;
}
}
if (js[k] != k)
{
for (i = 0; i < n; i++)
{
u = i*n + k;
v = i*n + js[k];
p = b[u];
b[u] = b[v];
b[v] = p;
}
}
l = k*n + k;
b[l] = 1.0 / b[l];
for (j = 0; j < n; j++)
{
if (j != k)
{
u = k*n + j;
b[u] = b[u] * b[l];
}
}
for (i = 0; i < n; i++)
{
if (i != k)
{
for (j = 0; j < n; j++)
{
if (j != k)
{
u = i*n + j;
b[u] = b[u] - b[i*n + k] * b[k*n + j];
}
}
}
}
for (i = 0; i < n; i++)
{
if (i != k)
{
u = i*n + k;
b[u] = -b[u] * b[l];
}
}
}
for (k = n - 1; k >= 0; k--)
{
if (js[k] != k)
{
for (j = 0; j < n; j++)
{
u = k*n + j;
v = js[k] * n + j;
p = b[u];
b[u] = b[v];
b[v] = p;
}
}
if (is[k] != k)
{
for (i = 0; i < n; i++)
{
u = i*n + k;
v = is[k] + i*n;
p = b[u];
b[u] = b[v];
b[v] = p;
}
}
}
return true;
}
double Calculate_Det(const double arr[],const int n)
{
double bak[100];
int i, j, k, c;
double sum = 0;
if (n == 1)
{
return arr[0];
}
for (i = 0; i < n; i++)
{
for (j = 1; j < n; j++)
{
for (c = 0, k = 0; k < n; k++)
{
if (k == i)
{
continue;
}
bak[(j - 1)*(n-1)+c] = arr[j*n+k];
c++;
}
}
double a = arr[i];
double tempt = sum;
sum += (i % 2 == 0 ? 1 : -1) * arr[i] * Calculate_Det(bak, n - 1);
}
return sum;
}
bool MatBTQB(const int row1, const int col1, const int row2, const int col2, const double B[], const double Q[],double output[])
{
int i = 0;
double BT[50 * 50];
MatTrans(32, 4, B, BT);
double BTQ[50 * 50];
MatMul_Mat(col1, row1, row2, col2, BT, Q, BTQ);
double BTQB[50 * 50];
MatMul_Mat(col1, col2, row1, col1, BTQ, B, BTQB);
for (i = 0; i < col1*col1;i++)
{
output[i] = BTQB[i];
}
return true;
}
bool MatEye(int n, double eye[])
{
int i = 0;
MatInit(n, n, eye);
for (i = 0; i < n; i++)
{
eye[i*n + i ] = 1;
}
return true;
}
void PrintfMat( const int row, const int col,const double Mat[])
{
printf("\n");
for (int i = 0; i < row*col;i++)
{
printf("%lf ", Mat[i]);
if ((i + 1) % col == 0)
{printf("\n");}
}
}