static void DrawGrid(SquareGrid field) //Prints the playing field to console. FOR DEBUGGING PURPOSES ONLY; DO NOT USE WHEN TESTING
{
field.CopyObstaclesToWalls();
HashSet <Location> path = field.GetShortestPath();
for (int y = field.Width; y >= 0; y--)
{
for (int x = 0; x <= field.Height; x++)
{
Location here = new Location(x, y);
if (here.Equals(field.Start))
{
Console.Write("S ");
}
else if (here.Equals(field.Goal))
{
Console.Write("G ");
}
else if (Util.ContainsLoc(field.walls, here))
{
Console.Write("# ");
}
else if (Util.ContainsLoc(path, here))
{
Console.Write($"{Util.LocIndex(path, here)} ");
}
else
{
Console.Write("* ");
}
}
Console.WriteLine();
}
}
static void DrawPassable(SquareGrid field) // Prints the playing field to console and highlights which points are passable. See warning above.
{
field.CopyObstaclesToWalls();
for (int y = field.Width; y >= 0; y--)
{
for (int x = 0; x <= field.Height; x++)
{
Location here = new Location(x, y);
if (field.Passable(here))
{
Console.Write("$ ");
}
else if (Util.ContainsLoc(field.walls, here))
{
Console.Write("# ");
}
else
{
Console.Write("* ");
}
}
Console.WriteLine();
}
}
// Conduct the A* search
public AStarSearch(SquareGrid graph, Vector3Int start, Vector3Int goal)
{
// start is current sprite Vector3Int
this.start = start;
// goal is sprite destination eg tile user clicked on
this.goal = goal;
// add the cross product of the start to goal and tile to goal vectors
// Vector3 startToGoalV = Vector3.Cross(start.vector3,goal.vector3);
// Vector3Int startToGoal = new Vector3Int(startToGoalV);
// Vector3 neighborToGoalV = Vector3.Cross(neighbor.vector3,goal.vector3);
// frontier is a List of key-value pairs:
// Vector3Int, (float) priority
var frontier = new PriorityQueue <Vector3Int>();
// Add the starting location to the frontier with a priority of 0
frontier.Enqueue(start, 0f);
cameFrom.Add(start, start); // is set to start, None in example
costSoFar.Add(start, 0f);
while (frontier.Count > 0f)
{
// Get the Vector3Int from the frontier that has the lowest
// priority, then remove that Vector3Int from the frontier
Vector3Int current = frontier.Dequeue();
// If we're at the goal Vector3Int, stop looking.
if (current.Equals(goal))
{
break;
}
// Neighbors will return a List of valid tile Vector3Ints
// that are next to, diagonal to, above or below current
foreach (var neighbor in graph.Neighbors(current))
{
// If neighbor is diagonal to current, graph.Cost(current,neighbor)
// will return Sqrt(2). Otherwise it will return only the cost of
// the neighbor, which depends on its type, as set in the TileType enum.
// So if this is a normal floor tile (1) and it's neighbor is an
// adjacent (not diagonal) floor tile (1), newCost will be 2,
// or if the neighbor is diagonal, 1+Sqrt(2). And that will be the
// value assigned to costSoFar[neighbor] below.
float newCost = costSoFar[current] + graph.Cost(current, neighbor);
// If there's no cost assigned to the neighbor yet, or if the new
// cost is lower than the assigned one, add newCost for this neighbor
if (!costSoFar.ContainsKey(neighbor) || newCost < costSoFar[neighbor])
{
// If we're replacing the previous cost, remove it
if (costSoFar.ContainsKey(neighbor))
{
costSoFar.Remove(neighbor);
cameFrom.Remove(neighbor);
}
costSoFar.Add(neighbor, newCost);
cameFrom.Add(neighbor, current);
float priority = newCost + Heuristic(neighbor, goal);
frontier.Enqueue(neighbor, priority);
}
}
}
}
static void Main(string[] args) //I realize there's a lot going on here. Due to time constraints, I can't really clean this up.
{
double californiaLatToMeter = 1.111785102E5;
double californiaLonToMeter = 9.226910619E4;
List <Waypoint> Waypoints = GrabMission();
List <Waypoint> Boundaries = GetBoundaries();
/*foreach (Waypoint w in Waypoints)
* {
* Console.WriteLine(w);
* }
* Console.WriteLine();*/// for debugging
//DrawPassable(playingField);
/*Console.WriteLine($"Width: {playingField.Width}; Height: {playingField.Height}");
* Console.WriteLine($"Start = {playingField.Start}");
* Console.WriteLine($"Goal = {playingField.Goal}");*/// DO NOT REMOVE OR UNCOMMENT: This part visualizes the grid by printing it out. For use when Length & Width < 30.
List <ParsedObstacle> ParsedObstacles = ParseObstacles(Waypoints[0].Altitude);
double upmost = 0, leftmost = 0, rightmost = 0, downmost = 0;
SquareGrid playingField = GenerateField(Waypoints, ParsedObstacles, Boundaries, ref downmost, ref upmost, ref leftmost, ref rightmost);
HashSet <Location> path = new HashSet <Location>();
path = playingField.GetShortestPath();
double count = 0.1;
foreach (Location p in path)
{
if (p != playingField.Goal && p != playingField.Start)
{
double order = playingField.FirstOrder + count;
count += 0.1;
double latitude = (p.X + leftmost - 5) / californiaLatToMeter;
double longitude = (p.Y + downmost - 5) / californiaLonToMeter;
Waypoints.Add(new Waypoint(order, latitude, longitude, (Waypoints[0].Altitude + Waypoints[1].Altitude) / 2));
}
}
Waypoints.Sort();
for (int i = 2; i <= Waypoints[Waypoints.Count - 1].Order; i++)
{
path.Clear();
int index = Util.GetIndexOfOrder(i, Waypoints);
//Console.WriteLine(Waypoints[index - 1].Latitude * californiaLatToMeter); // for debugging
double swrappedlat = (Waypoints[index - 1].Latitude * californiaLatToMeter) - leftmost;
//Console.WriteLine(Waypoints[index - 1].Longitude * californiaLonToMeter); // for debugging
double swrappedlon = (Waypoints[index - 1].Longitude * californiaLonToMeter) - downmost;
playingField.Start = new Location(Convert.ToInt32(swrappedlat) + 5, Convert.ToInt32(swrappedlon) + 5);
//Console.WriteLine(Waypoints[index].Latitude * californiaLatToMeter); // for debugging
double gwrappedlat = (Waypoints[index].Latitude * californiaLatToMeter) - leftmost;
//Console.WriteLine(Waypoints[index].Longitude * californiaLonToMeter); // for debugging
double gwrappedlon = (Waypoints[index].Longitude * californiaLonToMeter) - downmost;
playingField.Goal = new Location(Convert.ToInt32(gwrappedlat) + 5, Convert.ToInt32(gwrappedlon) + 5);
playingField.FirstOrder = i - 1;
playingField.Altitude = Waypoints[i - 1].Altitude;
path = playingField.GetShortestPath();
foreach (Location p in path)
{
if (p != playingField.Goal && p != playingField.Start)
{
double order = playingField.FirstOrder + count;
count += 0.1;
double latitude = (p.X + leftmost - 5) / californiaLatToMeter;
double longitude = (p.Y + downmost - 5) / californiaLonToMeter;
Waypoints.Add(new Waypoint(order, latitude, longitude, (Waypoints[i - 1].Altitude + Waypoints[i].Altitude) / 2));
}
}
}
Waypoints.Sort();
String final = "";
int counts = 1;
foreach (Waypoint wp in Waypoints)
{
final += wp.ToString();
if (counts != Waypoints.Count)
{
final += ",";
}
counts++;
}
System.IO.File.WriteAllText(@"C:\Users\Public\Downloads\InteropCode\interop\MissionPointsParsedObstacle.txt", final);
}
static SquareGrid GenerateField(List <Waypoint> Waypoints, List <ParsedObstacle> ParsedObstacles, List <Waypoint> Boundaries, ref double downmost, ref double upmost, ref double leftmost, ref double rightmost) //Generates the playing field given all necessary information.
{
double californiaLatToMeter = 1.111785102E5;
double californiaLonToMeter = 9.226910619E4;
/* To do: Add Maryland conversions */
double latitudeconversion = californiaLatToMeter;
double longitudeconversion = californiaLonToMeter;
upmost = Double.MinValue;
downmost = Double.MaxValue;
leftmost = Double.MaxValue;
rightmost = Double.MinValue;
foreach (Waypoint w in Waypoints)
{
if (w.Longitude < downmost)
{
downmost = w.Longitude;
}
if (w.Latitude < leftmost)
{
leftmost = w.Latitude;
}
if (w.Longitude > upmost)
{
upmost = w.Longitude;
}
if (w.Latitude > rightmost)
{
rightmost = w.Latitude;
}
}
double deltaHeight = upmost - downmost;
int height = (int)(deltaHeight * longitudeconversion) + 10;
double deltaWidth = rightmost - leftmost;
int width = (int)(deltaWidth * latitudeconversion) + 10;
rightmost *= latitudeconversion;
leftmost *= latitudeconversion;
upmost *= longitudeconversion;
downmost *= longitudeconversion;
//Console.WriteLine(Waypoints[0].Latitude * latitudeconversion); // for debugging
double swrappedlat = (Waypoints[0].Latitude * latitudeconversion) - leftmost;
//Console.WriteLine(Waypoints[0].Longitude * longitudeconversion); // for debugging
double swrappedlon = (Waypoints[0].Longitude * longitudeconversion) - downmost;
Location start = new Location(Convert.ToInt32(swrappedlat) + 5, Convert.ToInt32(swrappedlon) + 5);
//Console.WriteLine(Waypoints[1].Latitude * latitudeconversion); // for debugging
double gwrappedlat = (Waypoints[1].Latitude * latitudeconversion) - leftmost;
//Console.WriteLine(Waypoints[1].Longitude * longitudeconversion); // for debugging
double gwrappedlon = (Waypoints[1].Longitude * longitudeconversion) - downmost;
Location goal = new Location(Convert.ToInt32(gwrappedlat) + 5, Convert.ToInt32(gwrappedlon) + 5);
SquareGrid playingField = new SquareGrid(Waypoints[0].Altitude, width, height, start, goal, Waypoints[0].Order);
foreach (ParsedObstacle po in ParsedObstacles)
{
double wrappedlat = (po.Latitude * latitudeconversion) - leftmost;
double wrappedlon = (po.Longitude * longitudeconversion) - downmost;
Location center = new Location(Convert.ToInt32(wrappedlat) + 5, Convert.ToInt32(wrappedlon) + 5);
playingField.obstacles.Add(new Circle(center, po.Radius));
}
foreach (Waypoint boundarypoint in Boundaries)
{
double wrappedlat = (boundarypoint.Latitude * latitudeconversion) - leftmost;
double wrappedlon = (boundarypoint.Longitude * longitudeconversion) - downmost;
playingField.boundaries.Add(new OrderedLocation(boundarypoint.Order, Convert.ToInt32(wrappedlat), Convert.ToInt32(wrappedlon)));
playingField.boundaries.Sort();
}
return(playingField);
}
