public GVDKarla(ObstacleGrid grid) { this.grid = grid; open = new IntervalHeap <GridCellValue>(); ties = new LinkedList <GridCell>(); dist = new float[grid.NumColumns, grid.NumRows]; distNew = new float[grid.NumColumns, grid.NumRows]; parent = new GridCell[grid.NumColumns, grid.NumRows]; tie = new GridCell[grid.NumColumns, grid.NumRows]; obst = new int[grid.NumColumns, grid.NumRows]; valid = new HashSet <int>(); voro = new bool[grid.NumColumns, grid.NumRows]; for (int c = grid.NumColumns - 1; c >= 0; c--) { for (int r = grid.NumRows - 1; r >= 0; r--) { dist[c, r] = float.PositiveInfinity; distNew[c, r] = float.PositiveInfinity; parent[c, r] = GridCell.Unknown; tie[c, r] = GridCell.Unknown; obst[c, r] = -1; voro[c, r] = false; } } }
public GVDLau(ObstacleGrid grid) { this.grid = grid; // Obstacle distance map dist = new int[grid.NumColumns, grid.NumRows]; distActual = new float[grid.NumColumns, grid.NumRows]; obst = new GridCell[grid.NumColumns, grid.NumRows]; toRaise = new bool[grid.NumColumns, grid.NumRows]; toProcess = new bool[grid.NumColumns, grid.NumRows]; voro = new bool[grid.NumColumns, grid.NumRows]; voroQ = new IntervalHeap <GridCellValue>(); open = new IntervalHeap <GridCellValue>(); comp = new int[grid.NumColumns, grid.NumRows]; // Voronoi distance map voroDist = new int[grid.NumColumns, grid.NumRows]; voroDistActual = new float[grid.NumColumns, grid.NumRows]; nearestVoro = new GridCell[grid.NumColumns, grid.NumRows]; voroToRaise = new bool[grid.NumColumns, grid.NumRows]; voroToProcess = new bool[grid.NumColumns, grid.NumRows]; voroOpen = new IntervalHeap <GridCellValue>(); pathCost = new float[grid.NumColumns, grid.NumRows]; DmaxActual = DMaxDefault / grid.Resolution; AlphaActual = AlphaDefault / grid.Resolution; for (int c = grid.NumColumns - 1; c >= 0; c--) { for (int r = grid.NumRows - 1; r >= 0; r--) { // Obstacle distance map dist[c, r] = int.MaxValue; distActual[c, r] = float.PositiveInfinity; obst[c, r] = GridCell.Unknown; toRaise[c, r] = false; toProcess[c, r] = false; voro[c, r] = true; comp[c, r] = -1; // Voronoi distance map voroDist[c, r] = int.MaxValue; voroDistActual[c, r] = float.PositiveInfinity; nearestVoro[c, r] = GridCell.Unknown; voroToRaise[c, r] = false; voroToProcess[c, r] = false; pathCost[c, r] = 0f; } } }
private static HashSet <int> checkPath(ArrayList <Pose> path, ObstacleGrid grid) { HashSet <int> unsafeIndices = new HashSet <int>(); int count = path.Count; for (int i = 2; i < count - 2; i++) { if (path[i - 2].Gear != path[i - 1].Gear) { continue; } if (path[i - 1].Gear != path[i].Gear) { continue; } if (path[i].Gear != path[i + 1].Gear) { continue; } if (path[i + 1].Gear != path[i + 2].Gear) { continue; } Vector2 currPos = path[i].Position; Vector2 prevPos = path[i - 1].Position; Vector2 displacement = 0.25f * (currPos - prevPos) + 0.75f * (currPos - currPos); float pathOrientation = (float)Math.Atan2(displacement.Y, displacement.X); if (!grid.IsSafe(new Pose(currPos, pathOrientation), 1.1f)) { unsafeIndices.Add(i); unsafeIndices.Add(i - 1); } } return(unsafeIndices); }
public static ArrayList <Pose> Smooth(ArrayList <Pose> path, ObstacleGrid grid, HashSet <int> unsafeIndices) { UnsafeIndices = Anchoring ? unsafeIndices : null; if (path.Count < 5) { ArrayList <Pose> thepath = new ArrayList <Pose>(); thepath.AddAll(path); return(thepath); } int num = currentCount = path.Count; currentPath = new Pose[num]; for (int i = 0; i < num; i++) { currentPath[i] = path[i]; } int count = 0; float change = Tolerance; while (/*change >= Tolerance && */ count < MaxIterations) { float cdmax2 = CollisionDmax * CollisionDmax; change = 0f; for (int i = 2; i < num - 2; i++) { // Keep this point fixed if it is or is adjacent to a cusp point if (currentPath[i - 2].Gear != currentPath[i - 1].Gear) { continue; } if (currentPath[i - 1].Gear != currentPath[i].Gear) { continue; } if (currentPath[i].Gear != currentPath[i + 1].Gear) { continue; } if (currentPath[i + 1].Gear != currentPath[i + 2].Gear) { continue; } // Keep this point fixed if it is unsafe if (Anchoring && unsafeIndices != null && unsafeIndices.Contains(i)) { continue; } Vector2 curr = currentPath[i].Position; Vector2 correction = Vector2.Zero; // Original path term correction += PathWeight * (path[i].Position - curr); if (float.IsNaN(correction.X) || float.IsNaN(correction.Y)) { float noop; } if (grid.IsPointInGrid(curr)) { // Collision term Vector2 closestObstacleVec = curr - grid.CellPositionToPoint(grid.GVD.GetNearestObstacle(grid.PointToCellPosition(curr))); float obstDist = closestObstacleVec.Length(); if (obstDist < CollisionDmax) { correction -= CollisionWeight * (obstDist - CollisionDmax) * closestObstacleVec / obstDist; } if (float.IsNaN(correction.X) || float.IsNaN(correction.Y)) { float noop; } // Voronoi term if (obstDist < VoronoiDmax && VoronoiWeight > 0f) { Vector2 closestVoronoiVec = curr - grid.CellPositionToPoint(grid.GVD.GetNearestVoronoiEdge(grid.PointToCellPosition(curr))); float voroDist = closestVoronoiVec.Length(); if (voroDist > 0f) { float alphaplusdo = grid.GVD.AlphaActual + obstDist; float dominusdmax = obstDist - VoronoiDmax; float doplusdv = obstDist + voroDist; float dmaxsquared = VoronoiDmax * VoronoiDmax; float pvdv = (grid.GVD.AlphaActual / alphaplusdo) * (dominusdmax * dominusdmax / dmaxsquared) * (obstDist / (doplusdv * doplusdv)); float pvdo = (grid.GVD.AlphaActual / alphaplusdo) * (voroDist / doplusdv) * (dominusdmax / dmaxsquared) * (-dominusdmax / alphaplusdo - dominusdmax / doplusdv + 2); correction -= VoronoiWeight * (pvdo * closestObstacleVec / obstDist + pvdv * closestVoronoiVec / voroDist); if (float.IsNaN(correction.X) || float.IsNaN(correction.Y)) { float noop; } } } } // Smoothing term correction -= SmoothWeight * (currentPath[i - 2].Position - 4 * currentPath[i - 1].Position + 6 * curr - 4 * currentPath[i + 1].Position + currentPath[i + 2].Position); if (float.IsNaN(correction.X) || float.IsNaN(correction.Y)) { float noop; } // Curvature term correction -= CurvatureWeight * calcCurvatureTerm(currentPath[i - 1].Position, currentPath[i].Position, currentPath[i + 1].Position); if (float.IsNaN(correction.X) || float.IsNaN(correction.Y)) { calcCurvatureTerm(currentPath[i - 1].Position, currentPath[i].Position, currentPath[i + 1].Position); } correction /= CollisionWeight + SmoothWeight + VoronoiWeight + PathWeight + CurvatureWeight; currentPath[i].Position = curr + Alpha * correction; change += correction.Length(); } count++; if (Delay > 0) { Thread.Sleep(Delay); } } NumIterations = count; Change = change; ArrayList <Pose> thenewpath = new ArrayList <Pose>(); thenewpath.AddAll(currentPath); if (Anchoring && unsafeIndices == null) { HashSet <int> newUnsafes = checkPath(thenewpath, grid); if (newUnsafes.Count > 0) { thenewpath = Smooth(path, grid, newUnsafes); } } foreach (Pose p in thenewpath) { if (float.IsNaN(p.Position.X) || float.IsNaN(p.Position.Y)) { float noop; } } return(thenewpath); }
public static ArrayList <Pose> Smooth(ArrayList <Pose> path, ObstacleGrid grid) { return(Smooth(path, grid, null)); }