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