private void DrawOneWaypoint(Graphics g, WayPoint pt, Pen selpen, Pen normpen, Brush selbrush, Brush normbrush)
{
PointF[] triangle = new PointF[] { new PointF(m_tw, 0.0f), new PointF(-m_tw / 2.0f, m_tw / 2.0f), new PointF(-m_tw / 2.0f, -m_tw / 2.0f) };
Matrix mm;
mm = new Matrix();
mm.Translate((float)pt.X, (float)pt.Y);
mm.Rotate((float)pt.Heading);
mm.TransformPoints(triangle);
float angle = (float)XeroUtils.DegreesToRadians(pt.Heading);
//
// Draw the point
//
PointF p = WorldToWindow(new PointF((float)pt.X, (float)pt.Y));
WorldToWindowMatrix.TransformPoints(triangle);
g.FillPolygon(normbrush, triangle);
if (m_selected == pt)
{
g.DrawPolygon(selpen, triangle);
}
else
{
g.DrawPolygon(normpen, triangle);
}
//
// Draw the selection box
//
if (m_selected == pt)
{
PointF[] selbox = new PointF[]
{
new PointF((float)m_file.Robot.Length / 2.0f, (float)m_file.Robot.Width / 2.0f),
new PointF(-(float)m_file.Robot.Length / 2.0f, (float)m_file.Robot.Width / 2.0f),
new PointF(-(float)m_file.Robot.Length / 2.0f, -(float)m_file.Robot.Width / 2.0f),
new PointF((float)m_file.Robot.Length / 2.0f, -(float)m_file.Robot.Width / 2.0f),
};
PointF[] rotateline = new PointF[]
{
new PointF(0f, (float)m_file.Robot.Width / 2.0f),
new PointF(0f, 3.0f * (float)m_file.Robot.Width / 4.0f),
};
PointF[] circle = new PointF[]
{
new PointF(0f, 3.0f * (float)m_file.Robot.Width / 4.0f),
new PointF(0f, (float)m_file.Robot.Width / 4.0f),
};
mm.TransformPoints(selbox);
WorldToWindowMatrix.TransformPoints(selbox);
g.DrawPolygon(selpen, selbox);
mm.TransformPoints(rotateline);
WorldToWindowMatrix.TransformPoints(rotateline);
g.DrawLine(selpen, rotateline[0], rotateline[1]);
mm.TransformPoints(circle);
WorldToWindowMatrix.TransformPoints(circle);
RectangleF rectf = new RectangleF(circle[0], new SizeF(0, 0));
rectf.Inflate(5.0f, 5.0f);
g.FillEllipse(selbrush, rectf);
}
}
private void DrawPathRobot(Graphics g, RobotPath path)
{
if (path.AdditionalSegments == null)
{
return;
}
PointF[] pts = null;
Dictionary <string, PathSegment[]> segs = path.AdditionalSegments;
if (segs.Count == 2)
{
XeroPose left, right;
left = path.GetPositionForTime(segs["left"], m_time);
right = path.GetPositionForTime(segs["right"], m_time);
XeroPose center = left.Interpolate(right, 0.5);
pts = new PointF[]
{
new PointF((float)Robot.Length / 2.0f, -(float)Robot.Width / 2.0f),
new PointF(-(float)Robot.Length / 2.0f, -(float)Robot.Width / 2.0f),
new PointF(-(float)Robot.Length / 2.0f, (float)Robot.Width / 2.0f),
new PointF((float)Robot.Length / 2.0f, (float)Robot.Width / 2.0f),
};
Matrix mm = new Matrix();
mm.Translate((float)center.X, (float)center.Y);
mm.Rotate((float)center.HeadingDegrees);
mm.TransformPoints(pts);
WorldToWindowMatrix.TransformPoints(pts);
}
else
{
XeroPose fl, fr, bl, br;
fl = path.GetPositionForTime(segs["fl"], m_time);
fr = path.GetPositionForTime(segs["fr"], m_time);
bl = path.GetPositionForTime(segs["bl"], m_time);
br = path.GetPositionForTime(segs["br"], m_time);
pts = new PointF[4]
{
new PointF((float)fr.X, (float)fr.Y),
new PointF((float)br.X, (float)br.Y),
new PointF((float)bl.X, (float)bl.Y),
new PointF((float)fl.X, (float)fl.Y),
};
WorldToWindowMatrix.TransformPoints(pts);
}
using (Pen p = new Pen(Color.Yellow, 3.0f))
{
g.DrawLines(p, pts);
}
using (Pen p = new Pen(Color.Green, 3.0f))
{
g.DrawLine(p, pts[3], pts[0]);
}
}
