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