public RobotPath(RobotPath path)
{
m_segments = null;
m_splines = null;
Name = path.Name;
MaxVelocity = path.MaxVelocity;
MaxAcceleration = path.MaxAcceleration;
MaxJerk = path.MaxJerk;
StartVelocity = path.StartVelocity;
EndVelocity = path.EndVelocity;
StartFacingAngle = path.StartFacingAngle;
EndFacingAngle = path.EndFacingAngle;
FacingAngleStartDelay = path.FacingAngleStartDelay;
FacingAngleEndDelay = path.FacingAngleEndDelay;
Points = new WayPoint[path.Points.Length];
for (int i = 0; i < path.Points.Length; i++)
{
Points[i] = new WayPoint(path.Points[i]);
}
Constraints = new TimingContraint[path.Constraints.Length];
for (int i = 0; i < path.Constraints.Length; i++)
{
Constraints[i] = path.Constraints[i].Clone();
}
m_lock = new object();
}
private void GenerateSegmentsForPath(RobotParams robot, RobotPath path)
{
PathSegment[] segs = null;
Dictionary <string, PathSegment[]> add = null; DriveModifier mod = null;
double rotpercent = 0.0;
bool running = true;
if (robot.DriveType == RobotParams.TankDriveType)
{
mod = new TankDriveModifier();
}
else if (robot.DriveType == RobotParams.SwerveDriveType)
{
mod = new SwerveDriveModifier();
}
while (running)
{
RobotParams nrobot = new RobotParams(robot);
nrobot.MaxVelocity = (1 - rotpercent) * robot.MaxVelocity;
segs = GenerateDetailedPath(nrobot, path);
try
{
add = mod.ModifyPath(robot, path, segs, rotpercent * robot.MaxVelocity);
running = false;
}
catch (DriveModifier.VelocitySplitException)
{
rotpercent += 0.05;
}
}
path.SetSegments(segs, add);
}
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 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 void RemovePath(string name)
{
int index = -1;
for (int i = 0; i < Paths.Length; i++)
{
if (Paths[i].Name == name)
{
index = i;
break;
}
}
if (index != -1)
{
RobotPath[] temp = new RobotPath[Paths.Length - 1];
if (index > 0)
{
Array.Copy(Paths, 0, temp, 0, index);
}
if (index < Paths.Length - 1)
{
Array.Copy(Paths, index + 1, temp, index, Paths.Length - index - 1);
}
Paths = temp;
}
}
private void DrawSplines(Graphics g, RobotPath path)
{
for (int i = 0; i < path.Splines.Length; i++)
{
DrawSpline(g, path, i);
}
}
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 PathToCompare(PathGroup pg1, RobotPath p1, PathGroup pg2, RobotPath p2)
{
PathGroupOne = pg1;
PathOne = p1;
PathGroupTwo = pg2;
PathTwo = p2;
}
public BasicFieldView()
{
m_path = null;
m_world_to_window = new Matrix();
m_window_to_world = new Matrix();
DoubleBuffered = true;
InitializeComponent();
}
private void DrawPath(Graphics g, RobotPath path)
{
if (path.HasSplines)
{
DrawSplines(g, path);
}
DrawPoints(g, path.Points);
}
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 bool RenamePath(string oldname, string newname)
{
RobotPath pt = FindPathByName(oldname);
if (pt == null)
{
return(false);
}
pt.Name = newname;
return(true);
}
public void AddPath(string group, RobotPath path)
{
PathGroup gr = FindGroupByName(group);
if (gr == null)
{
return;
}
m_dirty = true;
gr.AddPath(path);
}
public bool Contains(RobotPath path)
{
foreach (PathGroup group in Groups)
{
if (group.Contains(path))
{
return(true);
}
}
return(false);
}
public void GenerateSegments(RobotParams robot, RobotPath path)
{
PathGenerationStateChangeEvent args = null;
Tuple <RobotParams, RobotPath> entry = new Tuple <RobotParams, RobotPath>(robot, path);
lock (m_lock)
{
m_queue.Add(entry);
int running = m_active ? 1 : 0;
int queued = m_queue.Count;
args = new PathGenerationStateChangeEvent(running, queued);
}
OnJobStateChanged(args);
}
public PathGroup FindGroupByPath(RobotPath p)
{
foreach (PathGroup gr in Groups)
{
foreach (RobotPath pa in gr.Paths)
{
if (pa == p)
{
return(gr);
}
}
}
return(null);
}
public void AddPath(RobotParams robot, string name)
{
RobotPath path = new RobotPath(name);
path.MaxVelocity = robot.MaxVelocity;
path.MaxAcceleration = robot.MaxAcceleration;
path.MaxJerk = robot.MaxJerk;
Array.Resize <RobotPath>(ref Paths, Paths.Length + 1);
Paths[Paths.Length - 1] = path;
double length = UnitConverter.Convert(100.0, "inches", robot.LengthUnits);
path.AddPoint(new WayPoint(0.0, 0.0, 0.0, 0.0));
path.AddPoint(new WayPoint(length, 0.0, 0.0, 0.0));
}
private void SetPath(RobotPath path)
{
if (m_path != null)
{
m_path.SegmentsUpdated -= PathSegmentsUpdated;
}
m_path = path;
if (m_path != null)
{
m_path.SegmentsUpdated += PathSegmentsUpdated;
PathSegmentsUpdated(null, null);
Time = 0.0;
}
else
{
m_chart.Series.Clear();
}
}
private void DrawSpline(Graphics g, RobotPath path, int index)
{
float diam = 2.0f;
using (Brush b = new SolidBrush(Color.LightCoral))
{
double step = 1.0 / 1000.0;
for (double t = 0.0; t < 1.0; t += step)
{
double x, y, heading;
RectangleF rf;
double px, py;
PointF pt, w;
path.Evaluate(index, t, out x, out y, out heading);
heading = XeroUtils.DegreesToRadians(heading);
double ca = Math.Cos(heading);
double sa = Math.Sin(heading);
px = x - m_file.Robot.Width * sa / 2.0;
py = y + m_file.Robot.Width * ca / 2.0;
pt = new PointF((float)px, (float)py);
w = WorldToWindow(pt);
rf = new RectangleF(w, new SizeF(0.0f, 0.0f));
rf.Offset(diam / 2.0f, diam / 2.0f);
rf.Inflate(diam, diam);
g.FillEllipse(b, rf);
px = x + m_file.Robot.Width * sa / 2.0;
py = y - m_file.Robot.Width * ca / 2.0;
pt = new PointF((float)px, (float)py);
w = WorldToWindow(pt);
rf = new RectangleF(w, new SizeF(0.0f, 0.0f));
rf.Offset(diam / 2.0f, diam / 2.0f);
rf.Inflate(diam, diam);
g.FillEllipse(b, rf);
}
}
}
protected bool GeneratePathFile(RobotParams robot, RobotPath path, string filename)
{
bool ret = true;
PathFile pf = new PathFile();
pf.Robot = robot;
pf.AddPathGroup("tmp");
pf.AddPath("tmp", path);
string json = JsonConvert.SerializeObject(pf);
try
{
File.WriteAllText(filename, json);
}
catch (Exception)
{
ret = false;
}
return(ret);
}
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 NoSegmentsException(RobotPath path)
{
Path = path;
}
public override Dictionary <string, PathSegment[]> ModifyPath(RobotParams robot, RobotPath path, PathSegment[] segs, double rotvel)
{
Dictionary <string, PathSegment[]> result = new Dictionary <string, PathSegment[]>();
if (segs == null)
{
return(null);
}
double lvel, lacc, ljerk, lpos = 0.0;
double rvel, racc, rjerk, rpos = 0.0;
double plx = 0.0, ply = 0.0;
double prx = 0.0, pry = 0.0;
double plvel = 0.0, prvel = 0.0;
double placc = 0.0, pracc = 0.0;
PathSegment[] lsegs = new PathSegment[segs.Length];
PathSegment[] rsegs = new PathSegment[segs.Length];
result["left"] = lsegs;
result["right"] = rsegs;
for (int i = 0; i < segs.Length; i++)
{
double time = segs[i].GetValue("time");
double heading = XeroUtils.DegreesToRadians(segs[i].GetValue("heading"));
double ca = Math.Cos(heading);
double sa = Math.Sin(heading);
double px = segs[i].GetValue("x");
double py = segs[i].GetValue("y");
double lx = px - robot.Width * sa / 2.0;
double ly = py + robot.Width * ca / 2.0;
double rx = px + robot.Width * sa / 2.0;
double ry = py - robot.Width * ca / 2.0;
if (i == 0)
{
lvel = 0.0;
lacc = 0.0;
lpos = 0.0;
ljerk = 0.0;
rvel = 0.0;
racc = 0.0;
rpos = 0.0;
rjerk = 0.0;
}
else
{
double dt = segs[i].GetValue("time") - segs[i - 1].GetValue("time");
double ldist = Math.Sqrt((lx - plx) * (lx - plx) + (ly - ply) * (ly - ply));
double rdist = Math.Sqrt((rx - prx) * (rx - prx) + (ry - pry) * (ry - pry));
lvel = ldist / dt;
rvel = rdist / dt;
lacc = (lvel - plvel) / dt;
racc = (rvel - prvel) / dt;
ljerk = (lacc - placc) / dt;
rjerk = (racc - pracc) / dt;
lpos += ldist;
rpos += rdist;
}
PathSegment left = new PathSegment();
left.SetValue("time", time);
left.SetValue("x", lx);
left.SetValue("y", ly);
left.SetValue("heading", segs[i].GetValue("heading"));
left.SetValue("position", lpos);
left.SetValue("velocity", lvel);
left.SetValue("acceleration", lacc);
left.SetValue("jerk", ljerk);
lsegs[i] = left;
PathSegment right = new PathSegment();
right.SetValue("time", segs[i].GetValue("time"));
right.SetValue("x", rx);
right.SetValue("y", ry);
right.SetValue("heading", segs[i].GetValue("heading"));
right.SetValue("position", rpos);
right.SetValue("velocity", rvel);
right.SetValue("acceleration", racc);
right.SetValue("jerk", rjerk);
rsegs[i] = right;
plx = lx;
ply = ly;
prx = rx;
pry = ry;
plvel = lvel;
prvel = rvel;
placc = lacc;
pracc = racc;
}
return(result);
}
public abstract Dictionary <string, PathSegment[]> ModifyPath(RobotParams robot, RobotPath path, PathSegment[] segs, double rotvel);
public override Dictionary <string, PathSegment[]> ModifyPath(RobotParams robot, RobotPath path, PathSegment[] segs, double rotvel)
{
Dictionary <string, PathSegment[]> result = new Dictionary <string, PathSegment[]>();
if (segs == null)
{
return(null);
}
double total = segs[segs.Length - 1].GetValue("time");
double rtime = total - path.FacingAngleStartDelay - path.FacingAngleEndDelay;
TrapezoidalProfile profile = CreateRotationProfile(robot, path.StartFacingAngle, path.EndFacingAngle, rtime, rotvel);
if (profile == null)
{
throw new DriveModifier.VelocitySplitException();
}
PathSegment[] fl = new PathSegment[segs.Length];
PathSegment[] fr = new PathSegment[segs.Length];
PathSegment[] bl = new PathSegment[segs.Length];
PathSegment[] br = new PathSegment[segs.Length];
result["fl"] = fl;
result["fr"] = fr;
result["bl"] = bl;
result["br"] = br;
for (int i = 0; i < segs.Length; i++)
{
//
// The time in the path
//
double time = segs[i].GetValue("time");
//
// The current facing angle based on the rotational trapezoidal profile
//
double current = XeroUtils.BoundDegrees(path.StartFacingAngle + profile.GetDistance(time - path.FacingAngleStartDelay));
//
// Get the required velocity to perform the rotation. This is the
// linear ground based velocity for the wheel that must be applied to
// each wheel in direction perpendicular to the vector from the center of
// the robot to the center of the wheel.
//
double rv = RotationalToGround(robot, profile.GetVelocity(time - path.FacingAngleStartDelay));
double ra = RotationalToGround(robot, profile.GetAcceleration(time - path.FacingAngleStartDelay));
//
// Get the velocity vector and acceleration vector relative to each of the
// wheels to rotate the robot based on the desired rotational velocity.
//
XeroVector rotflvel = GetWheelPerpendicularVector(robot, Wheel.FL, rv);
XeroVector rotfrvel = GetWheelPerpendicularVector(robot, Wheel.FR, rv);
XeroVector rotblvel = GetWheelPerpendicularVector(robot, Wheel.BL, rv);
XeroVector rotbrvel = GetWheelPerpendicularVector(robot, Wheel.BR, rv);
XeroVector rotflacc = GetWheelPerpendicularVector(robot, Wheel.FL, ra);
XeroVector rotfracc = GetWheelPerpendicularVector(robot, Wheel.FR, ra);
XeroVector rotblacc = GetWheelPerpendicularVector(robot, Wheel.BL, ra);
XeroVector rotbracc = GetWheelPerpendicularVector(robot, Wheel.BR, ra);
//
// Get the translational velocity vector to follow the path
//
XeroVector pathvel = XeroVector.FromDegreesMagnitude(segs[i].GetValue("heading"), segs[i].GetValue("velocity")).RotateDegrees(-current);
XeroVector pathacc = XeroVector.FromDegreesMagnitude(segs[i].GetValue("heading"), segs[i].GetValue("acceleration")).RotateDegrees(-current);
//
// For each wheel, the velocity vector is the sum of the rotational vector and the translational vector
//
XeroVector flv = rotflvel + pathvel;
XeroVector frv = rotfrvel + pathvel;
XeroVector blv = rotblvel + pathvel;
XeroVector brv = rotbrvel + pathvel;
XeroVector fla = rotflacc + pathvel;
XeroVector fra = rotfracc + pathvel;
XeroVector bla = rotblacc + pathvel;
XeroVector bra = rotbracc + pathvel;
//
// Now calculate the wheel positions based on the path position and the
// facing angle
//
XeroVector flpos = new XeroVector(robot.Length / 2.0, robot.Width / 2.0).RotateDegrees(current).Translate(segs[i].GetValue("x"), segs[i].GetValue("y"));
XeroVector frpos = new XeroVector(robot.Length / 2.0, -robot.Width / 2.0).RotateDegrees(current).Translate(segs[i].GetValue("x"), segs[i].GetValue("y"));
XeroVector blpos = new XeroVector(-robot.Length / 2.0, robot.Width / 2.0).RotateDegrees(current).Translate(segs[i].GetValue("x"), segs[i].GetValue("y"));
XeroVector brpos = new XeroVector(-robot.Length / 2.0, -robot.Width / 2.0).RotateDegrees(current).Translate(segs[i].GetValue("x"), segs[i].GetValue("y"));
PathSegment newseg = new PathSegment(segs[i]);
newseg.SetValue("x", flpos.X);
newseg.SetValue("y", flpos.Y);
newseg.SetValue("velocity", flv.Magnitude());
newseg.SetValue("heading", flv.AngleDegrees());
newseg.SetValue("acceleration", fla.Magnitude());
fl[i] = newseg;
newseg = new PathSegment(segs[i]);
newseg.SetValue("x", frpos.X);
newseg.SetValue("y", frpos.Y);
newseg.SetValue("velocity", frv.Magnitude());
newseg.SetValue("heading", frv.AngleDegrees());
newseg.SetValue("acceleration", fra.Magnitude());
fr[i] = newseg;
newseg = new PathSegment(segs[i]);
newseg.SetValue("x", blpos.X);
newseg.SetValue("y", blpos.Y);
newseg.SetValue("velocity", blv.Magnitude());
newseg.SetValue("heading", blv.AngleDegrees());
newseg.SetValue("acceleration", bla.Magnitude());
bl[i] = newseg;
newseg = new PathSegment(segs[i]);
newseg.SetValue("x", brpos.X);
newseg.SetValue("y", brpos.Y);
newseg.SetValue("velocity", brv.Magnitude());
newseg.SetValue("heading", brv.AngleDegrees());
newseg.SetValue("acceleration", bra.Magnitude());
br[i] = newseg;
}
return(result);
}
public void SetSelectedPath(PathGroup group, RobotPath path)
{
m_selected_path = new Tuple <string, string>(group.Name, path.Name);
}
public abstract PathSegment[] GenerateDetailedPath(RobotParams robot, RobotPath path);
public abstract void GenerateSplines(RobotPath path, List <Spline> xsplines, List <Spline> ysplines);
private void DrawPathRobot(Graphics g, RobotPath path)
{
if (path.AdditionalSegments == null)
{
return;
}
PointF[] pts = null;
Dictionary <string, PathSegment[]> segs = path.AdditionalSegments;
if (segs.Count == 2)
{
XeroPose left, right;
left = path.GetPositionForTime(segs["left"], m_time);
right = path.GetPositionForTime(segs["right"], m_time);
XeroPose center = left.Interpolate(right, 0.5);
pts = new PointF[]
{
new PointF((float)Robot.Length / 2.0f, -(float)Robot.Width / 2.0f),
new PointF(-(float)Robot.Length / 2.0f, -(float)Robot.Width / 2.0f),
new PointF(-(float)Robot.Length / 2.0f, (float)Robot.Width / 2.0f),
new PointF((float)Robot.Length / 2.0f, (float)Robot.Width / 2.0f),
};
Matrix mm = new Matrix();
mm.Translate((float)center.X, (float)center.Y);
mm.Rotate((float)center.HeadingDegrees);
mm.TransformPoints(pts);
WorldToWindowMatrix.TransformPoints(pts);
}
else
{
XeroPose fl, fr, bl, br;
fl = path.GetPositionForTime(segs["fl"], m_time);
fr = path.GetPositionForTime(segs["fr"], m_time);
bl = path.GetPositionForTime(segs["bl"], m_time);
br = path.GetPositionForTime(segs["br"], m_time);
pts = new PointF[4]
{
new PointF((float)fr.X, (float)fr.Y),
new PointF((float)br.X, (float)br.Y),
new PointF((float)bl.X, (float)bl.Y),
new PointF((float)fl.X, (float)fl.Y),
};
WorldToWindowMatrix.TransformPoints(pts);
}
using (Pen p = new Pen(Color.Yellow, 3.0f))
{
g.DrawLines(p, pts);
}
using (Pen p = new Pen(Color.Green, 3.0f))
{
g.DrawLine(p, pts[3], pts[0]);
}
}
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());
}
}
}
}
