public void SetArcVotingResults(ArcVotingResults results)
{
this.results = results;
this.Invalidate();
}
protected override void OnPaint(PaintEventArgs e)
{
Graphics g = e.Graphics;
g.ResetClip();
g.Clear(Color.Black);
ArcVotingResults results = this.results;
int maxHeight = this.ClientSize.Height - 20;
int halfHeight = maxHeight / 2;
int centerHeight = this.ClientRectangle.Top + halfHeight + 10;
int totalWidth = this.ClientSize.Width - 20;
int startX = this.ClientRectangle.Left + 10;
if (results != null && selectedField != null)
{
results.arcResults.Reverse();
int barWidth = totalWidth / results.arcResults.Count;
double[] values = GetValues(selectedField);
for (int i = 0; i < values.Length; i++)
{
int height = (int)Math.Round(halfHeight * values[i]);
int left = startX + barWidth * i;
if (height == 0)
{
continue;
}
Rectangle rect;
if (height > 0)
{
rect = new Rectangle(left, centerHeight - height, barWidth, height);
}
else
{
rect = new Rectangle(left, centerHeight, barWidth, -height);
}
Color color = Color.Blue;
if (results.arcResults[i].vetoed)
{
color = Color.Red;
}
else if (results.selectedArc != null && results.selectedArc.curvature == results.arcResults[i].curvature)
{
color = Color.Green;
}
using (SolidBrush b = new SolidBrush(color)) {
g.FillRectangle(b, rect);
}
}
}
using (Pen p = new Pen(Color.White, 1)) {
g.DrawLine(p, startX, centerHeight, this.ClientSize.Width - 10, centerHeight);
}
}
private static double FindBestCurvature(double prevCurvature, Coordinates relativeGoalPoint, List <Polygon> perimeterPolygons)
{
CarTimestamp curTimestamp = Services.RelativePose.CurrentTimestamp;
AbsoluteTransformer absTransform = Services.StateProvider.GetAbsoluteTransformer();
// get a list of obstacles
ObstacleCollection obstacles = Services.ObstaclePipeline.GetProcessedObstacles(curTimestamp, UrbanChallenge.Behaviors.SAUDILevel.None);
List <Polygon> obstaclePolygons = new List <Polygon>();
foreach (Obstacle obs in obstacles.obstacles)
{
obstaclePolygons.Add(obs.cspacePolygon);
}
obstaclePolygons.AddRange(perimeterPolygons);
List <ArcResults> arcs = new List <ArcResults>();
double maxUtility = double.MinValue;
ArcResults selectedArc = null;
// recalculate weights
double totalWeights = obstacle_weight + hysteresis_weight + straight_weight + goal_weight;
double obstacleWeight = obstacle_weight / totalWeights;
double hysteresisWeight = hysteresis_weight / totalWeights;
double straightWeight = straight_weight / totalWeights;
double goalWeight = goal_weight / totalWeights;
int start = num_arcs / 2;
double curvatureStep = max_curvature / start;
for (int i = -start; i <= start; i++)
{
double curvature = i * curvatureStep;
double collisionDist, clearanceDist, collisionUtility;
bool vetoed;
EvaluateObstacleUtility(curvature, 20, obstaclePolygons, out collisionDist, out clearanceDist, out collisionUtility, out vetoed);
double hystersisUtility = EvaluateHysteresisUtility(curvature, prevCurvature);
double straightUtility = EvaluateStraightUtility(curvature);
double goalUtility = EvaluateGoalUtility(curvature, relativeGoalPoint);
double totalUtility = collisionUtility * obstacleWeight + hystersisUtility * hysteresisWeight + straightUtility * straightWeight +
goalUtility * goalWeight;
ArcResults result = new ArcResults();
result.curvature = curvature;
result.vetoed = vetoed;
result.totalUtility = totalUtility;
result.obstacleHitDistance = collisionDist;
result.obstacleClearanceDistance = clearanceDist;
result.obstacleUtility = collisionUtility;
result.hysteresisUtility = hystersisUtility;
result.straightUtility = straightUtility;
result.goalUtility = goalUtility;
arcs.Add(result);
if (!vetoed && totalUtility > maxUtility)
{
maxUtility = totalUtility;
selectedArc = result;
}
}
ArcVotingResults results = new ArcVotingResults();
results.arcResults = arcs;
results.selectedArc = selectedArc;
Services.Dataset.ItemAs <ArcVotingResults>("arc voting results").Add(results, LocalCarTimeProvider.LocalNow);
if (selectedArc == null)
{
return(double.NaN);
}
else
{
return(selectedArc.curvature);
}
}
