public void UpdateObstacles(List <Polygon> obstacles, double r, out List <Polygon> bloated)
{
if (pose == null)
{
bloated = default(List <Polygon>);
return;
}
lock (bloatedObstacleLock)
{
bloatedObstacles = new List <Polygon>();
foreach (Polygon p in obstacles)
{
bloatedObstacles.Add(Polygon.ConvexMinkowskiConvolution(Polygon.VehiclePolygonWithRadius(r), Polygon.GrahamScan(p.points, 1e-3)));
}
}
bloated = bloatedObstacles;
// Following is deprecated for new height map planning
Bitmap bmp = new Bitmap((int)Math.Round(2 * extentX / resX), (int)Math.Round(2 * extentY / resY));
Graphics canvas = Graphics.FromImage(bmp);
Pen pen = new Pen(Color.Blue, 1);
PointF[] points;
lock (bloatedObstacleLock)
{
foreach (Polygon p in bloatedObstacles)
{
points = new PointF[p.points.Count];
for (int i = 0; i < p.points.Count; i++)
{
points[i] = new PointF((float)(p.points.ElementAt(i).X / resX + extentX / resX),
(float)(p.points.ElementAt(i).Y / resY + extentY / resY));
}
canvas.DrawPolygon(pen, points);
}
}
bmp = Blur(bmp, pose, (int)(10 / resX) * 2);
bmp = Blur(bmp, pose, (int)(10 / resX) * 2);
//bmp = Blur(bmp, pose, (int)(10 / resX) * 2);
//bmp = Blur(bmp, pose, (int)(10 / resX) * 2);
//bmp = Blur(bmp);
//bmp = Blur(bmp);
lock (ogLock)
{
og = new OccupancyGrid2D(resX, resY, extentX, extentY);
og.SetCellsFast(bmp);//, pose, og, 10);
}
}