public static void DrawArrow(IGraphics g, Coordinates startingLoc, Coordinates direction, double len, double headSize, Color lineColor, WorldTransform wt)
{
Coordinates endingLoc = startingLoc + direction.Normalize(len);
Coordinates headPt0 = endingLoc + direction.Rotate(135 * Math.PI / 180.0).Normalize(headSize);
Coordinates headPt1 = endingLoc + direction.Rotate(-135 * Math.PI / 180.0).Normalize(headSize);
IPen pen = g.CreatePen();
pen.Width = 3 / wt.Scale;
pen.Color = Color.White;
PointF ptfStart = Utility.ToPointF(startingLoc);
PointF ptfEnd = Utility.ToPointF(endingLoc);
PointF ptfHeadPt0 = Utility.ToPointF(headPt0);
PointF ptfHeadPt1 = Utility.ToPointF(headPt1);
PointF[] headPts = new PointF[] { ptfHeadPt0, ptfEnd, ptfHeadPt1 };
g.DrawLine(pen, ptfStart, ptfEnd);
g.DrawLines(pen, headPts);
pen.Width = 1 / wt.Scale;
pen.Color = lineColor;
g.DrawLine(pen, ptfStart, ptfEnd);
g.DrawLines(pen, headPts);
}
public void Render(IGraphics g, WorldTransform wt)
{
LinePath path = line;
if (path == null)
{
return;
}
PointF[] pts = Utility.ToPointF(path);
IPen p = g.CreatePen();
p.Color = color;
p.Width = nom_pixel_width / wt.Scale;
g.DrawLines(p, pts);
p.Dispose();
if (labelPoints)
{
for (int i = 0; i < path.Count; i++)
{
DrawingUtility.DrawControlPoint(g, path[i], Color.Black, i.ToString(), ContentAlignment.MiddleRight, ControlPointStyle.SmallX, wt);
}
}
}
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)
{
Polygon curPoly = poly;
if (curPoly == null)
{
return;
}
IPen pen = g.CreatePen();
pen.Width = nom_pixel_width / wt.Scale;
pen.Color = color;
if (drawFilled)
{
g.FillPolygon(Color.FromArgb(100, color), Utility.ToPointF(curPoly));
}
g.DrawPolygon(pen, Utility.ToPointF(curPoly));
if (drawCentroid)
{
DrawingUtility.DrawControlPoint(g, curPoly.GetCentroid(), color, null, ContentAlignment.BottomCenter, ControlPointStyle.LargeX, wt);
}
pen.Dispose();
}
public void Render(IGraphics g, WorldTransform wt)
{
float penWidth = nominal_pixel_width / wt.Scale;
IPen p = g.CreatePen();
p.Width = penWidth;
p.Color = color;
// check if the radius is too big
if (Math.Abs(curvature) < curvature_tol)
{
// draw a straight line
p.DashStyle = DashStyle.Dash;
Coordinates t = tangent.Normalize() * 100;
PointF start = Utility.ToPointF(edgePoint - t);
PointF end = Utility.ToPointF(edgePoint + t);
g.DrawLine(p, start, end);
}
else
{
// calculate center point
Circle c = Circle.FromPointSlopeRadius(edgePoint, tangent, 1 / curvature);
Rect rect = c.GetBoundingRectangle();
g.DrawEllipse(p, Utility.ToRectangleF(rect));
}
}
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();
}
private void DrawRuler(IGraphics g, WorldTransform transform, Coordinates start, Coordinates end)
{
// determine what alignment to use
Coordinates diff = end - start;
ContentAlignment align;
if (diff.X > 0 && diff.Y > 0)
{
align = ContentAlignment.MiddleRight;
}
else if (diff.X < 0 && diff.Y > 0)
{
align = ContentAlignment.MiddleLeft;
}
else if (diff.X < 0 && diff.Y < 0)
{
align = ContentAlignment.MiddleLeft;
}
else if (diff.X > 0 && diff.Y < 0)
{
align = ContentAlignment.MiddleRight;
}
else if (diff.X == 0 && diff.Y > 0)
{
align = ContentAlignment.MiddleRight;
}
else if (diff.X == 0 && diff.Y < 0)
{
align = ContentAlignment.MiddleRight;
}
else if (diff.X < 0 && diff.Y == 0)
{
align = ContentAlignment.MiddleLeft;
}
else if (diff.X > 0 && diff.Y == 0)
{
align = ContentAlignment.MiddleRight;
}
else
{
align = ContentAlignment.MiddleRight;
}
double dist = diff.VectorLength;
string label = dist.ToString("F2") + " m, (" + end.X.ToString("F1") + "," + end.Y.ToString("F1") + ")";
using (IPen pen = g.CreatePen()) {
pen.Color = Color.DarkRed;
pen.Width = 1.5f / transform.Scale;
g.DrawLine(pen, Utility.ToPointF(start), Utility.ToPointF(end));
}
DrawingUtility.DrawControlPoint(g, start, Color.DarkRed, null, ContentAlignment.BottomRight, ControlPointStyle.LargeCircle, transform);
DrawingUtility.DrawControlPoint(g, end, Color.DarkRed, label, align, ControlPointStyle.LargeCircle, transform);
}
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 static void DrawControlLine(IGraphics g, Color c, DashStyle style, Coordinates loc1, Coordinates loc2, WorldTransform wt)
{
float pw = 1.25f / wt.Scale;
using (IPen p = g.CreatePen()) {
p.Color = c;
p.Width = pw;
p.DashStyle = style;
g.DrawLine(p, Utility.ToPointF(loc1), Utility.ToPointF(loc2));
}
}
public static void DrawControlLine(IGraphics g, Color c, DashStyle style, Coordinates loc1, Coordinates loc2, WorldTransform wt)
{
float pw = 1.25f / wt.Scale;
using (IPen p = g.CreatePen()) {
p.Color = c;
p.Width = pw;
p.DashStyle = style;
g.DrawLine(p, Utility.ToPointF(loc1), Utility.ToPointF(loc2));
}
}
public void Render(IGraphics g, WorldTransform wt)
{
IPen p = g.CreatePen();
p.Color = color;
p.Width = nomPixelWidth / wt.Scale;
PointF[] points = Utility.ToPointF(segment.ToPoints(30));
g.DrawLines(p, points);
p.Dispose();
}
public void Render(IGraphics g, WorldTransform wt)
{
if (points == null || points.Count < 2)
{
return;
}
IPen pen = g.CreatePen();
pen.Width = nom_pixel_width / wt.Scale;
pen.Color = color;
g.DrawLines(pen, points.ToArray());
}
public void Render(IGraphics g, WorldTransform wt)
{
if (circle.Equals(Circle.Infinite))
{
return;
}
float penWidth = nominal_pixel_width / wt.Scale;
IPen p = g.CreatePen();
p.Width = penWidth;
p.Color = color;
g.DrawEllipse(p, Utility.ToRectangleF(circle.GetBoundingRectangle()));
}
private void DrawObstacle(IGraphics g, WorldTransform wt, OperationalObstacle obs)
{
IPen pen = g.CreatePen();
pen.Width = nom_pixel_width / wt.Scale;
pen.Color = classColors[(int)obs.obstacleClass];
g.DrawPolygon(pen, Utility.ToPointF(obs.poly));
if (obs.headingValid)
{
DrawingUtility.DrawArrow(g, obs.poly.GetCentroid(), Coordinates.FromAngle(obs.heading), 4, 0.75, Color.Black, wt);
}
if (ageRenderFlag || obs.ignored)
{
// draw the model confidence
string labelString = "";
if (ageRenderFlag)
{
labelString += obs.age.ToString();
}
if (obs.ignored)
{
if (labelString.Length != 0)
{
labelString += ", ";
}
labelString += "IGN";
}
SizeF stringSize = g.MeasureString(labelString, labelFont);
stringSize.Width /= wt.Scale;
stringSize.Height /= wt.Scale;
Coordinates labelPt = obs.poly.GetCentroid();
RectangleF rect = new RectangleF(Utility.ToPointF(labelPt), stringSize);
float inflateValue = 4 / wt.Scale;
rect.X -= inflateValue;
rect.Y -= inflateValue;
rect.Width += 2 / wt.Scale;
g.FillRectangle(Color.FromArgb(127, Color.White), rect);
g.DrawString(labelString, labelFont, Color.Black, Utility.ToPointF(labelPt));
}
}
private void DrawZoomBox(IGraphics g, WorldTransform transform, Coordinates start, Coordinates end)
{
float x, y;
float width, height;
if (start.X < end.X)
{
x = (float)start.X;
width = (float)(end.X - start.X);
}
else
{
x = (float)end.X;
width = (float)(start.X - end.X);
}
if (start.Y < end.Y)
{
y = (float)start.Y;
height = (float)(end.Y - start.Y);
}
else
{
y = (float)end.Y;
height = (float)(start.Y - end.Y);
}
// create the rectangle
RectangleF rect = new RectangleF(x, y, width, height);
// draw the transparent background
g.FillRectangle(Color.FromArgb(50, Color.Purple), rect);
IPen pen = g.CreatePen();
pen.Width = 1.0f / transform.Scale;
pen.Color = Color.Purple;
g.DrawRectangle(pen, rect);
pen.Dispose();
}
public void Render(IGraphics g, WorldTransform wt)
{
Polygon[] polys = this.polys;
if (polys == null)
{
return;
}
IPen p = g.CreatePen();
p.Width = nom_pixel_width / wt.Scale;
p.Color = color;
foreach (Polygon poly in polys)
{
g.DrawPolygon(p, Utility.ToPointF(poly));
}
p.Dispose();
}
public static void DrawArrow(IGraphics g, Coordinates startingLoc, Coordinates direction, double len, double headSize, Color lineColor, WorldTransform wt)
{
Coordinates endingLoc = startingLoc + direction.Normalize(len);
Coordinates headPt0 = endingLoc + direction.Rotate(135*Math.PI/180.0).Normalize(headSize);
Coordinates headPt1 = endingLoc + direction.Rotate(-135*Math.PI/180.0).Normalize(headSize);
IPen pen = g.CreatePen();
pen.Width = 3/wt.Scale;
pen.Color = Color.White;
PointF ptfStart = Utility.ToPointF(startingLoc);
PointF ptfEnd = Utility.ToPointF(endingLoc);
PointF ptfHeadPt0 = Utility.ToPointF(headPt0);
PointF ptfHeadPt1 = Utility.ToPointF(headPt1);
PointF[] headPts = new PointF[] { ptfHeadPt0, ptfEnd, ptfHeadPt1 };
g.DrawLine(pen, ptfStart, ptfEnd);
g.DrawLines(pen, headPts);
pen.Width = 1/wt.Scale;
pen.Color = lineColor;
g.DrawLine(pen, ptfStart, ptfEnd);
g.DrawLines(pen, headPts);
}
public void Render(IGraphics g, WorldTransform wt)
{
Coordinates wll = wt.WorldLowerLeft;
Coordinates wur = wt.WorldUpperRight;
PointF ll = new PointF((float)wll.X, (float)wll.Y);
PointF ur = new PointF((float)wur.X, (float)wur.Y);
float startX = (float)Math.Floor(wll.X / spacing) * spacing;
float endX = (float)Math.Ceiling(wur.X / spacing) * spacing;
float startY = (float)Math.Floor(wll.Y / spacing) * spacing;
float endY = (float)Math.Ceiling(wur.Y / spacing) * spacing;
IPen p = g.CreatePen();
p.Color = color;
p.Width = nominal_pixel_width / wt.Scale;
string formatString;
if (spacing >= 1)
{
formatString = "F0";
}
else if (spacing >= 0.1)
{
formatString = "F1";
}
else if (spacing >= 0.01)
{
formatString = "F2";
}
else
{
formatString = "F4";
}
// find the largest value (in magnitude) that we'll need to draw, assuming this will be the max length string
float testVal = Math.Max(Math.Max(Math.Abs(startX), Math.Abs(endX)), Math.Max(Math.Abs(startY), Math.Abs(endY)));
string testString = testVal.ToString(formatString);
SizeF nomStringSize = g.MeasureString(testString, labelFont);
SizeF unitStringSize = g.MeasureString(testString + " m", labelFont);
float pixelSpacing = spacing * wt.Scale;
bool drawLabels = showLabels && pixelSpacing >= (nomStringSize.Width + nominal_label_spacing * 2);
bool drawUnits = pixelSpacing >= (unitStringSize.Width + nominal_label_spacing * 2);
float labelSpacing = nominal_label_spacing / wt.Scale;
// don't draw if there are too many lines
if ((endX - startX) / spacing <= max_lines && (endY - startY) / spacing <= max_lines && pixelSpacing >= min_pixel_spacing)
{
for (float x = startX; x <= endX; x += spacing)
{
g.DrawLine(p, new PointF(x, ll.Y), new PointF(x, ur.Y));
}
for (float y = startY; y <= endY; y += spacing)
{
g.DrawLine(p, new PointF(ll.X, y), new PointF(ur.X, y));
}
if (drawLabels)
{
float minX = ll.X + unitStringSize.Width / wt.Scale + 2 * labelSpacing;
for (float x = startX; x <= endX; x += spacing)
{
if (x > minX)
{
g.DrawString(x.ToString(formatString) + (drawUnits ? " m" : ""), labelFont, Color.Black, new PointF(x + labelSpacing, ll.Y + labelSpacing + nomStringSize.Height / wt.Scale));
}
}
for (float y = startY; y <= endY; y += spacing)
{
g.DrawString(y.ToString(formatString) + (drawUnits ? " m" : ""), labelFont, Color.Black, new PointF(ll.X + labelSpacing, y + labelSpacing));
}
}
}
}
public void Render(IGraphics g, WorldTransform wt)
{
LocalLaneModel laneModel = this.laneModel;
if (laneModel == null || laneModel.LanePath == null || laneModel.LanePath.Count <= 1)
{
return;
}
// generate the left and right bounds
LinePath leftBound = laneModel.LanePath.ShiftLateral(laneModel.Width / 2.0);
LinePath leftBound2 = null;
if (laneModel.WidthVariance > 0.01)
{
leftBound2 = laneModel.LanePath.ShiftLateral(laneModel.Width / 2.0 + Math.Sqrt(laneModel.WidthVariance) * 1.96 / 2.0);
}
LinePath rightBound = laneModel.LanePath.ShiftLateral(-laneModel.Width / 2.0);
LinePath rightBound2 = null;
if (laneModel.WidthVariance > 0.01)
{
rightBound2 = laneModel.LanePath.ShiftLateral(-laneModel.Width / 2.0 - Math.Sqrt(laneModel.WidthVariance) * 1.96 / 2.0);
}
// generate a polygon of the confidence region of the center line
Polygon centerConfLeft = null;
Polygon centerConfRight = null;
Polygon centerConfFull = null;
if (sigma > 0 && laneModel.LaneYVariance != null && laneModel.LaneYVariance.Length == laneModel.LanePath.Count)
{
float varianceThreshold = 9f * 9f;
if (laneModel.LaneYVariance[0] > varianceThreshold)
{
varianceThreshold = 600;
}
int numPoints;
for (numPoints = 0; numPoints < laneModel.LaneYVariance.Length; numPoints++)
{
if (laneModel.LaneYVariance[numPoints] > varianceThreshold)
{
break;
}
}
double[] leftDist = new double[numPoints];
double[] rightDist = new double[numPoints];
double prevDist = 0;
for (int i = 0; i < numPoints; i++)
{
double dist = laneModel.LaneYVariance[i];
if (dist > 0.01)
{
dist = Math.Sqrt(dist) * sigma;
leftDist[i] = dist;
rightDist[i] = -dist;
prevDist = dist;
}
else
{
leftDist[i] = prevDist;
rightDist[i] = -prevDist;
}
}
// get the subset of the lane model we're interested in
LinePath subset = laneModel.LanePath.SubPath(0, numPoints - 1);
LinePath left = subset.ShiftLateral(leftDist);
LinePath right = subset.ShiftLateral(rightDist);
Coordinates midTop = (left[left.Count - 1] + right[right.Count - 1]) / 2.0;
Coordinates midBottom = (left[0] + right[0]) / 2.0;
centerConfLeft = new Polygon(numPoints + 2);
centerConfLeft.Add(midTop);
centerConfLeft.Add(midBottom);
centerConfLeft.AddRange(left);
centerConfRight = new Polygon(numPoints + 2);
centerConfRight.Add(midTop);
centerConfRight.Add(midBottom);
centerConfRight.AddRange(right);
centerConfFull = new Polygon(numPoints * 2);
right.Reverse();
centerConfFull.AddRange(right);
centerConfFull.AddRange(left);
}
// draw the shits
IPen pen = g.CreatePen();
pen.Width = 1.0f / wt.Scale;
pen.Color = color;
// first draw the confidence polygon
if (centerConfFull != null)
{
g.FillPolygon(Color.FromArgb(20, color), Utility.ToPointF(centerConfLeft));
g.FillPolygon(Color.FromArgb(20, color), Utility.ToPointF(centerConfRight));
pen.Color = Color.FromArgb(30, color);
g.DrawPolygon(pen, Utility.ToPointF(centerConfFull));
}
// next draw the center line
pen.Color = color;
//g.DrawLines(pen, Utility.ToPointF(laneModel.LanePath));
//// next draw the left lane confidence bound
//if (leftBound2 != null) {
// pen.DashStyle = System.Drawing.Drawing2D.DashStyle.Dot;
// g.DrawLines(pen, Utility.ToPointF(leftBound2));
//}
//// draw the right lane confidence bound
//if (rightBound2 != null) {
// pen.DashStyle = System.Drawing.Drawing2D.DashStyle.Dot;
// g.DrawLines(pen, Utility.ToPointF(rightBound2));
//}
// draw the left bound
pen.DashStyle = System.Drawing.Drawing2D.DashStyle.Solid;
g.DrawLines(pen, Utility.ToPointF(leftBound));
// draw the right bound
g.DrawLines(pen, Utility.ToPointF(rightBound));
// draw the model confidence
string labelString = laneModel.Probability.ToString("F3");
SizeF stringSize = g.MeasureString(labelString, labelFont);
stringSize.Width /= wt.Scale;
stringSize.Height /= wt.Scale;
RectangleF rect = new RectangleF(Utility.ToPointF(laneModel.LanePath[0]), stringSize);
float inflateValue = 4 / wt.Scale;
rect.X -= inflateValue;
rect.Y -= inflateValue;
g.FillRectangle(Color.FromArgb(127, Color.White), rect);
g.DrawString(labelString, labelFont, Color.Black, Utility.ToPointF(laneModel.LanePath[0]));
prevLeftBound = leftBound;
prevRightBound = rightBound;
}
/// <summary>
/// Creates pen object.
/// </summary>
/// <param name="graphics"><see cref="IGraphics"/> object.</param>
/// <param name="color">Pen color.</param>
/// <param name="thickness">Pen thickness.</param>
/// <returns>Pen object.</returns>
public static IPen CreatePen(this IGraphics graphics, Color color, double thickness = 1)
{
IBrush brush = graphics.CreateSolidColorBrush(color);
return(graphics.CreatePen(brush, thickness));
}
private void DrawZoomBox(IGraphics g, WorldTransform transform, Coordinates start, Coordinates end)
{
float x, y;
float width, height;
if (start.X < end.X) {
x = (float)start.X;
width = (float)(end.X - start.X);
}
else {
x = (float)end.X;
width = (float)(start.X - end.X);
}
if (start.Y < end.Y) {
y = (float)start.Y;
height = (float)(end.Y - start.Y);
}
else {
y = (float)end.Y;
height = (float)(start.Y - end.Y);
}
// create the rectangle
RectangleF rect = new RectangleF(x, y, width, height);
// draw the transparent background
g.FillRectangle(Color.FromArgb(50, Color.Purple), rect);
IPen pen = g.CreatePen();
pen.Width = 1.0f/transform.Scale;
pen.Color = Color.Purple;
g.DrawRectangle(pen, rect);
pen.Dispose();
}
public static void DrawControlPoint(IGraphics g, Coordinates loc, Color color, string label, ContentAlignment align, ControlPointStyle style, bool drawTextBox, WorldTransform wt)
{
// figure out the size the control box needs to be in world coordinates to make it
// show up as appropriate in view coordinates
// invert the scale
float scaled_size = 0;
if (style == ControlPointStyle.LargeBox || style == ControlPointStyle.LargeCircle || style == ControlPointStyle.LargeX)
{
scaled_size = cp_large_size / wt.Scale;
}
else
{
scaled_size = cp_small_size / wt.Scale;
}
float scaled_offset = 1 / wt.Scale;
// assume that the world transform is currently applied correctly to the graphics
RectangleF rect = new RectangleF(-scaled_size / 2, -scaled_size / 2, scaled_size, scaled_size);
rect.Offset(Utility.ToPointF(loc));
if (style == ControlPointStyle.LargeBox)
{
g.FillRectangle(Color.White, rect);
// shrink the rect down a little (nominally 1 pixel)
rect.Inflate(-scaled_offset, -scaled_offset);
g.FillRectangle(color, rect);
}
else if (style == ControlPointStyle.LargeCircle)
{
g.FillEllipse(Color.White, rect);
// shrink the rect down a little (nominally 1 pixel)
rect.Inflate(-scaled_offset, -scaled_offset);
g.FillEllipse(color, rect);
}
else if (style == ControlPointStyle.LargeX)
{
using (IPen p = g.CreatePen()) {
p.Width = 3 / wt.Scale;
p.Color = Color.White;
g.DrawLine(p, new PointF(rect.Left, rect.Top), new PointF(rect.Right, rect.Bottom));
g.DrawLine(p, new PointF(rect.Left, rect.Bottom), new PointF(rect.Right, rect.Top));
p.Width = scaled_offset;
p.Color = color;
g.DrawLine(p, new PointF(rect.Left, rect.Top), new PointF(rect.Right, rect.Bottom));
g.DrawLine(p, new PointF(rect.Left, rect.Bottom), new PointF(rect.Right, rect.Top));
}
}
else if (style == ControlPointStyle.SmallBox)
{
g.FillRectangle(color, rect);
}
else if (style == ControlPointStyle.SmallCircle)
{
g.FillEllipse(color, rect);
}
else if (style == ControlPointStyle.SmallX)
{
using (IPen p = g.CreatePen()) {
p.Width = 3 / wt.Scale;
p.Color = color;
g.DrawLine(p, new PointF(rect.Left, rect.Top), new PointF(rect.Right, rect.Bottom));
g.DrawLine(p, new PointF(rect.Left, rect.Bottom), new PointF(rect.Right, rect.Top));
}
}
if (!string.IsNullOrEmpty(label))
{
SizeF strSize = g.MeasureString(label, label_font);
float x = 0, y = 0;
if (align == ContentAlignment.BottomRight || align == ContentAlignment.MiddleRight || align == ContentAlignment.TopRight)
{
x = (float)loc.X + cp_label_space / wt.Scale;
}
else if (align == ContentAlignment.BottomCenter || align == ContentAlignment.MiddleCenter || align == ContentAlignment.TopCenter)
{
x = (float)loc.X - strSize.Width / (2 * wt.Scale);
}
else if (align == ContentAlignment.BottomLeft || align == ContentAlignment.MiddleLeft || align == ContentAlignment.TopLeft)
{
x = (float)loc.X - (strSize.Width + cp_label_space) / wt.Scale;
}
if (align == ContentAlignment.BottomCenter || align == ContentAlignment.BottomLeft || align == ContentAlignment.BottomRight)
{
y = (float)loc.Y - cp_label_space / wt.Scale;
}
else if (align == ContentAlignment.MiddleCenter || align == ContentAlignment.MiddleLeft || align == ContentAlignment.MiddleRight)
{
y = (float)loc.Y + strSize.Height / (2 * wt.Scale);
}
else if (align == ContentAlignment.TopCenter || align == ContentAlignment.TopLeft || align == ContentAlignment.TopRight)
{
y = (float)loc.Y + (strSize.Height + cp_label_space) / wt.Scale;
}
PointF text_loc = new PointF(x, y);
if (drawTextBox)
{
RectangleF text_rect = new RectangleF(text_loc.X - 4 / wt.Scale, text_loc.Y - 4 / wt.Scale, strSize.Width / wt.Scale, strSize.Height / wt.Scale);
g.FillRectangle(Color.FromArgb(127, Color.White), text_rect);
}
g.DrawString(label, label_font, color, text_loc);
}
}
public static void DrawControlPoint(IGraphics g, Coordinates loc, Color color, string label, ContentAlignment align, ControlPointStyle style, bool drawTextBox, WorldTransform wt)
{
// figure out the size the control box needs to be in world coordinates to make it
// show up as appropriate in view coordinates
// invert the scale
float scaled_size = 0;
if (style == ControlPointStyle.LargeBox || style == ControlPointStyle.LargeCircle || style == ControlPointStyle.LargeX) {
scaled_size = cp_large_size / wt.Scale;
}
else {
scaled_size = cp_small_size / wt.Scale;
}
float scaled_offset = 1 / wt.Scale;
// assume that the world transform is currently applied correctly to the graphics
RectangleF rect = new RectangleF(-scaled_size / 2, -scaled_size / 2, scaled_size, scaled_size);
rect.Offset(Utility.ToPointF(loc));
if (style == ControlPointStyle.LargeBox) {
g.FillRectangle(Color.White, rect);
// shrink the rect down a little (nominally 1 pixel)
rect.Inflate(-scaled_offset, -scaled_offset);
g.FillRectangle(color, rect);
}
else if (style == ControlPointStyle.LargeCircle) {
g.FillEllipse(Color.White, rect);
// shrink the rect down a little (nominally 1 pixel)
rect.Inflate(-scaled_offset, -scaled_offset);
g.FillEllipse(color, rect);
}
else if (style == ControlPointStyle.LargeX) {
using (IPen p = g.CreatePen()) {
p.Width = 3/wt.Scale;
p.Color = Color.White;
g.DrawLine(p, new PointF(rect.Left, rect.Top), new PointF(rect.Right, rect.Bottom));
g.DrawLine(p, new PointF(rect.Left, rect.Bottom), new PointF(rect.Right, rect.Top));
p.Width = scaled_offset;
p.Color = color;
g.DrawLine(p, new PointF(rect.Left, rect.Top), new PointF(rect.Right, rect.Bottom));
g.DrawLine(p, new PointF(rect.Left, rect.Bottom), new PointF(rect.Right, rect.Top));
}
}
else if (style == ControlPointStyle.SmallBox) {
g.FillRectangle(color, rect);
}
else if (style == ControlPointStyle.SmallCircle) {
g.FillEllipse(color, rect);
}
else if (style == ControlPointStyle.SmallX) {
using (IPen p = g.CreatePen()) {
p.Width = 3/wt.Scale;
p.Color = color;
g.DrawLine(p, new PointF(rect.Left, rect.Top), new PointF(rect.Right, rect.Bottom));
g.DrawLine(p, new PointF(rect.Left, rect.Bottom), new PointF(rect.Right, rect.Top));
}
}
if (!string.IsNullOrEmpty(label)) {
SizeF strSize = g.MeasureString(label, label_font);
float x = 0, y = 0;
if (align == ContentAlignment.BottomRight || align == ContentAlignment.MiddleRight || align == ContentAlignment.TopRight) {
x = (float)loc.X + cp_label_space / wt.Scale;
}
else if (align == ContentAlignment.BottomCenter || align == ContentAlignment.MiddleCenter || align == ContentAlignment.TopCenter) {
x = (float)loc.X - strSize.Width / (2 * wt.Scale);
}
else if (align == ContentAlignment.BottomLeft || align == ContentAlignment.MiddleLeft || align == ContentAlignment.TopLeft) {
x = (float)loc.X - (strSize.Width + cp_label_space) / wt.Scale;
}
if (align == ContentAlignment.BottomCenter || align == ContentAlignment.BottomLeft || align == ContentAlignment.BottomRight) {
y = (float)loc.Y - cp_label_space / wt.Scale;
}
else if (align == ContentAlignment.MiddleCenter || align == ContentAlignment.MiddleLeft || align == ContentAlignment.MiddleRight) {
y = (float)loc.Y + strSize.Height / (2 * wt.Scale);
}
else if (align == ContentAlignment.TopCenter || align == ContentAlignment.TopLeft || align == ContentAlignment.TopRight) {
y = (float)loc.Y + (strSize.Height + cp_label_space) / wt.Scale;
}
PointF text_loc = new PointF(x, y);
if (drawTextBox) {
RectangleF text_rect = new RectangleF(text_loc.X - 4/wt.Scale, text_loc.Y - 4/wt.Scale, strSize.Width/wt.Scale, strSize.Height/wt.Scale);
g.FillRectangle(Color.FromArgb(127, Color.White), text_rect);
}
g.DrawString(label, label_font, color, text_loc);
}
}
public void Render(IGraphics g, WorldTransform wt)
{
Coordinates wll = wt.WorldLowerLeft;
Coordinates wur = wt.WorldUpperRight;
PointF ll = new PointF((float)wll.X, (float)wll.Y);
PointF ur = new PointF((float)wur.X, (float)wur.Y);
float startX = (float)Math.Floor(wll.X / spacing) * spacing;
float endX = (float)Math.Ceiling(wur.X / spacing) * spacing;
float startY = (float)Math.Floor(wll.Y / spacing) * spacing;
float endY = (float)Math.Ceiling(wur.Y / spacing) * spacing;
IPen p = g.CreatePen();
p.Color = color;
p.Width = nominal_pixel_width/wt.Scale;
string formatString;
if (spacing >= 1) {
formatString = "F0";
}
else if (spacing >= 0.1) {
formatString = "F1";
}
else if (spacing >= 0.01) {
formatString = "F2";
}
else {
formatString = "F4";
}
// find the largest value (in magnitude) that we'll need to draw, assuming this will be the max length string
float testVal = Math.Max(Math.Max(Math.Abs(startX), Math.Abs(endX)), Math.Max(Math.Abs(startY), Math.Abs(endY)));
string testString = testVal.ToString(formatString);
SizeF nomStringSize = g.MeasureString(testString, labelFont);
SizeF unitStringSize = g.MeasureString(testString + " m", labelFont);
float pixelSpacing = spacing * wt.Scale;
bool drawLabels = showLabels && pixelSpacing >= (nomStringSize.Width + nominal_label_spacing*2);
bool drawUnits = pixelSpacing >= (unitStringSize.Width + nominal_label_spacing*2);
float labelSpacing = nominal_label_spacing/wt.Scale;
// don't draw if there are too many lines
if ((endX - startX) / spacing <= max_lines && (endY - startY) / spacing <= max_lines && pixelSpacing >= min_pixel_spacing) {
for (float x = startX; x <= endX; x += spacing) {
g.DrawLine(p, new PointF(x, ll.Y), new PointF(x, ur.Y));
}
for (float y = startY; y <= endY; y += spacing) {
g.DrawLine(p, new PointF(ll.X, y), new PointF(ur.X, y));
}
if (drawLabels) {
float minX = ll.X + unitStringSize.Width/wt.Scale + 2*labelSpacing;
for (float x = startX; x <= endX; x += spacing) {
if (x > minX) {
g.DrawString(x.ToString(formatString) + (drawUnits ? " m" : ""), labelFont, Color.Black, new PointF(x + labelSpacing, ll.Y + labelSpacing + nomStringSize.Height/wt.Scale));
}
}
for (float y = startY; y <= endY; y += spacing) {
g.DrawString(y.ToString(formatString) + (drawUnits ? " m" : ""), labelFont, Color.Black, new PointF(ll.X + labelSpacing, y + labelSpacing));
}
}
}
}
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();
}
