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