/// <summary> /// Compares axis information of two links /// </summary> /// <param name="source">First joint's axis information to be compared</param> /// <param name="exported">Second joint's axis information to be compared</param> /// <param name="indent">Indent level in the log file</param> /// <returns></returns> private bool CompareAxis(Joint.Axis source, Joint.Axis exported, int indent) { linkLog.AppendLine(String.Format("{0}Axis Checks", Indent(indent))); if (source == null && source == null) { linkLog.AppendLine(String.Format("{0},Origin Nullity Check: {1,6}", Indent(indent), "True")); } else if (source == null && exported != null) { bool axisEqual = exported.xyz.DoubleDeltaCompare(new double[] { 1, 0, 0 }, 0); linkLog.AppendLine(String.Format("{0}Axis", Indent(indent))); linkLog.AppendLine(String.Format("{0}Equal: {1,6}", Indent(indent), axisEqual)); linkLog.AppendLine(String.Format("{0}XYZ Source: NULL ", Indent(indent))); linkLog.AppendLine(String.Format("{0}XYZ Exported: ({1,5:F3},{2,5:F3},{3,5:F3}) ", Indent(indent), exported.xyz[0], exported.xyz[1], exported.xyz[2])); if (!axisEqual) { return(false); } } else if (exported == null && source != null) { bool axisEqual = source.xyz.DoubleDeltaCompare(new double[] { 1, 0, 0 }, 0); linkLog.AppendLine(String.Format("{0}Axis", Indent(indent))); linkLog.AppendLine(String.Format("{0}Equal: {1,6}", Indent(indent), axisEqual)); linkLog.AppendLine(String.Format("{0}XYZ Source: NULL ", Indent(indent))); linkLog.AppendLine(String.Format("{0}XYZ Exported: ({1,5:F3},{2,5:F3},{3,5:F3}) ", Indent(indent), exported.xyz[0], exported.xyz[1], exported.xyz[2])); if (!axisEqual) { return(false); } } else { for (int i = 0; i < 3; i++) { if (source.xyz[i] != exported.xyz[i]) { linkLog.AppendLine(String.Format("{0}Axis", Indent(indent))); linkLog.AppendLine(String.Format("{0}Equal: {1,6}", Indent(indent), "False")); linkLog.AppendLine(String.Format("{0}XYZ Source: ({1,5:F3},{2,5:F3},{3,5:F3}) ", Indent(indent), source.xyz[0], source.xyz[1], source.xyz[2])); linkLog.AppendLine(String.Format("{0}XYZ Exported: ({1,5:F3},{2,5:F3},{3,5:F3}) ", Indent(indent), exported.xyz[0], exported.xyz[1], exported.xyz[2])); return(false); } } linkLog.AppendLine(String.Format("{0}Axis", Indent(indent))); linkLog.AppendLine(String.Format("{0}Equal: {1,6}", Indent(indent), "True")); linkLog.AppendLine(String.Format("{0}XYZ : ({1,5:F3},{2,5:F3},{3,5:F3}) ", Indent(indent), source.xyz[0], source.xyz[1], source.xyz[2])); } return(true); }
protected static Vector3 GetAxis(Joint.Axis axis) { return(axis.xyz.ToVector3().Ros2Unity()); }