public void Render(System.Drawing.Graphics g, WorldTransform t)
{
if (staticObstacles.Obstacles != null)
{
foreach (ObservedObstacle obstacle in staticObstacles.Obstacles)
{
// get front of the vehicle
Coordinates vPos = vehicleState.xyPosition;
Coordinates vHead = vehicleState.heading;
// get imu position of vehicle
Coordinates imuPos = vPos + vHead.Normalize(0.4);
// get obstacle relative vector
Coordinates obstacleRelative = obstacle.ObstacleVector;
// get final vector
Coordinates final = imuPos + obstacleRelative.Rotate(vHead.ArcTan);
// draw the obstacle
DrawingUtility.DrawControlPoint(final, DrawingUtility.pointObstacleColor, null, System.Drawing.ContentAlignment.MiddleCenter, ControlPointStyle.SmallCircle, g, t);
}
}
}
public void Render(System.Drawing.Graphics g, WorldTransform t)
{
if (rndf != null && DrawingUtility.DrawRndf)
{
foreach (Segment segment in rndf.Segments.Values)
{
foreach (Way way in segment.Ways.Values)
{
foreach (Lane lane in way.Lanes.Values)
{
foreach (LanePartition lanePartition in lane.LanePartitions)
{
// Draw Line representing the Lane Partition w/ added WayColor if it is wanted
if (DrawingUtility.DrawLanePartitions)
{
if (way.WayID.WayNumber == 1)
{
DrawingUtility.DrawControlLine(lanePartition.InitialWaypoint.Position, lanePartition.FinalWaypoint.Position, DrawingUtility.LanePartitionWay1Color, g, t);
}
else
{
DrawingUtility.DrawControlLine(lanePartition.InitialWaypoint.Position, lanePartition.FinalWaypoint.Position, DrawingUtility.LanePartitionWay2Color, g, t);
}
}
foreach (UserPartition userPartition in lanePartition.UserPartitions)
{
// Draw Line representing the User Partition w/ added WayColor if it is wanted
if (DrawingUtility.DrawUserPartitions)
{
if (way.WayID.WayNumber == 1)
{
DrawingUtility.DrawControlLine(userPartition.InitialWaypoint.Position, userPartition.FinalWaypoint.Position, DrawingUtility.UserPartitionWay1Color, g, t);
}
else
{
DrawingUtility.DrawControlLine(userPartition.InitialWaypoint.Position, userPartition.FinalWaypoint.Position, DrawingUtility.UserPartitionWay2Color, g, t);
}
}
// Draw Final User Waypoints if User Waypoints and if wanted
if (DrawingUtility.DrawUserWaypoints)
{
if (userPartition.FinalWaypoint is UserWaypoint)
{
Color c = DrawingUtility.GetWaypointColor(userPartition.FinalWaypoint);
if (DrawingUtility.DrawUserWaypointText)
{
DrawingUtility.DrawControlPoint(userPartition.FinalWaypoint.Position, c, userPartition.FinalWaypoint.ToString(), ContentAlignment.MiddleCenter, ControlPointStyle.SmallCircle, g, t);
}
else
{
DrawingUtility.DrawControlPoint(userPartition.FinalWaypoint.Position, c, null, ContentAlignment.BottomCenter, ControlPointStyle.SmallCircle, g, t);
}
}
}
}
}
// draw splines
if (DrawingUtility.DisplayLaneSplines)
{
List <Coordinates> waypointLocations = new List <Coordinates>();
foreach (RndfWayPoint waypoint in lane.Waypoints.Values)
{
waypointLocations.Add(waypoint.Position);
}
// get spline
List <CubicBezier> c2Spline = RndfTools.SplineC2FromPoints(waypointLocations);
float nomPixelWidth = 2.0f;
float penWidth = nomPixelWidth / t.Scale;
Pen pen = new Pen(Color.FromArgb(100, Color.DarkSeaGreen), penWidth);
foreach (CubicBezier cb in c2Spline)
{
g.DrawBezier(pen, DrawingUtility.ToPointF(cb.P0), DrawingUtility.ToPointF(cb.P1), DrawingUtility.ToPointF(cb.P2), DrawingUtility.ToPointF(cb.P3));
}
}
if (DrawingUtility.DrawRndfWaypoints)
{
foreach (RndfWayPoint rndfWayPoint in lane.Waypoints.Values)
{
if (DrawingUtility.DrawRndfWaypointText)
{
DrawingUtility.DrawControlPoint(rndfWayPoint.Position, DrawingUtility.GetWaypointColor(rndfWayPoint), rndfWayPoint.WaypointID.ToString(), ContentAlignment.BottomCenter, ControlPointStyle.SmallCircle, g, t);
}
else
{
DrawingUtility.DrawControlPoint(rndfWayPoint.Position, DrawingUtility.GetWaypointColor(rndfWayPoint), null, ContentAlignment.BottomCenter, ControlPointStyle.SmallCircle, g, t);
}
if (DrawingUtility.DisplayRndfGoals && rndfWayPoint.IsCheckpoint)
{
DrawingUtility.DrawControlPoint(rndfWayPoint.Position, DrawingUtility.GetWaypointColor(rndfWayPoint), rndfWayPoint.CheckpointNumber.ToString(), ContentAlignment.TopCenter, ControlPointStyle.SmallCircle, g, t);
}
}
}
}
}
}
if (DrawingUtility.DrawInterconnects)
{
foreach (Interconnect interconnect in rndf.Interconnects.Values)
{
DrawingUtility.DrawControlLine(interconnect.InitialWaypoint.Position, interconnect.FinalWaypoint.Position, DrawingUtility.InterconnectColor, g, t);
}
}
if (DrawingUtility.DisplayIntersectionSplines)
{
foreach (Interconnect interconnect in rndf.Interconnects.Values)
{
try
{
Coordinates d0 = interconnect.InitialWaypoint.Position - interconnect.InitialWaypoint.PreviousLanePartition.InitialWaypoint.Position;
Coordinates p0 = interconnect.InitialWaypoint.Position;
Coordinates dn = interconnect.FinalWaypoint.NextLanePartition.FinalWaypoint.Position - interconnect.FinalWaypoint.Position;
Coordinates pn = interconnect.FinalWaypoint.Position;
List <Coordinates> coords = new List <Coordinates>();
coords.Add(p0);
if (interconnect.UserPartitions != null)
{
for (int i = 1; i < interconnect.UserPartitions.Count; i++)
{
coords.Add(interconnect.UserPartitions[i].InitialWaypoint.Position);
}
}
coords.Add(pn);
List <CubicBezier> c2Spline = RndfTools.SplineC2FromSegmentAndDerivatives(coords, d0, dn);
float nomPixelWidth = 2.0f;
float penWidth = nomPixelWidth / t.Scale;
Pen pen = new Pen(Color.FromArgb(100, Color.DarkSeaGreen), penWidth);
foreach (CubicBezier cb in c2Spline)
{
g.DrawBezier(pen, DrawingUtility.ToPointF(cb.P0), DrawingUtility.ToPointF(cb.P1), DrawingUtility.ToPointF(cb.P2), DrawingUtility.ToPointF(cb.P3));
}
}
catch (Exception ex)
{
Console.WriteLine(ex.ToString());
}
}
}
if (DrawingUtility.DisplayIntersectionBounds && rndf.intersections != null)
{
foreach (Intersection intersection in rndf.intersections.Values)
{
if (intersection.Perimeter != null)
{
foreach (BoundaryLine boundary in intersection.Perimeter)
{
DrawingUtility.DrawColoredControlLine(DrawingUtility.IntersectionAreaColor, boundary.p1, boundary.p2, g, t);
}
}
}
}
}
}
private void RenderRelativePath(VehicleState vs, Path transPath, Graphics g, WorldTransform t, bool isArbiterPath)
{
if ((isArbiterPath && DrawingUtility.DrawArbiterLanePath) ||
(!isArbiterPath && DrawingUtility.DrawOperationalLanePath))
{
// compute the rotation matrix to add in our vehicles rotation
/*Matrix3 rotMatrix = new Matrix3(
* Math.Cos(vs.heading.ArcTan), -Math.Sin(vs.heading.ArcTan), 0,
* Math.Sin(vs.heading.ArcTan), Math.Cos(vs.heading.ArcTan), 0,
* 0, 0, 1);
*
* // compute the translation matrix to move our vehicle's location
* Matrix3 transMatrix = new Matrix3(
* 1, 0, vs.xyPosition.X,
* 0, 1, vs.xyPosition.Y,
* 0, 0, 1);
*
* // compute the combined transformation matrix
* Matrix3 m = rotMatrix * transMatrix;
*
* // clone, transform and add each segment to our path
* transPath.Transform(m);*/
float nomPixelWidth = 2.0f;
float penWidth = nomPixelWidth / t.Scale;
Pen arbiterPen = new Pen(Color.FromArgb(100, DrawingUtility.ArbiterLanePath), penWidth);
Pen operationalPen = new Pen(Color.FromArgb(100, DrawingUtility.OperationalLanePath), penWidth);
// display path
foreach (IPathSegment ps in transPath)
{
if (ps is BezierPathSegment)
{
BezierPathSegment seg = (BezierPathSegment)ps;
CubicBezier cb = seg.cb;
if (isArbiterPath)
{
g.DrawBezier(arbiterPen, DrawingUtility.ToPointF(cb.P0), DrawingUtility.ToPointF(cb.P1), DrawingUtility.ToPointF(cb.P2), DrawingUtility.ToPointF(cb.P3));
}
else
{
g.DrawBezier(operationalPen, DrawingUtility.ToPointF(cb.P0), DrawingUtility.ToPointF(cb.P1), DrawingUtility.ToPointF(cb.P2), DrawingUtility.ToPointF(cb.P3));
}
}
}
}
}
public void Render(Graphics g, WorldTransform t)
{
Coordinates wll = t.WorldLowerLeft;
Coordinates wup = t.WorldUpperRight;
if ((position.X < wll.X || position.X > wup.X || position.Y < wll.Y || position.Y > wup.Y))
{
return;
}
Matrix bodyTrans = new Matrix();
bodyTrans.Rotate((float)(this.Heading) * 180 / (float)Math.PI - 90);
bodyTrans.Translate((float)position.X, (float)position.Y, MatrixOrder.Append);
Matrix origTrans = g.Transform.Clone();
bodyTrans.Multiply(g.Transform, MatrixOrder.Append);
g.Transform = bodyTrans;
float penWidth = nomPixelWidth / t.Scale;
using (Pen p = new Pen(color, penWidth))
{
DrawRectangle(g, p, bodyRect);
// build the transform for the rear wheels
// do the left wheel
Matrix wheelTransform = bodyTrans.Clone();
wheelTransform.Translate(-wheelOffset, 0, MatrixOrder.Prepend);
try
{
g.Transform = wheelTransform;
g.FillRectangle(Brushes.White, wheelRectL);
DrawRectangle(g, p, wheelRectL);
}
catch (Exception)
{
}
// do the right wheel
wheelTransform = bodyTrans.Clone();
wheelTransform.Translate(wheelOffset, 0, MatrixOrder.Prepend);
try
{
g.Transform = wheelTransform;
g.FillRectangle(Brushes.White, wheelRectR);
DrawRectangle(g, p, wheelRectR);
}
catch (Exception)
{
}
// do the front wheels
// do the left wheel
wheelTransform = bodyTrans.Clone();
wheelTransform.Translate(-wheelOffset, wheelbase, MatrixOrder.Prepend);
wheelTransform.Rotate(steeringAngle * 180 / (float)Math.PI, MatrixOrder.Prepend);
try
{
g.Transform = wheelTransform;
g.FillRectangle(Brushes.White, wheelRectL);
DrawRectangle(g, p, wheelRectL);
}
catch (Exception)
{
}
// do the right wheel
wheelTransform = bodyTrans.Clone();
wheelTransform.Translate(wheelOffset, wheelbase, MatrixOrder.Prepend);
wheelTransform.Rotate(steeringAngle * 180 / (float)Math.PI, MatrixOrder.Prepend);
try
{
g.Transform = wheelTransform;
g.FillRectangle(Brushes.White, wheelRectR);
DrawRectangle(g, p, wheelRectR);
}
catch (Exception)
{
}
}
g.Transform = origTrans;
// draw Position
DrawingUtility.DrawControlPoint(this.position, color, null, ContentAlignment.MiddleCenter, ControlPointStyle.LargeX, g, t);
}
public PointF GetScreenPoint(Coordinates worldLoc)
{
PointF[] pts = new PointF[] { DrawingUtility.ToPointF(worldLoc) };
GetTransform().TransformPoints(pts);
return(pts[0]);
}
public Coordinates GetWorldPoint(PointF screenLoc)
{
PointF[] pts = new PointF[] { screenLoc };
GetInverseTransform().TransformPoints(pts);
return(DrawingUtility.ToCoord(pts[0]));
}
public void Render(Graphics g, WorldTransform t)
{
if (this.observedVehicle.ObservationState != ObservedVehicleState.Deleted ||
(this.observedVehicle.ObservationState == ObservedVehicleState.Deleted &&
DrawingUtility.DisplayDeletedVehicles))
{
Coordinates wll = t.WorldLowerLeft;
Coordinates wup = t.WorldUpperRight;
if ((Position.X < wll.X || Position.X > wup.X || Position.Y < wll.Y || Position.Y > wup.Y))
{
return;
}
Matrix bodyTrans = new Matrix();
bodyTrans.Rotate((float)(this.Heading) * 180 / (float)Math.PI - 90);
bodyTrans.Translate((float)Position.X, (float)Position.Y, MatrixOrder.Append);
Matrix origTrans = g.Transform.Clone();
bodyTrans.Multiply(g.Transform, MatrixOrder.Append);
g.Transform = bodyTrans;
float penWidth = nomPixelWidth / t.Scale;
using (Pen p = new Pen(color, penWidth))
{
DrawRectangle(g, p, bodyRect);
// build the transform for the rear wheels
// do the left wheel
Matrix wheelTransform = bodyTrans.Clone();
wheelTransform.Translate(-WheelOffset, 0, MatrixOrder.Prepend);
try
{
g.Transform = wheelTransform;
g.FillRectangle(Brushes.White, wheelRectL);
DrawRectangle(g, p, wheelRectL);
}
catch (Exception)
{
}
// do the right wheel
wheelTransform = bodyTrans.Clone();
wheelTransform.Translate(WheelOffset, 0, MatrixOrder.Prepend);
try
{
g.Transform = wheelTransform;
g.FillRectangle(Brushes.White, wheelRectR);
DrawRectangle(g, p, wheelRectR);
}
catch (Exception)
{
}
// do the front wheels
// do the left wheel
wheelTransform = bodyTrans.Clone();
wheelTransform.Translate(-WheelOffset, WheelBase, MatrixOrder.Prepend);
wheelTransform.Rotate(steeringAngle * 180 / (float)Math.PI, MatrixOrder.Prepend);
try
{
g.Transform = wheelTransform;
g.FillRectangle(Brushes.White, wheelRectL);
DrawRectangle(g, p, wheelRectL);
}
catch (Exception)
{
}
// do the right wheel
wheelTransform = bodyTrans.Clone();
wheelTransform.Translate(WheelOffset, WheelBase, MatrixOrder.Prepend);
wheelTransform.Rotate(steeringAngle * 180 / (float)Math.PI, MatrixOrder.Prepend);
try
{
g.Transform = wheelTransform;
g.FillRectangle(Brushes.White, wheelRectR);
DrawRectangle(g, p, wheelRectR);
}
catch (Exception)
{
}
}
g.Transform = origTrans;
// draw Position
DrawingUtility.DrawControlPoint(this.Position, color, this.observedVehicle.Id.ToString(), ContentAlignment.MiddleCenter, ControlPointStyle.LargeBox, g, t);
Coordinates head = this.Position + this.observedVehicle.Heading.Normalize(this.observedVehicle.Length / 2.0);
DrawingUtility.DrawControlLine(this.Position, head, color, g, t);
}
}
