//矩阵相乘实现
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(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);
}