public TransformationMatrix(URCoordinates TCP)
{
RotationVector RV = new RotationVector(TCP.Rx.rad, TCP.Ry.rad, TCP.Rz.rad);
RotateMatrix RM = RotationVetor2RotateMatrix(RV);
var R = RM.toArray();
Value = new double[, ] {
{ R[0, 0], R[0, 1], R[0, 2], TCP.X.M },
{ R[1, 0], R[1, 1], R[1, 2], TCP.Y.M },
{ R[2, 0], R[2, 1], R[2, 2], TCP.Z.M },
{ 0, 0, 0, 1 }
};
}
RotateMatrix RotationVetor2RotateMatrix(RotationVector RV)
{
double x = RV.Rx;
double y = RV.Ry;
double z = RV.Rz;
double theta = Math.Sqrt(x * x + y * y + z * z);
x /= theta;
y /= theta;
z /= theta;
double C = Math.Cos(theta);
double[,] M1 = new double[, ] {
{ C, 0, 0 }, { 0, C, 0 }, { 0, 0, C }
};
double[,] rrt = new double[, ] {
{ x *x, x *y, x *z }, { x *y, y *y, y *z }, { x *z, y *z, z *z }
};
double C1 = 1.0 - Math.Cos(theta);
double[,] M2 = new double[, ] {
{ x *x *C1, x *y *C1, x *z *C1 }, { x *y *C1, y *y *C1, y *z *C1 }, { x *z *C1, y *z *C1, z *z *C1 }
};
double S = Math.Sin(theta);
double[,] r_x = new double[, ] {
{ 0, -z, y }, { z, 0, -x }, { -y, x, 0 }
};
double[,] M3 = new double[, ] {
{ 0, -z * S, y *S }, { z *S, 0, -x *S }, { -y * S, x *S, 0 }
};
// R = cos(theta)*I + (1 - cos(theta))*r*rT + sin(theta)*[r_x]
double[,] R = new double[3, 3];
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
R[i, j] = M1[i, j] + M2[i, j] + M3[i, j];
}
}
return(new RotateMatrix(R));
}
