public PathPlanner()
{
this.Options = PathSmoother.GetDefaultOptions();
this.Options.alpha_s = 100;
this.Options.alpha_d = 100;
this.Options.alpha_w = 0;
}
public PlanningSettings()
{
Options = PathSmoother.GetDefaultOptions();
Options.alpha_s = 100;
Options.alpha_d = 100;
Options.alpha_w = 0;
Options.num_passes = 2;
Options.reverse = false;
}
private void SmoothIt()
{
Stopwatch s = Stopwatch.StartNew();
SmootherOptions opt = PathSmoother.GetDefaultOptions();
if (optIPOPT.Checked)
{
opt.alg = SmoothingAlgorithm.Ipopt;
}
else
{
opt.alg = SmoothingAlgorithm.Loqo;
}
opt.alpha_w = getW();
opt.alpha_d = getD1();
opt.set_init_velocity = true;
opt.init_velocity = getV();
opt.set_init_heading = true;
opt.init_heading = Math.Atan2(pathPoints[1].Y - pathPoints[0].Y, pathPoints[1].X - pathPoints[0].X);
opt.set_final_heading = true;
opt.final_heading = Math.Atan2(pathPoints[pathPoints.Count - 1].Y - pathPoints[pathPoints.Count - 2].Y, pathPoints[pathPoints.Count - 1].X - pathPoints[pathPoints.Count - 2].X);
opt.set_final_offset = true;
List <PathPoint> path = new List <PathPoint>();
Boundary ub = new Boundary();
ub.Coords = ubPoints;
ub.DesiredSpacing = 1;
ub.MinSpacing = 0.25;
ub.Polygon = false;
List <Boundary> ub_bounds = new List <Boundary>();
ub_bounds.Add(ub);
Boundary lb = new Boundary();
lb.Coords = lbPoints;
lb.DesiredSpacing = 0.5;
lb.MinSpacing = 0;
lb.Polygon = false;
List <Boundary> lb_bounds = new List <Boundary>();
lb_bounds.Add(lb);
LineList basePath = new LineList();
basePath.AddRange(pathPoints);
SmoothResult sr = SmoothResult.Error;
try {
sr = PathSmoother.SmoothPath(basePath, ub_bounds, lb_bounds, opt, path);
}
catch (Exception ex) {
MessageBox.Show("exception: " + ex.Message);
return;
}
smoothedPoints = path.ConvertAll <Coordinates>(delegate(PathPoint p) { return(new Coordinates(p.x, p.y)); });
s.Stop();
long ms = s.ElapsedMilliseconds;
if (sr != SmoothResult.Sucess)
{
MessageBox.Show("Path smooth result: " + sr.ToString());
}
MessageBox.Show("Elapsed MS: " + ms);
picEntry.Invalidate();
}