public static double[,] GetTransformMatrix(MDHParameters parameters)
{
double crx = Math.Cos(parameters.Alpha);
double srx = Math.Sin(parameters.Alpha);
double crz = Math.Cos(parameters.Theta);
double srz = Math.Sin(parameters.Theta);
double a = parameters.A;
double d = parameters.D;
double[,] result =
{
{ crz, -srz, 0, a },
{ crx *srz, crx *crz, -srx, -d *srx },
{ srx *srz, crz *srx, crx, d *crx },
{ 0, 0, 0, 1 }
};
return(result);
}
public static double[] GetPosition(MDHParameters parameters)
{
return(GetPositionVector(GetTransformMatrix(parameters)));
}