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