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); } }