public double lazyComputeCost(Dictionary <GridLocation, GridLocation> cameFrom,
Dictionary <GridLocation, double> costSoFar, GridLocation start, GridLocation end)
{
var parent = cameFrom[start];
if (parent != null)
{
return(costSoFar[parent] + parent.distanceTo(end));
}
return(start.distanceTo(end));
}
public Improbable.Vector3f nearestNoFlyZonePoint(Improbable.Vector3f point)
{
// Find out where point is in the grid
int[] gridCo = findGridCoordinatesOfPoint(point);
int x = gridCo[0];
int z = gridCo[1];
GridLocation anchor = new GridLocation(x, z);
double nearestDistance = Double.PositiveInfinity;
GridLocation nearestLocation = new GridLocation(0, 0); // placeholder.
bool foundNoFlyZone = false;
for (int layer = 1, maxLayers = 6; !foundNoFlyZone && layer <= maxLayers; ++layer)
{
// Placeholders.
int k, xi;
// Loop from top left to top right.
int zi = z - layer;
if (0 <= zi && zi < Height)
{
for (k = -layer; k < layer; ++k)
{
xi = x + k;
if (InGridBounds(xi, zi) && getGridCell(xi, zi) == GridType.IN)
{
foundNoFlyZone = true;
GridLocation candidate = new GridLocation(xi, zi);
double dist = anchor.distanceTo(candidate);
if (dist < nearestDistance)
{
nearestDistance = Math.Min(nearestDistance, dist);
nearestLocation = candidate;
}
}
}
}
// Loop from top right to bottom right.
xi = x + layer;
if (0 <= xi && xi < Width)
{
for (k = -layer; k < layer; ++k)
{
zi = z + k;
if (InGridBounds(xi, zi) && getGridCell(xi, zi) == GridType.IN)
{
foundNoFlyZone = true;
GridLocation candidate = new GridLocation(xi, zi);
double dist = anchor.distanceTo(candidate);
if (dist < nearestDistance)
{
nearestDistance = Math.Min(nearestDistance, dist);
nearestLocation = candidate;
}
}
}
}
// Loop from bottom right to bottom left.
if (0 <= zi && zi < Height)
{
zi = z + layer;
for (k = layer; k > -layer; --k)
{
xi = x + k;
if (InGridBounds(xi, zi) && getGridCell(xi, zi) == GridType.IN)
{
foundNoFlyZone = true;
GridLocation candidate = new GridLocation(xi, zi);
double dist = anchor.distanceTo(candidate);
if (dist < nearestDistance)
{
nearestDistance = Math.Min(nearestDistance, dist);
nearestLocation = candidate;
}
}
}
}
// Loop from bottom left to top left.
xi = x - layer;
if (0 <= xi && xi < Width)
{
for (k = layer; k > -layer; --k)
{
zi = z + k;
if (InGridBounds(xi, zi) && getGridCell(xi, zi) == GridType.IN)
{
foundNoFlyZone = true;
GridLocation candidate = new GridLocation(xi, zi);
double dist = anchor.distanceTo(candidate);
if (dist < nearestDistance)
{
nearestDistance = Math.Min(nearestDistance, dist);
nearestLocation = candidate;
}
}
}
}
}
if (!foundNoFlyZone)
{
return(new Improbable.Vector3f(0, -1, 0));
}
//given set point, convert grid -> real world
Improbable.Vector3f nearPoint = getPointFromGridCoordinates(new int[] { nearestLocation.x, nearestLocation.z });
//assert vector.y is 0
//TODO: assert nearPoint only has 2 non-zero elements
return(nearPoint);
}
