public static void Diff(PathGroup p1, PathGroup p2, List <string> diff)
{
List <PathToCompare> todiff = new List <PathToCompare>();
foreach (RobotPath path in p1.Paths)
{
RobotPath second = Array.Find(p2.Paths, search => search.Name == path.Name);
if (second == null)
{
diff.Add("p2 missing path in group '" + p1.Name + "', path '" + path.Name + "'");
}
else
{
PathToCompare pcmp = new PathToCompare(p1, path, p2, second);
todiff.Add(pcmp);
}
}
foreach (RobotPath path in p2.Paths)
{
RobotPath first = Array.Find(p1.Paths, search => search.Name == path.Name);
if (first == null)
{
diff.Add("p1 missing path in group '" + p2.Name + "', path '" + path.Name + "'");
}
}
foreach (PathToCompare p in todiff)
{
Diff(p.PathGroupOne, p.PathOne, p.PathGroupTwo, p.PathTwo, diff);
}
}
public void RemoveGroup(string group)
{
int index = -1;
for (int i = 0; i < Groups.Length; i++)
{
if (Groups[i].Name == group)
{
index = i;
break;
}
}
if (index != -1)
{
m_dirty = true;
PathGroup[] temp = new PathGroup[Groups.Length - 1];
if (index > 0)
{
Array.Copy(Groups, 0, temp, 0, index);
}
if (index < Groups.Length - 1)
{
Array.Copy(Groups, index + 1, temp, index, Groups.Length - index - 1);
}
Groups = temp;
}
}
public static void Diff(PathGroup[] p1, PathGroup [] p2, List <string> diff)
{
List <Tuple <PathGroup, PathGroup> > todiff = new List <Tuple <PathGroup, PathGroup> >();
foreach (PathGroup gr in p1)
{
PathGroup second = Array.Find(p2, group => group.Name == gr.Name);
if (second == null)
{
diff.Add("p2 missing path group '" + gr.Name + "'");
}
else
{
var todo = new Tuple <PathGroup, PathGroup>(gr, second);
todiff.Add(todo);
}
}
foreach (PathGroup gr in p2)
{
PathGroup first = Array.Find(p1, group => group.Name == gr.Name);
if (first == null)
{
diff.Add("p1 missing path group '" + gr.Name + "'");
}
}
foreach (Tuple <PathGroup, PathGroup> todo in todiff)
{
Diff(todo.Item1, todo.Item2, diff);
}
}
public bool FindPathByWaypoint(WayPoint pt, out PathGroup group, out RobotPath path, out int index)
{
group = null;
path = null;
index = -1;
foreach (PathGroup gr in Groups)
{
foreach (RobotPath pa in gr.Paths)
{
for (int i = 0; i < pa.Points.Length; i++)
{
if (pa.Points[i] == pt)
{
group = gr;
path = pa;
index = i;
return(true);
}
}
}
}
return(false);
}
public PathToCompare(PathGroup pg1, RobotPath p1, PathGroup pg2, RobotPath p2)
{
PathGroupOne = pg1;
PathOne = p1;
PathGroupTwo = pg2;
PathTwo = p2;
}
protected override void OnMouseUp(MouseEventArgs e)
{
PathGroup gr = null;
RobotPath path = null;
base.OnMouseUp(e);
Capture = false;
if (m_dragging)
{
m_file.FindPathByWaypoint(m_selected, out gr, out path);
m_dragging = false;
if (!m_last_msg_selected)
{
m_file.FindPathByWaypoint(m_selected, out gr, out path);
OnWayPointChanged(new WaypointEventArgs(WaypointEventArgs.ReasonType.EndChange, gr, path, m_selected));
}
Invalidate();
}
else if (m_rotating != null)
{
m_selected = m_rotating;
m_rotating = null;
if (!m_last_msg_selected)
{
m_file.FindPathByWaypoint(m_selected, out gr, out path);
OnWayPointChanged(new WaypointEventArgs(WaypointEventArgs.ReasonType.EndChange, gr, path, m_selected));
}
Invalidate();
}
}
public void AddPathGroup(string name)
{
PathGroup group = new PathGroup(name);
Array.Resize <PathGroup>(ref Groups, Groups.Length + 1);
Groups[Groups.Length - 1] = group;
m_dirty = true;
}
public PathGroup(PathGroup pg)
{
Name = pg.Name;
Paths = new RobotPath[pg.Paths.Length];
for (int i = 0; i < Paths.Length; i++)
{
Paths[i] = new RobotPath(pg.Paths[i]);
}
}
public RobotPath FindPathByName(string group, string path)
{
PathGroup gr = FindGroupByName(group);
if (gr == null)
{
return(null);
}
return(gr.FindPathByName(path));
}
public void AddPath(string group, RobotPath path)
{
PathGroup gr = FindGroupByName(group);
if (gr == null)
{
return;
}
m_dirty = true;
gr.AddPath(path);
}
public void RemovePath(string grname, string pathname)
{
PathGroup gr = FindGroupByName(grname);
if (gr == null)
{
return;
}
gr.RemovePath(pathname);
m_dirty = true;
}
public PathFile(PathFile pf)
{
Robot = new RobotParams(pf.Robot);
Groups = new PathGroup[pf.Groups.Length];
for (int i = 0; i < Groups.Length; i++)
{
Groups[i] = new PathGroup(pf.Groups[i]);
}
IsDirty = true;
m_filename = pf.m_filename;
}
public bool RenameGroup(string oldname, string newname)
{
PathGroup gr = FindGroupByName(oldname);
if (gr == null)
{
return(false);
}
gr.Name = newname;
m_dirty = true;
return(true);
}
public bool RenamePath(string group, string oldname, string newname)
{
PathGroup gr = FindGroupByName(group);
if (gr == null)
{
return(false);
}
if (!gr.RenamePath(oldname, newname))
{
return(false);
}
m_dirty = true;
return(true);
}
public bool FindPathByWaypoint(WayPoint pt, out PathGroup group, out RobotPath path)
{
group = null;
path = null;
foreach (PathGroup gr in Groups)
{
foreach (RobotPath pa in gr.Paths)
{
foreach (WayPoint wy in pa.Points)
{
if (wy == pt)
{
group = gr;
path = pa;
return(true);
}
}
}
}
return(false);
}
public void SetSelectedPath(PathGroup group, RobotPath path)
{
m_selected_path = new Tuple <string, string>(group.Name, path.Name);
}
public bool Contains(PathGroup group)
{
return(Array.IndexOf(Groups, group) != -1);
}
public static void Diff(PathGroup pgr1, RobotPath p1, PathGroup pgr2, RobotPath p2, List <string> diff)
{
if (Math.Abs(p1.MaxVelocity - p2.MaxVelocity) > kEpsilon)
{
diff.Add("MaxVelocity: p1 = " + "PathGroup '" + pgr1.Name + "', Path '" + p1.Name + "'" + p1.MaxVelocity.ToString() + ", p2 = " + p2.MaxVelocity.ToString());
}
if (Math.Abs(p1.MaxAcceleration - p2.MaxAcceleration) > kEpsilon)
{
diff.Add("MaxAcceleration: p1 = " + "PathGroup '" + pgr1.Name + "', Path '" + p1.Name + "'" + p1.MaxAcceleration.ToString() + ", p2 = " + p2.MaxAcceleration.ToString());
}
if (Math.Abs(p1.MaxJerk - p2.MaxJerk) > kEpsilon)
{
diff.Add("MaxJerk: p1 = " + "PathGroup '" + pgr1.Name + "', Path '" + p1.Name + "'" + p1.MaxJerk.ToString() + ", p2 = " + p2.MaxJerk.ToString());
}
if (Math.Abs(p1.StartVelocity - p2.StartVelocity) > kEpsilon)
{
diff.Add("StartVelocity: p1 = " + "PathGroup '" + pgr1.Name + "', Path '" + p1.Name + "'" + p1.StartVelocity.ToString() + ", p2 = " + p2.StartVelocity.ToString());
}
if (Math.Abs(p1.EndVelocity - p2.EndVelocity) > kEpsilon)
{
diff.Add("EndVelocity: p1 = " + "PathGroup '" + pgr1.Name + "', Path '" + p1.Name + "'" + p1.EndVelocity.ToString() + ", p2 = " + p2.EndVelocity.ToString());
}
if (Math.Abs(p1.StartFacingAngle - p2.StartFacingAngle) > kEpsilon)
{
diff.Add("StartFacingAngle: p1 = " + "PathGroup '" + pgr1.Name + "', Path '" + p1.Name + "'" + p1.StartFacingAngle.ToString() + ", p2 = " + p2.StartFacingAngle.ToString());
}
if (Math.Abs(p1.EndFacingAngle - p2.EndFacingAngle) > kEpsilon)
{
diff.Add("EndFacingAngle: p1 = " + "PathGroup '" + pgr1.Name + "', Path '" + p1.Name + "'" + p1.EndFacingAngle.ToString() + ", p2 = " + p2.EndFacingAngle.ToString());
}
if (Math.Abs(p1.FacingAngleStartDelay - p2.FacingAngleStartDelay) > kEpsilon)
{
diff.Add("FacingAngleStartDelay: p1 = " + "PathGroup '" + pgr1.Name + "', Path '" + p1.Name + "'" + p1.FacingAngleStartDelay.ToString() + ", p2 = " + p2.FacingAngleStartDelay.ToString());
}
if (Math.Abs(p1.FacingAngleEndDelay - p2.FacingAngleEndDelay) > kEpsilon)
{
diff.Add("FacingAngleEndDelay: p1 = " + "PathGroup '" + pgr1.Name + "', Path '" + p1.Name + "'" + p1.FacingAngleEndDelay.ToString() + ", p2 = " + p2.FacingAngleEndDelay.ToString());
}
// Constraints
// Waypoints
if (p1.Points.Length != p2.Points.Length)
{
diff.Add("Waypoints: p1 has " + p1.Points.Length.ToString() + ", p2 has " + p2.Points.Length.ToString());
}
else
{
for (int i = 0; i < p1.Points.Length; i++)
{
if (Math.Abs(p1.Points[i].X - p2.Points[i].X) > kEpsilon)
{
diff.Add("X: WayPoint " + i.ToString() + "p1 = " + "PathGroup '" + pgr1.Name + "', Path '" + p1.Name + "'" + p1.Points[i].X.ToString() + ", p2 = " + p2.Points[i].X.ToString());
}
if (Math.Abs(p1.Points[i].Y - p2.Points[i].Y) > kEpsilon)
{
diff.Add("Y: WayPoint " + i.ToString() + "p1 = " + "PathGroup '" + pgr1.Name + "', Path '" + p1.Name + "'" + p1.Points[i].Y.ToString() + ", p2 = " + p2.Points[i].Y.ToString());
}
if (Math.Abs(p1.Points[i].Heading - p2.Points[i].Heading) > kEpsilon)
{
diff.Add("Heading: WayPoint " + i.ToString() + "p1 = " + "PathGroup '" + pgr1.Name + "', Path '" + p1.Name + "'" + p1.Points[i].Heading.ToString() + ", p2 = " + p2.Points[i].Heading.ToString());
}
if (Math.Abs(p1.Points[i].Velocity - p2.Points[i].Velocity) > kEpsilon)
{
diff.Add("Velocity: WayPoint " + i.ToString() + "p1 = " + "PathGroup '" + pgr1.Name + "', Path '" + p1.Name + "'" + p1.Points[i].Velocity.ToString() + ", p2 = " + p2.Points[i].Velocity.ToString());
}
if (Math.Abs(p1.Points[i].Velocity - p2.Points[i].Velocity) > kEpsilon)
{
diff.Add("Velocity: WayPoint " + i.ToString() + "p1 = " + "PathGroup '" + pgr1.Name + "', Path '" + p1.Name + "'" + p1.Points[i].Velocity.ToString() + ", p2 = " + p2.Points[i].Velocity.ToString());
}
}
}
}
