//compute present heading, future heading, and theta public static void computeTurn(Turn turn) { double theta1 = 0.0, theta2 = 0.0; double x1 = 0.0, y1 = 0.0, x2 = 0.0, y2 = 0.0; x1 = turn.Locs[0].GpxLocation.Easting; x2 = turn.Locs[1].GpxLocation.Easting; y1 = turn.Locs[0].GpxLocation.Northing; y2 = turn.Locs[1].GpxLocation.Northing; theta1 = calculateTheta(x2 - x1, y2 - y1); x1 = turn.Locs[1].GpxLocation.Easting; x2 = turn.Locs[2].GpxLocation.Easting; y1 = turn.Locs[1].GpxLocation.Northing; y2 = turn.Locs[2].GpxLocation.Northing; theta2 = calculateTheta(x2 - x1, y2 - y1); turn.TurnMagnitude = Math.Abs(theta2 - theta1); if (turn.TurnMagnitude > 5.0) { if (turn.TurnMagnitude > 180.0) turn.TurnMagnitude = 360.0 - turn.TurnMagnitude; if (theta2 - theta1 < 0.0 && Math.Abs(theta2 - theta1) >= 180.0) turn.TurnDirection = "left"; else if (theta2 - theta1 > 0.0 && Math.Abs(theta2 - theta1) >= 180.0) turn.TurnDirection = "right"; else if (theta2 - theta1 < 0.0 && Math.Abs(theta2 - theta1) < 180.0) turn.TurnDirection = "right"; else if (theta2 - theta1 > 0.0 && Math.Abs(theta2 - theta1) < 180.0) turn.TurnDirection = "left"; } else turn.TurnDirection = "straight"; }
public void drawTurn(ref Image map, Turn current) { Graphics g = Graphics.FromImage(map); SolidBrush sb = new SolidBrush(Color.Red); Font f = new Font(FontFamily.GenericMonospace, 12, FontStyle.Bold); Point pt0, pt1, pt2; pt0 = _fd.getPoint(current.Locs[0].GpxLocation); g.FillEllipse(sb, pt0.X - 3, pt0.Y - 3, 6, 6); pt1 = _fd.getPoint(current.Locs[1].GpxLocation); g.FillEllipse(sb, pt1.X - 3, pt1.Y - 3, 6, 6); pt2 = _fd.getPoint(current.Locs[2].GpxLocation); g.FillEllipse(sb, pt2.X - 3, pt2.Y - 3, 6, 6); Pen p = new Pen(sb, 3); g.DrawLine(p, pt0, pt1); g.DrawLine(p, pt1, pt2); }