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();
}
public void Draw(Renderer renderer)
{
if (grid == null)
{
return;
}
else if (newRenderingMethod)
{
Gl.glBegin(Gl.GL_QUADS);
float stepX = (float)grid.ResolutionX * sample;
float stepY = (float)grid.ResolutionY * sample;
float zOffset = -0f;
if (grid != null)
{
for (double x = -grid.ExtentX; x < grid.ExtentX; x += stepX)
{
for (double y = -grid.ExtentY; y < grid.ExtentY; y += stepY)
{
// Get The (X, Y, Z) Value For The Bottom Left Vertex
float xVert = (float)x;
float yVert = (float)y;
float zVert = (float)grid.GetCell(x, y);
GLUtility.SetGLColor(GLUtility.FalseColor((float)zVert, lowHeight, highHeight), transparency);
Gl.glVertex3f(xVert, yVert, zVert + zOffset);
// Get The (X, Y, Z) Value For The Top Left Vertex
xVert = (float)x;
yVert = (float)(y + stepY);
zVert = (float)grid.GetCell(xVert, yVert);
GLUtility.SetGLColor(GLUtility.FalseColor((float)zVert, lowHeight, highHeight), transparency);
Gl.glVertex3f(xVert, yVert, zVert + zOffset);
// Get The (X, Y, Z) Value For The Top Right Vertex
xVert = (float)(x + stepX);
yVert = (float)(y + stepY);
zVert = (float)grid.GetCell(xVert, yVert);
GLUtility.SetGLColor(GLUtility.FalseColor((float)zVert, lowHeight, highHeight), transparency);
Gl.glVertex3f(xVert, yVert, zVert + zOffset);
// Get The (X, Y, Z) Value For The Bottom Right Vertex
xVert = (float)(x + stepX);
yVert = (float)y;
zVert = (float)grid.GetCell(xVert, yVert);
GLUtility.SetGLColor(GLUtility.FalseColor((float)zVert, lowHeight, highHeight), transparency);
Gl.glVertex3f(xVert, yVert, zVert + zOffset);
}
}
}
Gl.glEnd();
}
else
{
GLUtility.DisableNiceLines();
for (double x = -grid.ExtentX; x < grid.ExtentX; x += grid.ResolutionX)
{
for (double y = -grid.ExtentY; y < grid.ExtentY; y += grid.ResolutionY)
{
RectangleF r = new RectangleF((float)(x), (float)(y + grid.ResolutionY), (float)grid.ResolutionX, -(float)grid.ResolutionY);
Color color;
if (logOdds)
{
color = GLUtility.FalseColor((float)grid.GetCellReal(x, y), lowHeight, highHeight);
}
else
{
if (Double.IsNaN(grid.GetCell(x, y)))
{
color = Color.Black;
}
else
{
color = GLUtility.FalseColor((float)grid.GetCell(x, y), lowHeight, highHeight);
}
}
//GLUtility.FillRectangle(color, r);
GLUtility.FillRectangle(color, r, transparency);
}
}
GLUtility.EnableNiceLines();
}
}
