Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- using System;
- using System.Collections.Generic;
- namespace MonoTest
- {
- public class AStar
- {
- public static int resultLength=0;
- class Node
- {
- public Node(int x, int y)
- {
- posX = x;
- posY = y;
- neighbors = new List<Node>();
- }
- public List<Node> neighbors;
- public int posX;
- public int posY;
- public float g = float.PositiveInfinity;
- public float f = float.PositiveInfinity;
- public Node previous = null;
- }
- public static void Main()
- {
- for(int i=0; i<1000; ++i)
- {
- performAStar();
- }
- }
- public static void performAStar()
- {
- string map = "S XXX X\n"
- +"XX XXXXXXXXXXXXX X\n"
- +" XX X\n"
- +"X XXX XXXXXX\n"
- +" XXXX XXXXXXXX\n"
- +"XXXXXXX X X\n"
- +"XX X XXXXX X\n"
- +" XX XX X\n"
- +"XXXXXXXXXXX X\n"
- +"XX XXXXXXXXX X\n"
- +" XXXXXX X\n"
- +"X XXX XXXXXX\n"
- +" XXXX XXXXXXXX\n"
- +"XXXXXXX X X\n"
- +"XX X XXXXX X\n"
- +" XX XX X\n"
- +"XXXXX XXXXX X\n"
- +"XX XXXX XXXX X\n"
- +" XXXXXX X\n"
- +"X XXX XXXXXX\n"
- +" XXXX XXXXXXXX\n"
- +"XXXXX XX X X\n"
- +"XX X XXXXX X\n"
- +" XX XX X\n"
- +"XXXXX XXXXXXXXXXXX X\n"
- +" G XX X\n"
- +"XXXXXXXXXXXXXXXXXXXXXXX";
- string[] lines = map.Split ('\n');
- Node start = null;
- Node goal = null;
- int width = lines [0].Length;
- int height = lines.Length;
- Node[,] nodes = new Node[width,height];
- //initialize the node grid with zeros
- for (int x = 0; x < width; x++)
- for (int y = 0; y < height; y++)
- nodes [x,y] = null;
- //create the accessible nodes on the grid
- for (int y = 0; y < height; y++)
- {
- string line = lines [y];
- for (int x = 0; x < width; x++)
- {
- char c = line [x];
- //accessible node?
- if (c != 'X')
- {
- Node node = new Node (x, y);
- if (c == 'S')
- start = node;
- if (c == 'G')
- goal = node;
- nodes [x,y] = node;
- }
- }
- }
- //connect the nodes
- for (int x = 0; x < width; x++)
- {
- for (int y = 0; y < height; y++)
- {
- if(nodes [x,y] == null)
- continue;
- //up
- if (y > 0 && nodes [x,y - 1] != null)
- nodes [x,y].neighbors.Add (nodes [x,y - 1]);
- //up right
- if (y > 0 && x < width - 1 && nodes [x + 1,y - 1] != null)
- nodes [x,y].neighbors.Add (nodes [x + 1,y - 1]);
- //right
- if (x < width - 1 && nodes [x + 1,y] != null)
- nodes [x,y].neighbors.Add (nodes [x + 1,y]);
- //right down
- if (x < width - 1 && y < height && nodes [x + 1,y + 1] != null)
- nodes [x,y].neighbors.Add (nodes [x + 1,y + 1]);
- //down
- if (y < height && nodes [x,y + 1] != null)
- nodes [x,y].neighbors.Add (nodes [x,y + 1]);
- //down left
- if (y < height && x > 0 && nodes [x - 1,y + 1] != null)
- nodes [x,y].neighbors.Add (nodes [x - 1,y + 1]);
- //left
- if (x > 0 && nodes [x - 1,y] != null)
- nodes [x,y].neighbors.Add (nodes [x - 1,y]);
- //left up
- if (x > 0 && y > 0 && nodes [x - 1,y - 1] != null)
- nodes [x,y].neighbors.Add (nodes [x - 1,y - 1]);
- }
- }
- //perform the actal pathfinding
- if (!aStar(start, goal))
- {
- Console.Error.WriteLine("Unable to find a path.");
- }
- //reconstruct the path
- List<Node> path = new List<Node> ();
- Node waypoint = goal;
- path.Add (goal);
- while (waypoint.previous != null)
- {
- path.Add (waypoint.previous);
- waypoint = waypoint.previous;
- }
- string result = "";
- for (int i = path.Count - 1; i >= 0; i--)
- {
- Node p = path[i];
- result += "("+p.posX.ToString()+","+p.posY.ToString()+")";
- }
- //make sure the compiler can't optimize away any important computations by saving the result:
- resultLength = result.Length;
- //Console.WriteLine(result);
- }
- private static bool aStar(Node start, Node goal)
- {
- List<Node> closedSet = new List<Node> ();
- List<Node> openSet = new List<Node> ();
- openSet.Add (start);
- start.g = 0.0f;
- start.f = h (start, goal);
- while (openSet.Count > 0)
- {
- Node current = findBest(openSet);
- if(current == goal)
- return true;
- openSet.Remove(current);
- closedSet.Add(current);
- foreach(Node neighbor in current.neighbors)
- {
- if(closedSet.Contains(neighbor))
- continue;
- float newG = current.g + h(current, neighbor);
- if(newG < neighbor.g)
- {
- neighbor.g = newG;
- neighbor.f = newG + h(neighbor, goal);
- neighbor.previous = current;
- }
- if(!openSet.Contains(neighbor))
- {
- openSet.Add(neighbor);
- }
- }
- }
- return false;
- }
- private static float h(Node current, Node goal)
- {
- double dx = current.posX - goal.posX;
- double dy = current.posY - goal.posY;
- return (float)Math.Sqrt(dx*dx + dy*dy);
- }
- private static Node findBest(List<Node> list)
- {
- Node best = null;
- float bestF = float.PositiveInfinity;
- foreach (Node node in list)
- {
- if(node.f < bestF)
- {
- best = node;
- bestF = node.f;
- }
- }
- return best;
- }
- }
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement