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