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============================");
}