public void Draw(Renderer cam)
{
if (poses.Count == 0)
{
return;
}
lock (poses)
{
RobotPose lastPose = poses[0];
foreach (RobotPose p in poses)
{
GLUtility.DrawCross(pen, p.ToVector2(), .2f);
//GLUtility.DrawLine(pen, lastPose.ToVector2(), p.ToVector2());
lastPose = p;
}
if (poses[poses.Count - 1].covariance[0, 0] != 0)
{
//calculate the error ellipse
Matrix cov = new Matrix(2, 2);
cov[0, 0] = poses[poses.Count - 1].covariance[0, 0];
cov[0, 1] = poses[poses.Count - 1].covariance[0, 1];
cov[1, 0] = poses[poses.Count - 1].covariance[1, 0];
cov[1, 1] = poses[poses.Count - 1].covariance[1, 1];
double theta = .5 * Math.Atan2((-2 * cov[0, 1]), (cov[0, 0] - cov[1, 1]));
if (Double.IsNaN(theta))
{
theta = 0;
}
double sigu2 = (cov[0, 0] * Math.Sin(theta) * Math.Sin(theta)) +
(2 * cov[0, 1] * Math.Sin(theta) * Math.Cos(theta)) +
(cov[1, 1] * Math.Cos(theta) * Math.Cos(theta));
double sigv2 = (cov[0, 0] * Math.Cos(theta) * Math.Cos(theta)) +
(2 * cov[0, 1] * Math.Sin(theta) * Math.Cos(theta)) +
(cov[1, 1] * Math.Sin(theta) * Math.Sin(theta));
GLUtility.GoToTransformXYZ((float)poses[poses.Count - 1].x, (float)poses[poses.Count - 1].y, 0);
GLUtility.GoToTransformYPR((float)theta, 0, 0);
GLUtility.DrawEllipse(ellipsePen, new RectangleF((float)-sigu2 / 2, (float)-sigv2 / 2, (float)sigu2, (float)sigv2));
GLUtility.ComeBackFromTransform();
GLUtility.ComeBackFromTransform();
}
}
}
/// <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 (isDrawing == true)
{
double i = 0;
Vector2 prevRP1 = new Vector2();
Vector2 prevRP2 = new Vector2();
Vector2 prevRP3 = new Vector2();
foreach (PDFReferencePointRenderer rp in PDFrenderer.PDFReferencePointList)
{
Vector2 refPoint = new Vector2(rp.X, rp.Y); //third point
GLUtility.DrawCircle(new GLPen(Color.Red, 1.0f), refPoint, 0.25f);
prevRP3 = prevRP2;
prevRP2 = prevRP1; //first point
prevRP1.X = rp.X; //second point
prevRP1.Y = rp.Y;
i++;
if (i % 3 == 0 & i > 0)
{
Vector2 RPfirst = prevRP3;
Vector2 RPsecond = prevRP2;
Vector2 RPthird = prevRP1;
Vector2 midpoint = new Vector2(RPfirst.X + (RPsecond.X - RPfirst.X) / 2, RPfirst.Y + (RPsecond.Y - RPfirst.Y) / 2);
GLUtility.DrawEllipse(new GLPen(Color.Green, 1.0f), midpoint, RPthird, RPsecond);
}
}
mu = new Vector2(firstPDFPoint.X + (secondPDFPoint.X - firstPDFPoint.X) / 2, firstPDFPoint.Y + (secondPDFPoint.Y - firstPDFPoint.Y) / 2);
Vector2 delta = new Vector2(secondPDFPoint.X - firstPDFPoint.X, secondPDFPoint.Y - firstPDFPoint.Y);
double thirdPointAngle = delta.ToRadians();
thirdPDFPoint = new Vector2(mu.X + Math.Sin(thirdPointAngle) * height2, mu.Y - Math.Cos(thirdPointAngle) * height2);
if (isGenerating)
{
GLUtility.DrawCross(new GLPen(Color.Red, 1.0f), firstPDFPoint, 0.3f);
GLUtility.DrawCross(new GLPen(Color.Red, 1.0f), mu, 0.3f);
GLUtility.DrawCross(new GLPen(Color.Red, 1.0f), secondPDFPoint, 0.3f);
GLUtility.DrawCross(new GLPen(Color.Red, 1.0f), thirdPDFPoint, 0.3f);
GLUtility.DrawEllipse(new GLPen(Color.Blue, 1.0f), mu, secondPDFPoint, thirdPDFPoint);
}
}
}
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 (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);
}
