public void Render(IGraphics g, WorldTransform wt)
{
if (avoidanceDetails == null)
{
return;
}
if (drawBoundPoints)
{
PointF vehicleLoc = Utility.ToPointF(Services.VehicleStateService.Location);
g.GoToVehicleCoordinates(vehicleLoc, (float)Services.VehicleStateService.Heading + (float)Math.PI / 2.0f);
for (int i = 0; i < avoidanceDetails.smoothingDetails.leftBounds.Length; i++)
{
BoundInformation b = avoidanceDetails.smoothingDetails.leftBounds[i];
DrawingUtility.DrawControlPoint(g, b.point, Color.Blue, null, ContentAlignment.MiddleRight, ControlPointStyle.LargeX, wt);
}
for (int i = 0; i < avoidanceDetails.smoothingDetails.rightBounds.Length; i++)
{
BoundInformation b = avoidanceDetails.smoothingDetails.rightBounds[i];
DrawingUtility.DrawControlPoint(g, b.point, Color.Red, null, ContentAlignment.MiddleRight, ControlPointStyle.LargeX, wt);
}
g.ComeBackFromVehicleCoordinates();
}
}
public void Render(IGraphics g, WorldTransform wt)
{
PointF[][] clusters = clusterPoints;
if (clusters == null)
{
return;
}
IPen pen = g.CreatePen();
pen.Width = nominal_pixel_size / wt.Scale;
pen.Color = color;
// get the current vehicle position/heading
float vehicleHeading = (float)Services.VehicleStateService.Heading;
PointF vehiclePos = Utility.ToPointF(Services.VehicleStateService.Location);
g.GoToVehicleCoordinates(vehiclePos, vehicleHeading + (float)Math.PI / 2.0f);
for (int i = 0; i < clusters.Length; i++)
{
PointF[] pts = clusters[i];
for (int j = 0; j < pts.Length; j++)
{
g.DrawRectangle(pen, new RectangleF(pts[j].X - box_size, pts[j].Y - box_size, 2 * box_size, 2 * box_size));
}
}
g.ComeBackFromVehicleCoordinates();
}
public void Render(IGraphics g, WorldTransform wt)
{
// if we're attached to the vehicle state, update our shits
if (attachToVehicleState)
{
location = Utility.ToPointF(Services.VehicleStateService.Location);
heading = (float)Services.VehicleStateService.Heading;
}
g.GoToVehicleCoordinates(location, heading);
float penWidth = nomPixelWidth / wt.Scale;
//if (penWidth > 0.01f) penWidth = 0.01f;
IPen p = g.CreatePen();
p.Width = penWidth;
p.Color = color;
g.DrawRectangle(p, bodyRect);
// build the transform for the rear wheels
// do the left wheel
g.PushMatrix();
g.Translate(-wheelOffset, 0);
g.FillRectangle(Color.White, wheelRectL);
g.DrawRectangle(p, wheelRectL);
g.PopMatrix();
// do the right wheel
g.PushMatrix();
g.Translate(wheelOffset, 0);
g.FillRectangle(Color.White, wheelRectR);
g.DrawRectangle(p, wheelRectR);
g.PopMatrix();
// do the front wheels
// do the left wheel
g.PushMatrix();
g.Translate(-wheelOffset, wheelbase);
g.FillRectangle(Color.White, wheelRectL);
g.DrawRectangle(p, wheelRectL);
g.PopMatrix();
// do the right wheel
g.PushMatrix();
g.Translate(wheelOffset, wheelbase);
g.FillRectangle(Color.White, wheelRectR);
g.DrawRectangle(p, wheelRectR);
g.PopMatrix();
// draw the forward arrow
g.DrawLine(p, new PointF(bodyRect.Left + 0.5f, bodyRect.Bottom - bodyRect.Width / 2), new PointF(bodyRect.Left + bodyRect.Width / 2, bodyRect.Bottom - .5f));
g.DrawLine(p, new PointF(bodyRect.Right - 0.5f, bodyRect.Bottom - bodyRect.Width / 2), new PointF(bodyRect.Right - bodyRect.Width / 2, bodyRect.Bottom - .5f));
g.DrawCross(p, new PointF(0, 0), 8 / wt.Scale);
g.ComeBackFromVehicleCoordinates();
p.Dispose();
}
public void Render(IGraphics g, WorldTransform wt)
{
ArcVotingResults results = this.results;
if (results == null)
{
return;
}
// get the current vehicle position/heading
float vehicleHeading = (float)Services.VehicleStateService.Heading;
PointF vehiclePos = Utility.ToPointF(Services.VehicleStateService.Location);
g.GoToVehicleCoordinates(vehiclePos, vehicleHeading + (float)Math.PI / 2.0f);
IPen p = g.CreatePen();
p.Color = color;
p.Width = nomPixelWidth / wt.Scale;
foreach (ArcResults result in results.arcResults)
{
// generate the arc
if (Math.Abs(result.curvature) < 1e-4)
{
g.DrawLine(p, new PointF(0, 0), new PointF(dist, 0));
}
else
{
double curvature = result.curvature;
bool leftTurn = curvature > 0;
double radius = Math.Abs(1 / curvature);
double frontRadius = Math.Sqrt(TahoeParams.FL * TahoeParams.FL + radius * radius);
CircleSegment rearSegment;
if (leftTurn)
{
Coordinates center = new Coordinates(0, radius);
rearSegment = new CircleSegment(radius, center, Coordinates.Zero, dist, true);
}
else
{
Coordinates center = new Coordinates(0, -radius);
rearSegment = new CircleSegment(radius, center, Coordinates.Zero, dist, false);
}
PointF[] points = Utility.ToPointF(rearSegment.ToPoints(10));
g.DrawLines(p, points);
}
}
g.ComeBackFromVehicleCoordinates();
p.Dispose();
}
public void Render(IGraphics g, WorldTransform wt)
{
if (grid == null)
{
return;
}
// go through and draw each rectangle
float min = grid.MinValue;
float max = grid.MaxValue;
float a0 = minColor.A;
float a1 = maxColor.A - minColor.A;
float r0 = minColor.R;
float r1 = maxColor.R - minColor.R;
float g0 = minColor.G;
float g1 = maxColor.G - minColor.G;
float b0 = minColor.B;
float b1 = maxColor.B - minColor.B;
float offsetX = grid.OffsetX;
float offsetY = grid.OffsetY;
float spacing = grid.Spacing;
SizeF boxSize = new SizeF(spacing, spacing);
// go to the vehicle position
PointF vehicleLoc = Utility.ToPointF(Services.VehicleStateService.Location);
g.GoToVehicleCoordinates(vehicleLoc, (float)Services.VehicleStateService.Heading + (float)Math.PI / 2.0f);
if (max == min)
{
max = min + 1;
}
for (int x = 0; x < grid.SizeX; x++)
{
float xStart = offsetX + x * spacing;
for (int y = 0; y < grid.SizeY; y++)
{
float val = grid[x, y];
float frac = (val - min) / (max - min);
Color c = Color.FromArgb((int)(a0 + a1 * frac), (int)(r0 + r1 * frac), (int)(g0 + g1 * frac), (int)(b0 + b1 * frac));
float yStart = offsetY + y * spacing;
g.FillRectangle(c, new RectangleF(new PointF(xStart, yStart), boxSize));
}
}
g.ComeBackFromVehicleCoordinates();
}
public void Render(IGraphics g, WorldTransform wt)
{
if (avoidanceDetails == null)
return;
if (drawBoundPoints) {
PointF vehicleLoc = Utility.ToPointF(Services.VehicleStateService.Location);
g.GoToVehicleCoordinates(vehicleLoc, (float)Services.VehicleStateService.Heading + (float)Math.PI/2.0f);
for (int i = 0; i < avoidanceDetails.smoothingDetails.leftBounds.Length; i++) {
BoundInformation b = avoidanceDetails.smoothingDetails.leftBounds[i];
DrawingUtility.DrawControlPoint(g, b.point, Color.Blue, null, ContentAlignment.MiddleRight, ControlPointStyle.LargeX, wt);
}
for (int i = 0; i < avoidanceDetails.smoothingDetails.rightBounds.Length; i++) {
BoundInformation b = avoidanceDetails.smoothingDetails.rightBounds[i];
DrawingUtility.DrawControlPoint(g, b.point, Color.Red, null, ContentAlignment.MiddleRight, ControlPointStyle.LargeX, wt);
}
g.ComeBackFromVehicleCoordinates();
}
}
public void Render(IGraphics g, WorldTransform wt)
{
List <InternalTrackedCluster> trackedClusters = this.trackedClusters;
float vehicleHeading = (float)Services.VehicleStateService.Heading;
PointF vehiclePos = Utility.ToPointF(Services.VehicleStateService.Location);
g.GoToVehicleCoordinates(vehiclePos, vehicleHeading + (float)Math.PI / 2.0f);
SizeF boxSize = new SizeF(2 * point_box_size, 2 * point_box_size);
foreach (InternalTrackedCluster cluster in trackedClusters)
{
// determine cluster color
Color clusterColor;
switch (cluster.targetClass)
{
case SceneEstimatorTargetClass.TARGET_CLASS_CARLIKE:
clusterColor = carlikeColor;
break;
case SceneEstimatorTargetClass.TARGET_CLASS_NOTCARLIKE:
clusterColor = notcarColor;
break;
case SceneEstimatorTargetClass.TARGET_CLASS_UNKNOWN:
default:
clusterColor = unknownColor;
break;
}
if (cluster.targetStatus == SceneEstimatorTargetStatusFlag.TARGET_STATE_DELETED)
{
clusterColor = Color.FromArgb(deleted_alpha, clusterColor);
}
if (drawPoints)
{
// render the points
IPen pen = g.CreatePen();
pen.Color = clusterColor;
pen.Width = nominal_pixel_size / wt.Scale;
for (int j = 0; j < cluster.points.Length; j++)
{
g.DrawRectangle(pen, new RectangleF(cluster.points[j], boxSize));
}
}
if (drawPolygon)
{
IPen pen = g.CreatePen();
pen.Color = clusterColor;
pen.Width = nominal_pixel_size / wt.Scale;
g.DrawPolygon(pen, Utility.ToPointF(cluster.polygon));
}
// if we're stopped, draw an x at the center of the polygon
if (cluster.isStopped)
{
DrawingUtility.DrawControlPoint(g, cluster.polygon.GetCentroid(), Color.Red, null, ContentAlignment.BottomCenter, ControlPointStyle.LargeX, wt);
}
else
{
// draw a vector pointing from the centroid of the polygon out
}
}
g.ComeBackFromVehicleCoordinates();
}
