//矩阵相乘实现
public static ClsMatrixCompute Mutiply(ClsMatrixCompute m1, ClsMatrixCompute m2)
{
double temp = 0;
ClsMatrixCompute ret;
if (m1.col != m2.row)
{
System.Exception e = new Exception("前者列数不等于后者行数，无法相乘");
throw e;
}
ret = new ClsMatrixCompute(m1.row, m1.col);
for (int i = 0; i < m1.row; i++)
{
for (int j = 0; j < m1.col; j++)
{
temp = 0;
for (int p = 0; p < m1.col; p++)
{
//temp += m1.getNum(i, p) + m2.getNum(p, i);
temp += m1.getNum(i, p) * m2.getNum(p, j);
}
ret.SetNum(i, j, temp);
}
}
return(ret);
}
//矩阵相加实现
public static ClsMatrixCompute Add(ClsMatrixCompute lm, ClsMatrixCompute rm)
{
//行出错
if (lm.row != rm.row)
{
System.Exception e = new Exception("相加的两个矩阵的行数不等");
throw e;
}
//列出错
if (lm.col != rm.col)
{
System.Exception e = new Exception("相加的两个矩阵的列数不等");
throw e;
}
ClsMatrixCompute another = new ClsMatrixCompute(lm.row, lm.col);
for (int i = 0; i < lm.row; i++)
{
for (int j = 0; j < lm.col; j++)
{
double temp = lm.getNum(i, j) + rm.getNum(i, j);
another.SetNum(i, j, temp);
}
}
return(another);
}
//矩阵转置实现
public ClsMatrixCompute Transpose()
{
ClsMatrixCompute another = new ClsMatrixCompute(row, col);
for (int i = 0; i < row; i++)
{
for (int j = 0; j < col; j++)
{
another.SetNum(j, i, matrix[i, j]);
}
}
return(another);
}
//复制构造函数
public ClsMatrixCompute(ClsMatrixCompute m)
{
int row = m.row;
int col = m.col;
matrix = new double[row, col];
for (int i = 0; i < row; i++)
{
for (int j = 0; j < col; j++)
{
matrix[i, j] = m.getNum(i, j);
}
}
}
//坐标转换
//input
//Array 原三维点坐标
//rotateMat 3*3旋转矩阵，按行存储
//tranVec 1*3 平移向量
//output result 转换后三维坐标
public static void coord_Trans(double[] Array, double[] rotateMat, double[] tranVec, double[] result)
{
ClsMatrixCompute mat1 = new ClsMatrixCompute(1, 3);
mat1.SetNum(0, 0, Array[0]);
mat1.SetNum(0, 1, Array[1]);
mat1.SetNum(0, 2, Array[2]);
ClsMatrixCompute rotMat = new ClsMatrixCompute(3, 3);
rotMat.SetNum(0, 0, rotateMat[0]);
rotMat.SetNum(0, 1, rotateMat[1]);
rotMat.SetNum(0, 2, rotateMat[2]);
rotMat.SetNum(1, 0, rotateMat[3]);
rotMat.SetNum(1, 1, rotateMat[4]);
rotMat.SetNum(1, 2, rotateMat[5]);
rotMat.SetNum(2, 0, rotateMat[6]);
rotMat.SetNum(2, 1, rotateMat[7]);
rotMat.SetNum(2, 2, rotateMat[8]);
ClsMatrixCompute matTrans = new ClsMatrixCompute(1, 3);
matTrans.SetNum(0, 0, tranVec[0]);
matTrans.SetNum(0, 1, tranVec[1]);
matTrans.SetNum(0, 2, tranVec[2]);
ClsMatrixCompute mat2 = new ClsMatrixCompute(1, 3);
ClsMatrixCompute matRes = new ClsMatrixCompute(1, 3);
mat2 = Mutiply(mat1, rotMat);
matRes = Add(mat2, matTrans);
result[0] = matRes.getNum(0, 0);
result[1] = matRes.getNum(0, 1);
result[2] = matRes.getNum(0, 2);
}
//矩阵求逆实现
public static ClsMatrixCompute Inverse(ClsMatrixCompute M)
{
int m = M.row;
int n = M.col;
if (m != n)
{
Exception myException = new Exception("求逆的矩阵不是方阵");
throw myException;
}
ClsMatrixCompute ret = new ClsMatrixCompute(m, n);
double[,] a0 = M.Detail;
double[,] a = (double[, ])a0.Clone();
double[,] b = ret.Detail;
int i, j, row, k;
double max, temp;
//单位矩阵
for (i = 0; i < n; i++)
{
b[i, i] = 1;
}
for (k = 0; k < n; k++)
{
max = 0; row = k;
//找最大元，其所在行为row
for (i = k; i < n; i++)
{
temp = Math.Abs(a[i, k]);
if (max < temp)
{
max = temp;
row = i;
}
}
if (max == 0)
{
Exception myException = new Exception("该矩阵无逆矩阵");
throw myException;
}
//交换k与row行
if (row != k)
{
for (j = 0; j < n; j++)
{
temp = a[row, j];
a[row, j] = a[k, j];
a[k, j] = temp;
temp = b[row, j];
b[row, j] = b[k, j];
b[k, j] = temp;
}
}
//首元化为1
for (j = k + 1; j < n; j++)
{
a[k, j] /= a[k, k];
}
for (j = 0; j < n; j++)
{
b[k, j] /= a[k, k];
}
a[k, k] = 1;
//k列化为0
//对a
for (j = k + 1; j < n; j++)
{
for (i = 0; i < k; i++)
{
a[i, j] -= a[i, k] * a[k, j];
}
for (i = k + 1; i < n; i++)
{
a[i, j] -= a[i, k] * a[k, j];
}
}
//对b
for (j = 0; j < n; j++)
{
for (i = 0; i < k; i++)
{
b[i, j] -= a[i, k] * b[k, j];
}
for (i = k + 1; i < n; i++)
{
b[i, j] -= a[i, k] * b[k, j];
}
}
for (i = 0; i < n; i++)
{
a[i, k] = 0;
}
a[k, k] = 1;
}
return(ret);
}
