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