private void matriInit() { Rz0 = new double[, ] //二维嵌套数组 { { Math.Cos(angle_z1), -Math.Sin(angle_z1), 0 }, { Math.Sin(angle_z1), Math.Cos(angle_z1), 0 }, { 0, 0, 1 } }; Rx0 = new double[, ] { { 1, 0, 0 }, { 0, Math.Cos(angle_x1), -Math.Sin(angle_x1) }, { 0, Math.Sin(angle_x1), Math.Cos(angle_x1) } }; Ry0 = new double[, ] { { Math.Cos(angle_y1), 0, Math.Sin(angle_y1) }, { 0, 1, 0 }, { -Math.Sin(angle_y1), 0, Math.Cos(angle_y1) } }; Rz.Detail = Rz0; Rx.Detail = Rx0; Ry.Detail = Ry0; ROM = MatrixOperator.MatrixMulti(Rz, Ry); ROM = MatrixOperator.MatrixMulti(ROM, Rx); }
//矩阵求逆(伴随矩阵法) public static Matrix MatrixInvByCom(Matrix Ma) { double d = MatrixOperator.MatrixDet(Ma); if (d == 0) { Exception myException = new Exception("没有逆矩阵"); throw myException; } Matrix Ax = MatrixOperator.MatrixCom(Ma); Matrix An = MatrixOperator.MatrixSimpleMulti((1.0 / d), Ax); return(An); }
private void get_result() { Matrix R01 = new Matrix(3, 1); Matrix R02 = new Matrix(3, 1); R01 = MatrixOperator.MatrixMulti(ROM, B1); R02 = MatrixOperator.MatrixMulti(ROM, B2); R1 = MatrixOperator.MatrixSub(R01, A1); R2 = MatrixOperator.MatrixSub(R02, A2); L1 = (float)(getlength(R1)); L2 = (float)(getlength(R2)); L1_variation = L1 - l_right_1; L2_variation = L2 - l_left_2; }