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