public void Draw(Renderer cam)
{
for (int i = 0; i < pt1List.Count; i++)
{
GLUtility.DrawLine(new GLPen(this.color, width), pt1List[i], pt2List[i]);
}
}
public void DrawSimulationResults(ITreeNode n, GLPen p)
{
for (int i = 0; i < n.SimulationPoints.Count - 1; i++)
{
GLUtility.DrawLine(p, n.SimulationPoints[i], n.SimulationPoints[i + 1]);
}
}
public void Draw(Renderer cam)
{
Vector2 bodyPnt = new Vector2(pose.x, pose.y);
Vector2 bodyHeading = new Vector2(bodyPnt.X + Math.Cos(pose.yaw), bodyPnt.Y + Math.Sin(pose.yaw));
GLUtility.DrawLineLoop(new GLPen(color, drawLineWidth), bodyPlygn.ToArray());
// Draw heading
GLUtility.DrawLine(new GLPen(Color.Red, drawLineWidth), bodyPnt, bodyHeading);
// Draw the name
GLUtility.DrawString(name, Color.Black, new PointF((float)bodyPnt.X, (float)bodyPnt.Y));
}
public void Draw(Renderer cam)
{
Vector2 lastPoint = currentPoints.Count > 0 ? currentPoints.ElementAt <Vector2>(0) : new Vector2();
foreach (Vector2 p in currentPoints)
{
GLUtility.DrawCircle(new GLPen(Color.Chartreuse, 1.0f), new PointF((float)p.X, (float)p.Y), 0.25f);
if (!p.Equals(currentPoints.ElementAt <Vector2>(0)))
{
GLUtility.DrawLine(new GLPen(Color.Chartreuse, 1.0f), lastPoint, p);
lastPoint = p;
}
}
}
public void Draw(Renderer r)
{
float lineWidth = selected ? 2.0f : 1.0f;
PointF bodyPnt = new PointF(x, y);
PointF bodyHeading = new PointF(bodyPnt.X + (float)Math.Cos(heading), bodyPnt.Y + (float)Math.Sin(heading));
GLUtility.DrawLineLoop(new GLPen(color, lineWidth), bodyPlygn.ToArray());
GLUtility.FillTriangle(color, 0.6f, bodyPlygn[0].ToPointF(), bodyPlygn[1].ToPointF(), bodyPlygn[2].ToPointF());
GLUtility.FillTriangle(color, 0.6f, bodyPlygn[2].ToPointF(), bodyPlygn[3].ToPointF(), bodyPlygn[0].ToPointF());
// Draw heading
//GLUtility.DrawLine(new GLPen(Color.Red, lineWidth), Vector2.FromPointF(bodyPnt), Vector2.FromPointF(bodyHeading));
GLUtility.DrawLine(new GLPen(Color.Red, lineWidth), bodyPnt, bodyHeading);
//// Draw the name
//if (modeString.Equals(""))
// GLUtility.DrawString(MythName + ": " + name, Color.Black, bodyPnt);
//else if (modeQualifier.Equals(""))
// GLUtility.DrawString(name + ": " + modeString, Color.Black, bodyPnt);
//else// if (defaultRenderer != null)
//{
// try
// {
// GLUtility.DrawStringMultiLine(name + ": " + modeString + '\n' + "NOTE: " + modeQualifier, Color.Black, bodyPnt, r.CurrentCamera);
// }
// catch { GLUtility.DrawString(name + ": " + modeString, Color.Black, bodyPnt); }
//}//else
//// GLUtility.DrawString(name + ": " + modeString + "(NOTE: " + modeQualifier + ")", Color.Black, bodyPnt);
GLUtility.DrawString(name, Color.Black, bodyPnt);
DrawFlagLine(x, y, color);
if (drawCameraView && IsSelected)
{
PointF p1 = bodyPnt;
PointF p2 = new PointF(bodyPnt.X + 5 * (float)Math.Cos(heading - 0.5 * (cameraFOVangle * Math.PI / 180)), bodyPnt.Y + 5 * (float)Math.Sin(heading - 0.5 * (cameraFOVangle * Math.PI / 180)));
PointF p3 = new PointF(bodyPnt.X + 5 * (float)Math.Cos(heading - 0.167 * (cameraFOVangle * Math.PI / 180)), bodyPnt.Y + 5 * (float)Math.Sin(heading - 0.167 * (cameraFOVangle * Math.PI / 180)));
PointF p4 = new PointF(bodyPnt.X + 5 * (float)Math.Cos(heading + 0.167 * (cameraFOVangle * Math.PI / 180)), bodyPnt.Y + 5 * (float)Math.Sin(heading + 0.167 * (cameraFOVangle * Math.PI / 180)));
PointF p5 = new PointF(bodyPnt.X + 5 * (float)Math.Cos(heading + 0.5 * (cameraFOVangle * Math.PI / 180)), bodyPnt.Y + 5 * (float)Math.Sin(heading + 0.5 * (cameraFOVangle * Math.PI / 180)));
GLUtility.FillTriangle(color, 0.1f, p1, p2, p3);
GLUtility.FillTriangle(color, 0.1f, p1, p3, p4);
GLUtility.FillTriangle(color, 0.1f, p1, p4, p5);
//GLUtility.FillTriangle(Color.BlueViolet, 0.3f, p1, p2, p3);
//GLUtility.FillTriangle(Color.Blue, 0.3f, p1, p3, p4);
//GLUtility.FillTriangle(Color.Turquoise, 0.3f, p1, p4, p5);
}
}
public void Draw(Renderer r)
{
if (currentPath == null)
{
return;
}
if (renderer != null)
{
//if (renderer.GetBoundingPolygon().Count > 0)
// Why do we need this?
//GLUtility.DrawLine(new GLPen(color, 1.0f), renderer.GetBoundingPolygon().Center.ToPointF(), currentPath[0].Start.ToPointF());
}
if (draw == true)
{
foreach (IPathSegment seg in currentPath)
{
if (seg is BezierPathSegment)
{
CubicBezier bezier = ((BezierPathSegment)seg).Bezier;
GLUtility.DrawCircle(new GLPen(color, 1.0f), seg.Start.ToPointF(), 0.25f);
GLUtility.DrawCircle(new GLPen(color, 1.0f), seg.End.ToPointF(), 0.25f);
GLUtility.DrawBezier(new GLPen(color, 1.0f), bezier.P0, bezier.P1, bezier.P2, bezier.P3);
GLUtility.DrawCircle(new GLPen(Color.Pink, 1.0f), bezier.P0.ToPointF(), nodeWidth);
GLUtility.DrawCircle(new GLPen(Color.Pink, 1.0f), bezier.P1.ToPointF(), nodeWidth);
GLUtility.DrawCircle(new GLPen(Color.Pink, 1.0f), bezier.P2.ToPointF(), nodeWidth);
GLUtility.DrawCircle(new GLPen(Color.Pink, 1.0f), bezier.P3.ToPointF(), nodeWidth);
}
else
{
GLUtility.DrawCircle(new GLPen(color, 1.0f), seg.Start.ToPointF(), nodeWidth);
GLUtility.DrawCircle(new GLPen(color, 1.0f), seg.End.ToPointF(), nodeWidth);
GLUtility.DrawLine(new GLPen(color, 1.0f), (float)seg.Start.X, (float)seg.Start.Y, (float)seg.End.X, (float)seg.End.Y);
}
}
}
if (currentPath.Count != 0 && renderer != null)
{
IPathSegment ips = currentPath.ElementAt <IPathSegment>(currentPath.Count / 2);
/*GLUtility.DrawString(renderer.GetName(), Color.Black, new PointF((ips.End.ToPointF().X + ips.Start.ToPointF().X) / 2,
* (ips.End.ToPointF().Y + ips.Start.ToPointF().Y) / 2));//*/
}
}
/// <summary>
/// This is called whenever the renderer is going to draw stuff. Essentially this is where you put your
/// tool drawing code.
/// </summary>
/// <param name="r"></param>
public void Draw(Renderer r)
{
#region ruler drawing code
if (shouldDraw && isActive)
{
PointF p1 = r.ScreenToWorld(downPoint);
PointF p2 = r.ScreenToWorld(movePoint);
GLPen pen = new GLPen(Color.Purple, 1.0f);
GLUtility.DrawLine(pen, p1, p2);
GLUtility.DrawEllipse(pen, new RectangleF(p1.X - .1f, p1.Y - .1f, .2f, .2f));
GLUtility.DrawEllipse(pen, new RectangleF(p2.X - .1f, p2.Y - .1f, .2f, .2f));
distance = Math.Sqrt(((p1.X - p2.X) * (p1.X - p2.X)) + ((p1.Y - p2.Y) * (p1.Y - p2.Y)));
PointF m = new PointF((p1.X + p2.X) / 2.0f + .3f, (p1.Y + p2.Y) / 2.0f + .3f);
GLUtility.DrawString(distance.ToString("F2") + "m", Color.Black, m);
}
#endregion
}
public void Draw(Renderer r)
{
#region angle drawing code
if (IsActive)
{
if (angleState == 1)
{
PointF p1 = r.ScreenToWorld(clickPoint);
PointF p2 = r.ScreenToWorld(movePoint);
GLPen pen = new GLPen(Color.Purple, 1.0f);
GLUtility.DrawLine(pen, Vector2.FromPointF(p1), Vector2.FromPointF(p2));
GLUtility.DrawEllipse(pen, new RectangleF(p1.X - .1f, p1.Y - .1f, .2f, .2f));
GLUtility.DrawEllipse(pen, new RectangleF(p2.X - .1f, p2.Y - .1f, .2f, .2f));
dist12 = Math.Sqrt(((p1.X - p2.X) * (p1.X - p2.X)) + ((p1.Y - p2.Y) * (p1.Y - p2.Y)));
PointF m12 = new PointF((p1.X + p2.X) / 2.0f, (p1.Y + p2.Y) / 2.0f);
GLUtility.DrawString(dist12.ToString("F2") + "m", Color.Black, m12);
}
if (angleState == 2)
{
PointF p1 = r.ScreenToWorld(clickPoint);
PointF p2 = r.ScreenToWorld(clickPoint2);
PointF p3 = r.ScreenToWorld(movePoint);
GLPen pen = new GLPen(Color.Purple, 1.0f);
GLUtility.DrawLine(pen, p1, p2);
GLUtility.DrawLine(pen, p2, p3);
GLUtility.DrawEllipse(pen, new RectangleF(p1.X - .1f, p1.Y - .1f, .2f, .2f));
GLUtility.DrawEllipse(pen, new RectangleF(p2.X - .1f, p2.Y - .1f, .2f, .2f));
GLUtility.DrawEllipse(pen, new RectangleF(p3.X - .1f, p3.Y - .1f, .2f, .2f));
dist12 = Math.Sqrt(((p1.X - p2.X) * (p1.X - p2.X)) + ((p1.Y - p2.Y) * (p1.Y - p2.Y)));
PointF m12 = new PointF((p1.X + p2.X) / 2.0f, (p1.Y + p2.Y) / 2.0f);
GLUtility.DrawString(dist12.ToString("F2") + "m", Color.Black, m12);
dist23 = Math.Sqrt(((p2.X - p3.X) * (p2.X - p3.X)) + ((p2.Y - p3.Y) * (p2.Y - p3.Y)));
PointF m23 = new PointF((p2.X + p3.X) / 2.0f, (p2.Y + p3.Y) / 2.0f);
GLUtility.DrawString(dist23.ToString("F2") + "m", Color.Black, m23);
dist13 = Math.Sqrt(((p1.X - p3.X) * (p1.X - p3.X)) + ((p1.Y - p3.Y) * (p1.Y - p3.Y)));
cosangle = (dist12 * dist12 + dist23 * dist23 - dist13 * dist13) / (2 * dist12 * dist23);
angleDEG = Math.Acos(cosangle) * (180.0 / Math.PI);
p2.X += .5f; p2.Y += .5f;
GLUtility.DrawString(angleDEG.ToString("F2") + " deg", Color.Black, p2);
}
}
#endregion
}
public void Draw(Renderer r)
{
if (wallPoints.Count > 0)
{
for (int i = 0; i < wallPoints.Count; i++)
{
List <PointF> wallSegment = wallPoints[i];
for (int j = 0; j < wallSegment.Count - 1; j++)
{
GLUtility.DrawLine(new GLPen(color, 5), wallSegment[j], wallSegment[j + 1]);
}
}
}
if (homeBase.Count > 0)
{
GLUtility.DrawLine(new GLPen(color, 1), homeBase[0], homeBase[1]);
GLUtility.DrawLine(new GLPen(color, 1), homeBase[1], homeBase[2]);
GLUtility.DrawLine(new GLPen(color, 1), homeBase[2], homeBase[3]);
GLUtility.DrawString("HOME BASE", Color.DarkSlateGray, new PointF(7.6f, -2.4f));
GLUtility.DrawString("HOME BASE", Color.DarkSlateGray, new PointF(8.7f, -0.6f));
}
}
public void Draw(Renderer r)
{
if (type.Equals("circle string"))
{
GLUtility.DrawString("CIRCLE", color, location);
}
else if (type.Equals("path start string"))
{
GLUtility.DrawString("PATH START", color, location);
}
else if (type.Equals("path end string"))
{
GLUtility.DrawString("PATH END", color, location);
}
else if (type.Equals("box string"))
{
GLUtility.DrawString("SQUARE", color, location);
}
else if (type.Equals("x string"))
{
GLUtility.DrawString("X", color, location);
}
else if (type.Equals("arrow string"))
{
PointF arrowHead = new PointF((float)(location.X + arrowlength * Math.Cos(angle)), (float)(location.Y + arrowlength * Math.Sin(angle)));
GLUtility.DrawString("ARROW TAIL", color, location);
GLUtility.DrawString("ARROW HEAD", color, arrowHead);
}
else if (type.Equals("important string"))
{
GLUtility.DrawString("EXCLAMATION MARK", color, location);
}
else if (type.Equals("spiral string"))
{
GLUtility.DrawString("SPIRAL", color, location);
}
else if (type.Equals("polygon string"))
{
GLUtility.DrawString("SHADED AREA", color, location);
}
else if (type.Equals("triangle string"))
{
GLUtility.DrawString("TRIANGLE", color, location);
}
else if (type.Equals("circle"))
{
PointF upperLeft = new PointF((float)(location.X - 0.25), (float)(location.Y - .25));
RectangleF rect = new RectangleF(upperLeft, new SizeF(.5f, .5f));
GLUtility.FillEllipse(color, rect);
}
else if (type.Equals("box"))
{
PointF upperLeft = new PointF((float)(location.X - 0.25), (float)(location.Y - .25));
RectangleF rect = new RectangleF(upperLeft, new SizeF(.5f, .5f));
GLUtility.FillRectangle(color, rect);
}
else if (type.Equals("triangle"))
{
PointF p1 = new PointF((float)(location.X - 0.25), (float)(location.Y - 0.25));
PointF p2 = new PointF((float)(location.X + 0.25), (float)(location.Y - 0.25));
PointF p3 = new PointF((float)(location.X), (float)(location.Y + 0.25));
GLUtility.FillTriangle(color, p1, p2, p3);
}
else if (type.Equals("spiral"))
{
double size = 0.8;
GLPen spiralPen = new GLPen(color, 3f);
GLUtility.DrawCircle(spiralPen, location, (float)(size / 2));
GLUtility.DrawCircle(spiralPen, location, (float)(size * 3 / 8));
GLUtility.DrawCircle(spiralPen, location, (float)(size / 4));
GLUtility.DrawCircle(spiralPen, location, (float)(size * 1 / 8));
}
else if (type.Equals("important"))
{
GLUtility.DrawLine(new GLPen(color, 3f), new Vector2(location.X, location.Y + 0.15), new Vector2(location.X, location.Y - .05));
//GLUtility.DrawCross(new GLPen(color, 3f), new Vector2(location.X, location.Y - .12), .1f);
GLUtility.DrawCircle(new GLPen(color, 2f), new Vector2(location.X, location.Y - .12), .02f);
GLUtility.DrawDiamond(new GLPen(color, 3f), new Vector2((double)location.X, (double)location.Y), .6f);
GLUtility.DrawDiamond(new GLPen(Color.Red, 3f), new Vector2((double)location.X, (double)location.Y), .8f);
}
else if (type.Equals("x"))
{
GLUtility.DrawLine(new GLPen(color, 3f), new Vector2((double)location.X - .2, (double)location.Y + .2), new Vector2((double)location.X + .2, (double)location.Y - .2));
GLUtility.DrawLine(new GLPen(color, 3f), new Vector2((double)location.X + .2, (double)location.Y + .2), new Vector2((double)location.X - .2, (double)location.Y - .2));
}
else if (type.Equals("empty circle"))
{
PointF upperLeft = new PointF((float)(location.X - 0.25), (float)(location.Y - .25));
RectangleF rect = new RectangleF(upperLeft, new SizeF(.5f, .5f));
GLUtility.DrawEllipse(new GLPen(color, 3f), rect);
}
else if (type.Equals("arrow"))
{
Vector2 arrowHead = new Vector2(location.X + 0.7 * Math.Cos(angle), location.Y + 0.7 * Math.Sin(angle));
PointF upperLeft = new PointF((float)(location.X - 0.1), (float)(location.Y - .1));
RectangleF rect = new RectangleF(upperLeft, new SizeF(.2f, .2f));
GLUtility.FillEllipse(color, rect);
GLUtility.DrawLine(new GLPen(color, 3f), new Vector2(location.X, location.Y), arrowHead);
}
//else if(type.Equals("triangle"))//man
//{
// //head
// PointF upperLeftHead = new PointF((float)(location.X - 0.25), (float)(location.Y + .25));
// RectangleF rect = new RectangleF(upperLeftHead, new SizeF(.5f, .5f));
// GLUtility.DrawEllipse(new GLPen(color, 3f), rect);
// //body
// GLUtility.DrawLine(new GLPen(color, 3f), new Vector2(location.X, location.Y-.25), new Vector2(location.X,location.Y+.25));
// //arms
// GLUtility.DrawLine(new GLPen(color, 3f), new Vector2(location.X, location.Y), new Vector2(location.X+.3,location.Y));
// GLUtility.DrawLine(new GLPen(color, 3f), new Vector2(location.X, location.Y), new Vector2(location.X-.3,location.Y));
// //legs
// GLUtility.DrawLine(new GLPen(color, 3f), new Vector2(location.X, location.Y-.25), new Vector2(location.X+.25,location.Y-.5));
// GLUtility.DrawLine(new GLPen(color, 3f), new Vector2(location.X, location.Y-.25), new Vector2(location.X-.25,location.Y-.5));
//}
else if (type.Equals("polygon"))
{
double maxx = -1.0 / zero;
double maxy = -1.0 / zero;
double minx = 1.0 / zero;
double miny = 1.0 / zero;
if (polygonPoints.Count == 0)
{
polygonPoints.Add(new PointF((float)(location.X - 1), (float)(location.Y + 1)));
polygonPoints.Add(new PointF((float)(location.X - 1), (float)(location.Y - 1)));
polygonPoints.Add(new PointF((float)(location.X + 2), (float)(location.Y - 2)));
polygonPoints.Add(new PointF((float)(location.X + 3), (float)(location.Y)));
polygonPoints.Add(new PointF((float)(location.X + 1), (float)(location.Y + 2)));
polygonPoints.Add(new PointF((float)(location.X - 1), (float)(location.Y + 1)));
}
for (int i = 0; i < polygonPoints.Count - 1; i++)
{
if (polygonPoints[i].X > maxx)
{
maxx = polygonPoints[i].X;
}
if (polygonPoints[i].Y > maxy)
{
maxy = polygonPoints[i].Y;
}
if (polygonPoints[i].X < minx)
{
minx = polygonPoints[i].X;
}
if (polygonPoints[i].Y < miny)
{
miny = polygonPoints[i].Y;
}
this.location = new PointF((float)(0.5 * (maxx + minx)), (float)(0.5 * (maxy + miny)));
GLUtility.DrawLine(new GLPen(color, 3), polygonPoints[i], polygonPoints[i + 1]);
}
}
else if (type.Equals("fill polygon"))
{
double maxx = -1.0 / zero;
double maxy = -1.0 / zero;
double minx = 1.0 / zero;
double miny = 1.0 / zero;
if (polygonPoints.Count == 0)
{
polygonPoints.Add(new PointF((float)(location.X - 1), (float)(location.Y + 1)));
polygonPoints.Add(new PointF((float)(location.X - 1), (float)(location.Y - 1)));
polygonPoints.Add(new PointF((float)(location.X + 2), (float)(location.Y - 2)));
polygonPoints.Add(new PointF((float)(location.X + 3), (float)(location.Y)));
polygonPoints.Add(new PointF((float)(location.X + 1), (float)(location.Y + 2)));
polygonPoints.Add(new PointF((float)(location.X - 1), (float)(location.Y + 1)));
}
for (int i = 0; i < polygonPoints.Count - 1; i++)
{
if (polygonPoints[i].X > maxx)
{
maxx = polygonPoints[i].X;
}
if (polygonPoints[i].Y > maxy)
{
maxy = polygonPoints[i].Y;
}
if (polygonPoints[i].X < minx)
{
minx = polygonPoints[i].X;
}
if (polygonPoints[i].Y < miny)
{
miny = polygonPoints[i].Y;
}
this.location = new PointF((float)(0.5 * (maxx + minx)), (float)(0.5 * (maxy + miny)));
GLUtility.DrawLine(new GLPen(color, 3), polygonPoints[i], polygonPoints[i + 1]);
}
for (int i = 0; i < polygonPoints.Count - 1; i++)
{
GLUtility.FillTriangle(color, 0.3f, polygonPoints[i], polygonPoints[i + 1], new PointF((float)(0.5 * (maxx + minx)), (float)(0.5 * (maxy + miny))));
}
}
else
{
}
//GLUtility.DrawString(name,Color.Black, location);
}
public void Draw(Renderer r)
{
if (options.Show == false)
{
return;
}
if (scan == null)
{
return; //nothing to do
}
GLUtility.DisableNiceLines();
GLUtility.GoToSensorPose(laserToBody);
lock (this.drawLock)
{
//foreach (ILidar2DPoint sp in scan.Points)
for (int i = startIdx; i <= endIdx; i++)
{
PointF p = PointF.Empty;
ILidar2DPoint sp = scan.Points[i];
if (options.IsUpsideDown)
{
p = new PointF((float)(sp.RThetaPoint.R * Math.Cos(-sp.RThetaPoint.theta + robotPose.yaw) + robotPose.x), (float)(sp.RThetaPoint.R * Math.Sin(-sp.RThetaPoint.theta + robotPose.yaw) + robotPose.y));
}
else
{
p = new PointF((float)(sp.RThetaPoint.R * Math.Cos(sp.RThetaPoint.theta + robotPose.yaw) + robotPose.x), (float)(sp.RThetaPoint.R * Math.Sin(sp.RThetaPoint.theta + robotPose.yaw) + robotPose.y));
}
GLUtility.DrawCross(new GLPen(options.RenderColor, 1), Vector2.FromPointF(p), .1f);
if (options.ShowIndex)
{
GLUtility.DrawString("Index: " + i, Color.Black, p);
}
}
}
if (options.ShowTS)
{
GLUtility.DrawString("Time: " + scan.Timestamp.ToString(), Color.Black, new PointF(0, 0));
}
if (options.ShowOrigin)
{
GLUtility.DrawCircle(new GLPen(Color.Red, 1), new PointF(0, 0), .075f);
}
if (options.ShowBoundary)
{
PointF p = PointF.Empty, p2 = PointF.Empty;
PointF pose = PointF.Empty;
ILidar2DPoint spInitial = scan.Points[0];
ILidar2DPoint spEnd = scan.Points[scan.Points.Count - 1];
if (options.IsUpsideDown)
{
p = new PointF((float)(spInitial.RThetaPoint.R * Math.Cos(-spInitial.RThetaPoint.theta + robotPose.yaw) + robotPose.x), (float)(spInitial.RThetaPoint.R * Math.Sin(-spInitial.RThetaPoint.theta + robotPose.yaw) + robotPose.y));
p2 = new PointF((float)(spEnd.RThetaPoint.R * Math.Cos(-spEnd.RThetaPoint.theta + robotPose.yaw) + robotPose.x), (float)(spEnd.RThetaPoint.R * Math.Sin(-spEnd.RThetaPoint.theta + robotPose.yaw) + robotPose.y));
}
else
{
p = new PointF((float)(spInitial.RThetaPoint.R * Math.Cos(spInitial.RThetaPoint.theta + robotPose.yaw) + robotPose.x), (float)(spInitial.RThetaPoint.R * Math.Sin(spInitial.RThetaPoint.theta + robotPose.yaw) + robotPose.y));
p2 = new PointF((float)(spEnd.RThetaPoint.R * Math.Cos(spEnd.RThetaPoint.theta + robotPose.yaw) + robotPose.x), (float)(spEnd.RThetaPoint.R * Math.Sin(spEnd.RThetaPoint.theta + robotPose.yaw) + robotPose.y));
}
pose.X = (float)robotPose.x; pose.Y = (float)robotPose.y;
GLUtility.DrawLine(new GLPen(options.BoundaryColor, options.BoundaryWidth), p, pose);
GLUtility.DrawLine(new GLPen(options.BoundaryColor, options.BoundaryWidth), p2, pose);
}
GLUtility.ComeBackFromSensorPose();
GLUtility.EnableNiceLines();
}
/// <summary>
/// Draw the waypoints & path (circles & lines)
/// </summary>
/// <param name="r"></param>
#region path drawing code
public void Draw(Renderer r)
{
//if only one waypoint, draw the waypoint
if (newPath.WaypointList.Count == 1 && shouldDraw)
{
WaypointRenderer wpFirst = newPath.WaypointList[0];
PointF drawPoint = new PointF((float)wpFirst.X, (float)wpFirst.Y);
GLUtility.DrawCircle(new GLPen(wpFirst.Color, 1.0f), drawPoint, 0.2f);
if (!shouldMove && tempActive)
{
GLUtility.DrawLine(new GLPen(Color.LawnGreen, 1.0f), drawPoint, r.ScreenToWorld(mousePoint));
}
}
// if more than one waypoint, draw waypoints and connect subsequent waypoints with a line
else if (newPath.WaypointList.Count > 1 && shouldDraw)
{
WaypointRenderer wpLast = newPath.WaypointList.First();
foreach (WaypointRenderer wp in newPath.WaypointList)
{
PointF pathPoint = new PointF((float)wp.X, (float)wp.Y);
if (shouldMove && wp.Equals(movePointWP))
{
GLUtility.DrawCircle(new GLPen(Color.Red, 1.0f), pathPoint, 0.2f);
}
else
{
GLUtility.DrawCircle(new GLPen(wp.Color, 1.0f), pathPoint, 0.2f);
}
if (!wp.Equals(newPath.WaypointList.First()))
{
PointF dummyLast = new PointF((float)wpLast.X, (float)wpLast.Y);
GLUtility.DrawLine(new GLPen(newPath.Color, 1.0f), dummyLast, pathPoint);
}
wpLast = wp;
}
// if we are not moving a waypoint, draw a light green line between last waypoint and current mouse location
if (!shouldMove && tempActive)
{
PointF dummyLast = new PointF((float)wpLast.X, (float)wpLast.Y);
GLUtility.DrawLine(new GLPen(Color.LawnGreen, 1.0f), dummyLast, r.ScreenToWorld(mousePoint));
}
}
if (newPath.WaypointList.Count > 0)
{
foreach (ISelectable selectable in r.Selectables)
{
PointF dummyFirst = new PointF();
RobotRenderer robot = selectable as RobotRenderer;
if (robot == null)
{
continue;
}
else if (robot.IsSelected)
{
Polygon p = robot.GetBoundingPolygon();
if (p.points.Count > 0)
{
dummyFirst = new PointF((float)newPath.WaypointList[0].X, (float)newPath.WaypointList[0].Y);
GLUtility.DrawLine(new GLPen(Color.Green, 1.0f), p.Center.ToPointF(), dummyFirst);
}
}
}
}
}
/// <summary>
/// Drawing code
/// </summary>
/// <param name="r"></param>
public void Draw(Renderer r)
{
if (pointList.Count == 0)
{
UpdatePointList();
}
if ((gestureName == "CIRCLE") || (gestureName == "SQUARE") || (gestureName == "TRIANGLE"))
{
//DrawToolOld.ResetRefPoints();
ctrPtWorld = r.ScreenToWorld(pointList.ElementAt(0));
GLUtility.DrawCircle(new GLPen(Color.Blue, 0.2f), ctrPtWorld, 0.3f);
}
else if (gestureName == "X")
{
//DrawToolOld.ResetRefPoints();
ctrPtWorld = r.ScreenToWorld(pointList.ElementAt(0));
GLUtility.DrawCross(new GLPen(Color.Red, 0.2f), new Vector2(ctrPtWorld.X, ctrPtWorld.Y), 0.5f);
}
else if ((gestureName == "ARROW") || (gestureName == "PATH") || (gestureName == "SENTENCE"))
{
//DrawToolOld.ResetRefPoints();
pthStartWorld = r.ScreenToWorld(pointList.ElementAt(0));
pthEndWorld = r.ScreenToWorld(pointList.ElementAt(1));
GLUtility.DrawCircle(new GLPen(Color.Blue, 0.2f), pthStartWorld, 0.3f);
GLUtility.DrawCross(new GLPen(Color.Red, 0.2f), new Vector2(pthEndWorld.X, pthEndWorld.Y), 0.5f);
}
else
{
//DrawToolOld.ResetRefPoints();
}
if ((gestureName == "PATH") || (gestureName == "SENTENCE"))
{
//DrawToolOld.ResetRefPoints();
ln1StartWorld = r.ScreenToWorld(pointList.ElementAt(2));
ln1EndWorld = r.ScreenToWorld(pointList.ElementAt(3));
ln2StartWorld = r.ScreenToWorld(pointList.ElementAt(4));
ln2EndWorld = r.ScreenToWorld(pointList.ElementAt(5));
ln3StartWorld = r.ScreenToWorld(pointList.ElementAt(6));
ln3EndWorld = r.ScreenToWorld(pointList.ElementAt(7));
GLUtility.DrawLine(new GLPen(Color.Black, 1.0f), ln1StartWorld, ln1EndWorld);
GLUtility.DrawLine(new GLPen(Color.Black, 1.0f), ln2StartWorld, ln2EndWorld);
GLUtility.DrawLine(new GLPen(Color.Black, 1.0f), ln3StartWorld, ln3EndWorld);
}
// draw pixels
if (pixelList.Count > 0)
{
foreach (PointF v3 in drawPixelList)
{
drawPixel.X = ((float)v3.X - (w / 2));
drawPixel.Y = ((float)v3.Y - (h / 2));
drawPixel2.X = ((float)v3.X - (w / 4));
drawPixel2.Y = ((float)v3.Y - (h / 4));
float scale = r.Scale();
pixelRect = new RectangleF((float)drawPixel.X, (float)drawPixel.Y, w, h);
RectangleF pixelRect2 = new RectangleF((float)drawPixel2.X, (float)drawPixel2.Y, w / 2, h / 2);
GLUtility.DrawRectangle(new GLPen(Color.Black, 0.1f), pixelRect);
GLUtility.DrawRectangle(new GLPen(Color.Black, 0.1f), pixelRect2);
}
}
}
