//6軸座標旋轉
public bool rotateAxis6X(float[] angle, float rotateAngle)
{
setPreT06(angle);
Matrix4_4 flag;
Matrix4_4 rx = new Matrix4_4();
rx.Rx(rotateAngle);
flag = preM.x(rx);
preM.changeRotateMatrix(flag);
return(setUniversalCoordinate());
}
public Matrix4_4 T06()
{
//check
Matrix4_4 rx6 = new Matrix4_4();
Matrix4_4 rz6 = new Matrix4_4();
rx6.Rx(alp6, a6);
rz6.Rz(th6, d6);
Matrix4_4 mat = T05().x(rx6).x(rz6);
return(mat);
}
public Matrix4_4 T05()
{
//check
Matrix4_4 rx5 = new Matrix4_4();
Matrix4_4 rz5 = new Matrix4_4();
rx5.Rx(alp5, a5);
rz5.Rz(th5, d5);
Matrix4_4 mat = T04().x(rx5).x(rz5);
//mat.show();
return(mat);
}