private void drawPoint(Graphics g, Pen pen, int xCenter, int yCenter, JagPoint p)
{
//Draw x,y
int radius = 5;
int xPoint = (int)(p.x * mapResolution);
int yPoint = (int)(p.y * mapResolution);
g.DrawEllipse(pen, new Rectangle(xCenter + xPoint - radius, yCenter - yPoint - radius, 2 * radius, 2 * radius));
//Draw theta
if (p.hasTheta())
{
int dy = (int)(3 * radius * Math.Sin(p.theta));
int dx = (int)(3 * radius * Math.Cos(p.theta));
g.DrawLine(pen, xCenter + xPoint, yCenter - yPoint, xCenter + xPoint + dx, yCenter - yPoint - dy);
}
}
