public void Draw(Renderer cam)
{
if (points == null)
{
return;
}
List <Vector2> copy = new List <Vector2>(points);
lock (this.drawLock)
{
foreach (Vector2 v in copy)
{
GLUtility.DrawCross(new GLPen(color, 1.0f), v, size);
}
}
}
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();
}
}
}
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 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>
/// 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);
}
}
}
