private void MultiplyBtn_Click(object sender, EventArgs e) { SetMatrixAB(); Matrix MatrixResult = MatrixOperator.MultiplyMatrices(MatrixA, MatrixB); DisplayMatrix(MatrixResult, resultText); }
public object Convert(object value, Type targetType, object parameter, CultureInfo culture) { try { MatrixOperator op = (MatrixOperator)value; switch (op) { case MatrixOperator.Add: return("+"); case MatrixOperator.Sub: return("-"); case MatrixOperator.Multiply: return("x"); case MatrixOperator.First: return("<"); case MatrixOperator.Second: return(">"); default: return("<"); } } catch { return("<"); } }
//矩阵求逆(伴随矩阵法) 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); }
public static void Main() { string[] args = Console.ReadLine() .Split(new[] { ' ' }, StringSplitOptions.RemoveEmptyEntries); ArrayParser parser = new ArrayParser(); int[] dimensions = parser.IntegersParser(args); int rows = dimensions[0]; int cols = dimensions[1]; MatrixGenerator matrixGenerator = new MatrixGenerator(); MatrixOperator matrixOperator = new MatrixOperator(); int[,] matrix = matrixGenerator.GenerateIntegersMatrix(rows, cols); string command = null; long sum = 0; while ((command = Console.ReadLine()) != "Let the Force be with you") { string[] ivoArgs = command.Split(); int[] ivoCoordinates = parser.IntegersParser(ivoArgs); string[] evilArgs = Console.ReadLine().Split(); int[] evilCoordinates = parser.IntegersParser(evilArgs); int evilRow = evilCoordinates[0]; int evilCol = evilCoordinates[1]; matrixOperator.DeleteElementsFromBottomRightToUpperLeft(matrix, evilRow, evilCol); int ivoRow = ivoCoordinates[0]; int ivoCol = ivoCoordinates[1]; sum += matrixOperator.SumElementsFromBottomLeftToUpperRight(matrix, ivoRow, ivoCol); } Console.WriteLine(sum); }
private static void DealOrinData2(ref Matrix returnData) { Matrix TempMatrix = new Matrix(4, 4, "TempMatrix"); TempMatrix.Detail[0, 0] = 1; TempMatrix.Detail[0, 1] = 0; TempMatrix.Detail[0, 2] = 0; TempMatrix.Detail[0, 3] = 0; TempMatrix.Detail[1, 0] = 0; TempMatrix.Detail[1, 1] = 1; TempMatrix.Detail[1, 2] = 0; TempMatrix.Detail[1, 3] = 0; TempMatrix.Detail[2, 0] = 0; TempMatrix.Detail[2, 1] = 0; TempMatrix.Detail[2, 2] = 1; TempMatrix.Detail[2, 3] = -215; TempMatrix.Detail[3, 0] = 0; TempMatrix.Detail[3, 1] = 0; TempMatrix.Detail[3, 2] = 0; TempMatrix.Detail[3, 3] = 1; returnData = MatrixOperator.MatrixMulti(returnData, MatrixOperator.MatrixInvByCom(TempMatrix)); }
/// <summary> /// 计算theta4、6 /// </summary> /// <param name="theta1"></param> /// <param name="theta2"></param> /// <param name="theta3"></param> /// <param name="theta5"></param> /// <param name="PValue"></param> /// <param name="T06"></param> /// <param name="i"></param> /// <param name="theta"></param> private static void CalculateTheta4_6(double theta1, double theta2, double theta3, double theta5, double[] PValue, Matrix T06, int i, double[] oldtheta, ref double[,] theta) { Debug.Log("============================计算theta4_6============================"); #region//前三个矩阵 Matrix T01 = new Matrix(4, 4, "T01"); T01.Detail[0, 0] = Math.Cos(theta1 / 180D * Math.PI); T01.Detail[0, 1] = -Math.Sin(theta1 / 180D * Math.PI); T01.Detail[0, 2] = 0; T01.Detail[0, 3] = 0; T01.Detail[1, 0] = Math.Sin(theta1 / 180D * Math.PI); T01.Detail[1, 1] = Math.Cos(theta1 / 180D * Math.PI); T01.Detail[1, 2] = 0; T01.Detail[1, 3] = 0; T01.Detail[2, 0] = 0; T01.Detail[2, 1] = 0; T01.Detail[2, 2] = 1; T01.Detail[2, 3] = PValue[0]; T01.Detail[3, 0] = 0; T01.Detail[3, 1] = 0; T01.Detail[3, 2] = 0; T01.Detail[3, 3] = 1; Matrix T12 = new Matrix(4, 4, "T12"); T12.Detail[0, 0] = Math.Sin(theta2 / 180D * Math.PI); T12.Detail[0, 1] = Math.Cos(theta2 / 180D * Math.PI); T12.Detail[0, 2] = 0; T12.Detail[0, 3] = PValue[1]; T12.Detail[1, 0] = 0; T12.Detail[1, 1] = 0; T12.Detail[1, 2] = 1; T12.Detail[1, 3] = 0; T12.Detail[2, 0] = Math.Cos(theta2 / 180D * Math.PI);; T12.Detail[2, 1] = -Math.Sin(theta2 / 180D * Math.PI); T12.Detail[2, 2] = 0; T12.Detail[2, 3] = 0; T12.Detail[3, 0] = 0; T12.Detail[3, 1] = 0; T12.Detail[3, 2] = 0; T12.Detail[3, 3] = 1; Matrix T23 = new Matrix(4, 4, "T23"); T23.Detail[0, 0] = Math.Cos(theta3 / 180D * Math.PI); T23.Detail[0, 1] = -Math.Sin(theta3 / 180D * Math.PI); T23.Detail[0, 2] = 0; T23.Detail[0, 3] = PValue[3]; T23.Detail[1, 0] = -Math.Sin(theta3 / 180D * Math.PI); T23.Detail[1, 1] = -Math.Cos(theta3 / 180D * Math.PI); T23.Detail[1, 2] = 0; T23.Detail[1, 3] = 0; T23.Detail[2, 0] = 0;; T23.Detail[2, 1] = 0; T23.Detail[2, 2] = -1; T23.Detail[2, 3] = 0; T23.Detail[3, 0] = 0; T23.Detail[3, 1] = 0; T23.Detail[3, 2] = 0; T23.Detail[3, 3] = 1; #endregion Matrix iT03_T06 = MatrixOperator.MatrixMulti(MatrixOperator.MatrixInvByCom(MatrixOperator.MatrixMulti(MatrixOperator.MatrixMulti(T01, T12), T23)), T06); if (Math.Floor(Math.Abs(theta5)) == 0) { Debug.Log("theta5为0,其值为:" + theta5); theta[i, 3] = oldtheta[3]; theta[i, 5] = Math.Atan2(-iT03_T06.Detail[2, 0], -iT03_T06.Detail[2, 1]) / Math.PI * 180 - oldtheta[3]; } else { theta[i, 3] = Math.Atan2(iT03_T06.Detail[2, 2] * Math.Sin(theta5 / 180D * Math.PI), -iT03_T06.Detail[0, 2] * Math.Sin(theta5 / 180D * Math.PI)) / Math.PI * 180; theta[i, 5] = Math.Atan2(-iT03_T06.Detail[1, 1] * Math.Sin(theta5 / 180D * Math.PI), iT03_T06.Detail[1, 0] * Math.Sin(theta5 / 180D * Math.PI)) / Math.PI * 180; } Debug.Log("============================结束theta4_6============================"); }
/// <summary> /// 计算theta5 /// </summary> /// <param name="theta1"></param> /// <param name="theta2"></param> /// <param name="theta3"></param> /// <param name="PValue"></param> /// <param name="T06"></param> /// <param name="i"></param> /// <param name="theta"></param> private static void CalculateTheta5(double theta1, double theta2, double theta3, double[] PValue, Matrix T06, int i, ref double[,] theta) { Debug.Log("============================计算theta5============================"); #region//前三个矩阵 Matrix T01 = new Matrix(4, 4, "T01"); T01.Detail[0, 0] = Math.Cos(theta1 / 180D * Math.PI); T01.Detail[0, 1] = -Math.Sin(theta1 / 180D * Math.PI); T01.Detail[0, 2] = 0; T01.Detail[0, 3] = 0; T01.Detail[1, 0] = Math.Sin(theta1 / 180D * Math.PI); T01.Detail[1, 1] = Math.Cos(theta1 / 180D * Math.PI); T01.Detail[1, 2] = 0; T01.Detail[1, 3] = 0; T01.Detail[2, 0] = 0; T01.Detail[2, 1] = 0; T01.Detail[2, 2] = 1; T01.Detail[2, 3] = PValue[0]; T01.Detail[3, 0] = 0; T01.Detail[3, 1] = 0; T01.Detail[3, 2] = 0; T01.Detail[3, 3] = 1; Matrix T12 = new Matrix(4, 4, "T12"); T12.Detail[0, 0] = Math.Sin(theta2 / 180D * Math.PI); T12.Detail[0, 1] = Math.Cos(theta2 / 180D * Math.PI); T12.Detail[0, 2] = 0; T12.Detail[0, 3] = PValue[1]; T12.Detail[1, 0] = 0; T12.Detail[1, 1] = 0; T12.Detail[1, 2] = 1; T12.Detail[1, 3] = 0; T12.Detail[2, 0] = Math.Cos(theta2 / 180D * Math.PI);; T12.Detail[2, 1] = -Math.Sin(theta2 / 180D * Math.PI); T12.Detail[2, 2] = 0; T12.Detail[2, 3] = 0; T12.Detail[3, 0] = 0; T12.Detail[3, 1] = 0; T12.Detail[3, 2] = 0; T12.Detail[3, 3] = 1; Matrix T23 = new Matrix(4, 4, "T23"); T23.Detail[0, 0] = Math.Cos(theta3 / 180D * Math.PI); T23.Detail[0, 1] = -Math.Sin(theta3 / 180D * Math.PI); T23.Detail[0, 2] = 0; T23.Detail[0, 3] = PValue[3]; T23.Detail[1, 0] = -Math.Sin(theta3 / 180D * Math.PI); T23.Detail[1, 1] = -Math.Cos(theta3 / 180D * Math.PI); T23.Detail[1, 2] = 0; T23.Detail[1, 3] = 0; T23.Detail[2, 0] = 0;; T23.Detail[2, 1] = 0; T23.Detail[2, 2] = -1; T23.Detail[2, 3] = 0; T23.Detail[3, 0] = 0; T23.Detail[3, 1] = 0; T23.Detail[3, 2] = 0; T23.Detail[3, 3] = 1; #endregion Matrix iT03_T06 = MatrixOperator.MatrixMulti(MatrixOperator.MatrixInvByCom(MatrixOperator.MatrixMulti(MatrixOperator.MatrixMulti(T01, T12), T23)), T06); double c5 = iT03_T06.Detail[1, 2]; double s5 = Math.Sqrt(1 - Math.Pow(c5, 2)); if (i % 2 != 0) { s5 = -Math.Sqrt(1 - Math.Pow(c5, 2)); } theta[i, 4] = Math.Atan2(s5, c5) / Math.PI * 180; Debug.Log("============================结束theta5============================"); }
/// <summary> /// 输入数据xyzwpr换算为T06的计算 /// </summary> /// <param name="IKInput"></param> /// <returns></returns> private static Matrix DealOrinData(double[] IKInput) { Debug.Log("开始==================输入数据处理为矩阵====================="); #region //变量定义 Matrix returnData = new Matrix(4, 4, "T06"); //T06:{6}在{0}下的位姿 Matrix XW = new Matrix(3, 3, "R(X,W)"); //沿着x旋转的矩阵 Matrix YP = new Matrix(3, 3, "R(Y,P)"); //沿着Y旋转的矩阵 Matrix ZR = new Matrix(3, 3, "R(Z,R)"); //沿着Z旋转的矩阵 Matrix temp = new Matrix(3, 3, "R06"); //过渡 #endregion #region//X的赋值 XW.Detail[0, 0] = 1; XW.Detail[0, 1] = 0; XW.Detail[0, 2] = 0; XW.Detail[1, 0] = 0; XW.Detail[1, 1] = Math.Cos(IKInput[3] / 180D * Math.PI);//度对应的数 XW.Detail[1, 2] = -Math.Sin(IKInput[3] / 180D * Math.PI); XW.Detail[2, 0] = 0; XW.Detail[2, 1] = Math.Sin(IKInput[3] / 180D * Math.PI); XW.Detail[2, 2] = Math.Cos(IKInput[3] / 180D * Math.PI); #endregion #region//Y的赋值 YP.Detail[0, 0] = Math.Cos(IKInput[4] / 180D * Math.PI); YP.Detail[0, 1] = 0; YP.Detail[0, 2] = Math.Sin(IKInput[4] / 180D * Math.PI); YP.Detail[1, 0] = 0; YP.Detail[1, 1] = 1; YP.Detail[1, 2] = 0; YP.Detail[2, 0] = -Math.Sin(IKInput[4] / 180D * Math.PI); YP.Detail[2, 1] = 0; YP.Detail[2, 2] = Math.Cos(IKInput[4] / 180D * Math.PI); #endregion #region//Z的赋值 ZR.Detail[0, 0] = Math.Cos(IKInput[5] / 180D * Math.PI); ZR.Detail[0, 1] = -Math.Sin(IKInput[5] / 180D * Math.PI); ZR.Detail[0, 2] = 0; ZR.Detail[1, 0] = Math.Sin(IKInput[5] / 180D * Math.PI); ZR.Detail[1, 1] = Math.Cos(IKInput[5] / 180D * Math.PI); ZR.Detail[1, 2] = 0; ZR.Detail[2, 0] = 0; ZR.Detail[2, 1] = 0; ZR.Detail[2, 2] = 1; #endregion #region//数据精度整理 for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { if (Math.Abs(XW.Detail[i, j]) < 0.0000001) { XW.Detail[i, j] = 0; } if (Math.Abs(YP.Detail[i, j]) < 0.0000001) { YP.Detail[i, j] = 0; } if (Math.Abs(ZR.Detail[i, j]) < 0.0000001) { ZR.Detail[i, j] = 0; } } } #endregion #region//XYZ旋转矩阵数据计算打印 //for (int i = 0; i < 3; i++) //{ // Debug.Log("XW" + i + ": " + XW.Detail[i, 0] + " " + XW.Detail[i, 1] + " " + XW.Detail[i, 2]); // Debug.Log("YP" + i + ": " + YP.Detail[i, 0] + " " + YP.Detail[i, 1] + " " + YP.Detail[i, 2]); // Debug.Log("ZR" + i + ": " + ZR.Detail[i, 0] + " " + ZR.Detail[i, 1] + " " + ZR.Detail[i, 2]); //} #endregion temp = MatrixOperator.MatrixMulti(MatrixOperator.MatrixMulti(ZR, YP), XW); #region//赋值:T其余数值 for (int i = 0; i < returnData.getM - 1; i++) { for (int j = 0; j < returnData.getN - 1; j++) { returnData.Detail[i, j] = temp.Detail[i, j]; } } returnData.Detail[0, 3] = IKInput[0]; returnData.Detail[1, 3] = IKInput[1]; returnData.Detail[2, 3] = IKInput[2]; returnData.Detail[3, 0] = 0; returnData.Detail[3, 1] = 0; returnData.Detail[3, 2] = 0; returnData.Detail[3, 3] = 1; #endregion DealOrinData2(ref returnData); #region//T的打印 for (int i = 0; i < returnData.getM; i++) { Debug.Log("第" + i + "行: " + returnData.Detail[i, 0] + " " + returnData.Detail[i, 1] + " " + returnData.Detail[i, 2] + " " + returnData.Detail[i, 3]); } #endregion Debug.Log("结束==================输入数据处理为矩阵====================="); return(returnData); }
/// <summary> /// 机器人正解 /// </summary> /// <param name="theta">输入的六个关节角度</param> /// <param name="staticValue">DH链表中机器人的尺寸</param> /// <param name="TempMatrix">6个旋转矩阵的初始化</param> /// <returns>正解得出的T06</returns> private static Matrix FK(double[] theta, double[] staticValue, ref List <Matrix> TempMatrix) { Debug.Log("============================计算正解============================"); Debug.Log("所输入的角度为:" + theta[0] + " " + theta[1] + " " + theta[2] + " " + theta[3] + " " + theta[4] + " " + theta[5] + " "); Matrix ReturnMatrix = new Matrix(4, 4, "正解T06"); #region//给6个位置矩阵赋值 for (int i = 0; i < TempMatrix.Count; i++) { switch (i) { case 0: TempMatrix[0].Detail[0, 0] = Math.Cos(theta[0]); TempMatrix[0].Detail[0, 1] = -Math.Sin(theta[0]); TempMatrix[0].Detail[0, 2] = 0; TempMatrix[0].Detail[0, 3] = 0; TempMatrix[0].Detail[1, 0] = Math.Sin(theta[0]); TempMatrix[0].Detail[1, 1] = Math.Cos(theta[0]); TempMatrix[0].Detail[1, 2] = 0; TempMatrix[0].Detail[1, 3] = 0; TempMatrix[0].Detail[2, 0] = 0; TempMatrix[0].Detail[2, 1] = 0; TempMatrix[0].Detail[2, 2] = 1; TempMatrix[0].Detail[2, 3] = staticValue[0]; TempMatrix[0].Detail[3, 0] = 0; TempMatrix[0].Detail[3, 1] = 0; TempMatrix[0].Detail[3, 2] = 0; TempMatrix[0].Detail[3, 3] = 1; break; case 1: TempMatrix[1].Detail[0, 0] = Math.Sin(theta[1]); TempMatrix[1].Detail[0, 1] = Math.Cos(theta[1]); TempMatrix[1].Detail[0, 2] = 0; TempMatrix[1].Detail[0, 3] = staticValue[1]; TempMatrix[1].Detail[1, 0] = 0; TempMatrix[1].Detail[1, 1] = 0; TempMatrix[1].Detail[1, 2] = 1; TempMatrix[1].Detail[1, 3] = 0; TempMatrix[1].Detail[2, 0] = Math.Cos(theta[1]); TempMatrix[1].Detail[2, 1] = -Math.Sin(theta[1]); TempMatrix[1].Detail[2, 2] = 0; TempMatrix[1].Detail[2, 3] = 0; TempMatrix[1].Detail[3, 0] = 0; TempMatrix[1].Detail[3, 1] = 0; TempMatrix[1].Detail[3, 2] = 0; TempMatrix[1].Detail[3, 3] = 1; break; case 2: //TempMatrix[2].Detail[0, 0] = Math.Cos(theta[2] + theta[1]);用于没使用最优解时 //TempMatrix[2].Detail[0, 1] = -Math.Sin(theta[2] + theta[1]); TempMatrix[2].Detail[0, 0] = Math.Cos(theta[2]); TempMatrix[2].Detail[0, 1] = -Math.Sin(theta[2]); TempMatrix[2].Detail[0, 2] = 0; TempMatrix[2].Detail[0, 3] = staticValue[2]; //TempMatrix[2].Detail[1, 0] = -Math.Sin(theta[2] + theta[1]); //TempMatrix[2].Detail[1, 1] = -Math.Cos(theta[2] + theta[1]); TempMatrix[2].Detail[1, 0] = -Math.Sin(theta[2]); TempMatrix[2].Detail[1, 1] = -Math.Cos(theta[2]); TempMatrix[2].Detail[1, 2] = 0; TempMatrix[2].Detail[1, 3] = 0; TempMatrix[2].Detail[2, 0] = 0; TempMatrix[2].Detail[2, 1] = 0; TempMatrix[2].Detail[2, 2] = -1; TempMatrix[2].Detail[2, 3] = 0; TempMatrix[2].Detail[3, 0] = 0; TempMatrix[2].Detail[3, 1] = 0; TempMatrix[2].Detail[3, 2] = 0; TempMatrix[2].Detail[3, 3] = 1; break; case 3: TempMatrix[3].Detail[0, 0] = Math.Cos(theta[3]); TempMatrix[3].Detail[0, 1] = -Math.Sin(theta[3]); TempMatrix[3].Detail[0, 2] = 0; TempMatrix[3].Detail[0, 3] = staticValue[3]; TempMatrix[3].Detail[1, 0] = 0; TempMatrix[3].Detail[1, 1] = 0; TempMatrix[3].Detail[1, 2] = 1; TempMatrix[3].Detail[1, 3] = -staticValue[4]; TempMatrix[3].Detail[2, 0] = -Math.Sin(theta[3]);; TempMatrix[3].Detail[2, 1] = -Math.Cos(theta[3]); TempMatrix[3].Detail[2, 2] = 0; TempMatrix[3].Detail[2, 3] = 0; TempMatrix[3].Detail[3, 0] = 0; TempMatrix[3].Detail[3, 1] = 0; TempMatrix[3].Detail[3, 2] = 0; TempMatrix[3].Detail[3, 3] = 1; break; case 4: TempMatrix[4].Detail[0, 0] = Math.Cos(theta[4]); TempMatrix[4].Detail[0, 1] = -Math.Sin(theta[4]); TempMatrix[4].Detail[0, 2] = 0; TempMatrix[4].Detail[0, 3] = 0; TempMatrix[4].Detail[1, 0] = 0; TempMatrix[4].Detail[1, 1] = 0; TempMatrix[4].Detail[1, 2] = -1; TempMatrix[4].Detail[1, 3] = 0; TempMatrix[4].Detail[2, 0] = Math.Sin(theta[4]);; TempMatrix[4].Detail[2, 1] = Math.Cos(theta[4]); TempMatrix[4].Detail[2, 2] = 0; TempMatrix[4].Detail[2, 3] = 0; TempMatrix[4].Detail[3, 0] = 0; TempMatrix[4].Detail[3, 1] = 0; TempMatrix[4].Detail[3, 2] = 0; TempMatrix[4].Detail[3, 3] = 1; break; case 5: TempMatrix[5].Detail[0, 0] = Math.Cos(theta[5]); TempMatrix[5].Detail[0, 1] = -Math.Sin(theta[5]); TempMatrix[5].Detail[0, 2] = 0; TempMatrix[5].Detail[0, 3] = 0; TempMatrix[5].Detail[1, 0] = 0; TempMatrix[5].Detail[1, 1] = 0; TempMatrix[5].Detail[1, 2] = 1; TempMatrix[5].Detail[1, 3] = 0; TempMatrix[5].Detail[2, 0] = -Math.Sin(theta[5]);; TempMatrix[5].Detail[2, 1] = -Math.Cos(theta[5]); TempMatrix[5].Detail[2, 2] = 0; TempMatrix[5].Detail[2, 3] = 0; TempMatrix[5].Detail[3, 0] = 0; TempMatrix[5].Detail[3, 1] = 0; TempMatrix[5].Detail[3, 2] = 0; TempMatrix[5].Detail[3, 3] = 1; break; default: break; } } #endregion #region//打印6个赋值了的位姿矩阵,看是否正确 //for (int j = 0; j < TempMatrix.Count; j++) //{ // Matrix item = TempMatrix[j]; // Debug.Log("这是矩阵:" + item.Name); // for (int i = 0; i < item.getM; i++) // { // Debug.Log("第" + i + "行:" + item.Detail[i, 0] + " " + item.Detail[i, 1] + " " + item.Detail[i, 2]); // } //} #endregion #region//6个位置矩阵相乘 ReturnMatrix = TempMatrix[0]; for (int i = 0; i < TempMatrix.Count - 1; i++) { ReturnMatrix = MatrixOperator.MatrixMulti(ReturnMatrix, TempMatrix[i + 1]); } #endregion for (int i = 0; i < ReturnMatrix.getM; i++) { Debug.Log(ReturnMatrix.Name + "的第" + i + "行:" + ReturnMatrix.Detail[i, 0] + " " + ReturnMatrix.Detail[i, 1] + " " + ReturnMatrix.Detail[i, 2] + " " + ReturnMatrix.Detail[i, 3]); } Debug.Log("============================结束正解============================"); return(ReturnMatrix); }