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 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 cam)
{
//Console.WriteLine("trying to draw notepoint");
PointF npCenter = new PointF((float)x, (float)y);
GLUtility.DrawCircle(new GLPen(color, 0.25f), npCenter, .1f);
GLUtility.DrawCircle(new GLPen(color, 0.25f), npCenter, .25f);
GLUtility.DrawString(comments, Color.Black, npCenter);
if (name != null)
{
GLUtility.DrawString(name, Color.Black, npCenter);
}
}
/// <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 cam)
{
GLUtility.DrawEllipse(new GLPen(Color.Red, 1.0f), new RectangleF((float)(currentPoint.X - .1), (float)(currentPoint.Y - .1), .2f, .2f));
GLUtility.DrawString(currentPoint.X.ToString("F2") + "," + currentPoint.Y.ToString("F2"), Color.Black, currentPoint.ToPointF());
//if (poiList.Count > 0)
//{
// foreach (NotepointRenderer np in poiList)
// {
// PointF newPoint = new PointF((float)np.X,(float)np.Y);
// if ((shouldMove == true) && np.Equals(pointToMove))
// {
// GLUtility.DrawCircle(new GLPen(Color.Red, 0.25f), newPoint, 0.25f);
// }
// else
// {
// GLUtility.DrawCircle(new GLPen(np.Color, 0.25f), newPoint, 0.25f);
// }
// }
//}
}
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();
}
