public Matrix4_4 T01()
{
Matrix4_4 mat = new Matrix4_4();
mat.setM(1, 1, cos(th1));
mat.setM(1, 2, -sin(th1));
mat.setM(2, 1, sin(th1));
mat.setM(2, 2, cos(th1));
return(mat);
}
public Matrix4_4 T02()
{
float[,] m = new float[4, 4] {
{ cos(th1) * cos(th2), -cos(th1) * sin(th2), sin(th1), a2 *cos(th1) },
{ sin(th1) * cos(th2), -sin(th1) * sin(th2), -cos(th1), a2 *sin(th1) },
{ sin(th2), cos(th2), 0, 0 },
{ 0, 0, 0, 1 }
};
Matrix4_4 mat = new Matrix4_4();
mat.setM(m);
//check
/*
* Matrix4_4 rx2 = new Matrix4_4();
* Matrix4_4 rz2 = new Matrix4_4();
* rx2.Rx(alp2, a2);
* rz2.Rz(th2, d2);
* Matrix4_4 checkM = T01().x(rx2).x(rz2);
* mat.show("T02: ");
* checkM.show("T02 check: ");
*/
return(mat);
}
//反矩陣
public Matrix4_4 invert()
{
Matrix4_4 output = new Matrix4_4();
for (int i = 1; i <= 3; i++)
{
for (int j = 1; j <= 3; j++)
{
output.setM(i, j, matrix[j - 1, i - 1]);
}
//output.setM(i, 4, -matrix[i - 1, 3]);
}
output.setM(1, 4, -(matrix[0, 0] * matrix[0, 3] + matrix[1, 0] * matrix[1, 3] + matrix[2, 0] * matrix[2, 3]));
output.setM(2, 4, -(matrix[0, 1] * matrix[0, 3] + matrix[1, 1] * matrix[1, 3] + matrix[2, 1] * matrix[2, 3]));
output.setM(3, 4, -(matrix[0, 2] * matrix[0, 3] + matrix[1, 2] * matrix[1, 3] + matrix[2, 2] * matrix[2, 3]));
return(output);
}
public Matrix4_4 T36()
{
float[,] m = new float[4, 4] {
{ cos(th4) * cos(th5) * cos(th6) - sin(th4) * sin(th6), -cos(th4) * cos(th5) * sin(th6) - sin(th4) * cos(th5), cos(th4) * sin(th5), a4 + d6 * cos(th4) * sin(th5) },
{ sin(th5) * cos(th6), -sin(th5) * sin(th6), -cos(th5), -d4 - d6 * cos(th5) },
{ sin(th4) * cos(th5) * cos(th6) + cos(th4) * sin(th6), -sin(th4) * cos(th5) * sin(th6) + cos(th4) * cos(th6), sin(th4) * sin(th5), d6 *sin(th4) * sin(th5) },
{ 0, 0, 0, 1 }
};
Matrix4_4 mat = new Matrix4_4();
mat.setM(m);
return(mat);
}
public Matrix4_4 T03()
{
float[,] m = new float[4, 4] {
{ cos(th1) * cos(th2) * cos(th3) - cos(th1) * sin(th2) * sin(th3), -cos(th1) * cos(th2) * sin(th3) - cos(th1) * sin(th2) * cos(th3), sin(th1), a2 *cos(th1) + a3 * cos(th1) * cos(th2) },
{ sin(th1) * cos(th2) * cos(th3) - sin(th1) * sin(th2) * sin(th3), -sin(th1) * cos(th2) * sin(th3) - sin(th1) * sin(th2) * cos(th3), -cos(th1), a2 *sin(th1) + a3 * sin(th1) * cos(th2) },
{ sin(th2) * cos(th3) + cos(th2) * sin(th3), -sin(th2) * sin(th3) + cos(th2) * cos(th3), 0, a3 *sin(th2) },
{ 0, 0, 0, 1 }
};
Matrix4_4 mat = new Matrix4_4();
mat.setM(m);
//check
/*
* Matrix4_4 rx3 = new Matrix4_4();
* Matrix4_4 rz3 = new Matrix4_4();
* rx3.Rx(alp3, a3);
* rz3.Rz(th3, d3);
* Matrix4_4 checkM = T02().x(rx3).x(rz3);
* mat.show("T03 R:");
*/
return(mat);
}
public Matrix4_4 T04()
{
Matrix4_4 mat = new Matrix4_4();
mat.setM(1, 1, cos(th1) * cos(th2) * cos(th3) * cos(th4) - cos(th1) * sin(th2) * sin(th3) * cos(th4) + sin(th1) * sin(th4));
mat.setM(2, 1, sin(th1) * cos(th2) * cos(th3) * cos(th4) - sin(th1) * sin(th2) * sin(th3) * cos(th4) - cos(th1) * sin(th4));
mat.setM(3, 1, sin(th2) * cos(th3) * cos(th4) + cos(th2) * sin(th3) * cos(th4));
mat.setM(1, 2, -cos(th1) * cos(th2) * cos(th3) * sin(th4) + cos(th1) * sin(th2) * sin(th3) * sin(th4) + sin(th1) * cos(th4));
mat.setM(2, 2, -sin(th1) * cos(th2) * cos(th3) * sin(th4) + sin(th1) * sin(th2) * sin(th3) * sin(th4) - cos(th1) * cos(th4));
mat.setM(3, 2, -sin(th2) * cos(th3) * sin(th4) - cos(th2) * sin(th3) * sin(th4));
mat.setM(1, 3, cos(th1) * cos(th2) * sin(th3) + cos(th1) * sin(th2) * cos(th3));
mat.setM(2, 3, sin(th1) * cos(th2) * sin(th3) + sin(th1) * sin(th2) * cos(th3));
mat.setM(3, 3, sin(th2) * sin(th3) - cos(th2) * cos(th3));
mat.setM(1, 4, a2 * cos(th1) + a3 * cos(th1) * cos(th2) + a4 * cos(th1) * cos(th2) * cos(th3) - a4 * cos(th1) * sin(th2) * sin(th3) + d4 * cos(th1) * cos(th2) * sin(th3) + d4 * cos(th1) * sin(th2) * cos(th3));
mat.setM(2, 4, a2 * sin(th1) + a3 * sin(th1) * cos(th2) + a4 * sin(th1) * cos(th2) * cos(th3) - a4 * sin(th1) * sin(th2) * sin(th3) + d4 * sin(th1) * cos(th2) * sin(th3) + d4 * sin(th1) * sin(th2) * cos(th3));
mat.setM(3, 4, a3 * sin(th2) + a4 * sin(th2) * cos(th3) + a4 * cos(th2) * sin(th3) + d4 * sin(th2) * sin(th3) - d4 * cos(th2) * cos(th3));
//check
/*
* Matrix4_4 rx4 = new Matrix4_4();
* Matrix4_4 rz4 = new Matrix4_4();
* rx4.Rx(alp4, a4);
* rz4.Rz(th4, d4);
* Matrix4_4 checkM = T03().x(rx4).x(rz4);
*
* mat.show("T04 R:");
* checkM.show("T04 C:");
* mat.invert().show("invert");
* mat.invert().x(mat).show("I :");
*/
return(mat);
}
