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