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();
}
public SmoothingResult(SmoothResult result, SmoothedPath path, AvoidanceDetails details)
{
this.result = result;
this.path = path;
this.details = details;
}
public SmoothingResult(SmoothResult result, SmoothedPath path, AvoidanceDetails details)
{
this.result = result;
this.path = path;
this.details = details;
}
public SmoothingResult PlanPath(PlanningSettings settings)
{
SmootherOptions opts = settings.Options;
// for now, just run the smoothing
opts.init_heading = settings.initHeading;
opts.set_init_heading = true;
opts.min_init_velocity = settings.startSpeed * 0.5;
opts.set_min_init_velocity = true;
opts.max_init_velocity = Math.Max(settings.maxSpeed, settings.startSpeed);
opts.set_max_init_velocity = true;
opts.min_velocity = 0.1;
opts.max_velocity = Math.Max(opts.min_velocity + 0.1, settings.maxSpeed);
opts.k_max = Math.Min(TahoeParams.CalculateCurvature(-TahoeParams.SW_max, settings.startSpeed), TahoeParams.CalculateCurvature(TahoeParams.SW_max, settings.startSpeed)) * 0.97;
opts.generate_details = true; // GenerateDetails;
if (settings.endingHeading != null)
{
opts.set_final_heading = true;
opts.final_heading = settings.endingHeading.Value;
}
else
{
opts.set_final_heading = false;
}
opts.set_final_offset = settings.endingPositionFixed;
opts.final_offset_min = settings.endingPositionMin;
opts.final_offset_max = settings.endingPositionMax;
if (settings.maxEndingSpeed != null)
{
opts.set_final_velocity_max = true;
opts.final_velocity_max = Math.Max(opts.min_velocity + 0.1, settings.maxEndingSpeed.Value);
}
else
{
opts.set_final_velocity_max = false;
}
opts.a_lat_max = 6;
// create the boundary list
List <UrbanChallenge.PathSmoothing.PathPoint> ret = new List <UrbanChallenge.PathSmoothing.PathPoint>();
smoother = new PathSmoother();
OperationalTrace.WriteVerbose("calling smooth path");
SmoothResult result = smoother.SmoothPath(settings.basePath, settings.targetPaths, settings.leftBounds, settings.rightBounds, opts, ret);
if (result != SmoothResult.Sucess)
{
OperationalTrace.WriteWarning("smooth path result: {0}", result);
}
else
{
OperationalTrace.WriteVerbose("smooth path result: {0}", result);
}
AvoidanceDetails details = null;
if (opts.generate_details)
{
details = new AvoidanceDetails();
details.leftBounds = settings.leftBounds;
details.rightBounds = settings.rightBounds;
details.smoothingDetails = smoother.GetSmoothingDetails();
LastAvoidanceDetails = details;
// push out the points
Coordinates[] leftPoints = new Coordinates[details.smoothingDetails.leftBounds.Length];
for (int i = 0; i < leftPoints.Length; i++)
{
leftPoints[i] = details.smoothingDetails.leftBounds[i].point;
}
Coordinates[] rightPoints = new Coordinates[details.smoothingDetails.rightBounds.Length];
for (int i = 0; i < rightPoints.Length; i++)
{
rightPoints[i] = details.smoothingDetails.rightBounds[i].point;
}
Services.UIService.PushPoints(leftPoints, settings.timestamp, "left bound points", true);
Services.UIService.PushPoints(rightPoints, settings.timestamp, "right bound points", true);
}
//if (result == SmoothResult.Sucess) {
Coordinates[] points = new Coordinates[ret.Count];
double[] speeds = new double[ret.Count];
for (int i = 0; i < ret.Count; i++)
{
points[i] = new Coordinates(ret[i].x, ret[i].y);
speeds[i] = ret[i].v;
}
SmoothedPath path = new SmoothedPath(settings.timestamp, points, speeds);
return(new SmoothingResult(result, path, details));
/*}
* else {
* SmoothedPath path = new SmoothedPath(settings.timestamp, settings.basePath, null);
*
* return new SmoothingResult(result, path);
* }*/
}
