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