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