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