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