Advertisement
upvoid

Naive C# AStar

Feb 15th, 2013
1,209
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C# 7.88 KB | None | 0 0
  1. using System;
  2. using System.Collections.Generic;
  3.  
  4. namespace MonoTest
  5. {
  6.     public class AStar
  7.     {
  8.         public static int resultLength=0;
  9.  
  10.         class Node
  11.         {
  12.             public Node(int x, int y)
  13.             {
  14.                 posX = x;
  15.                 posY = y;
  16.                 neighbors = new List<Node>();
  17.  
  18.             }
  19.  
  20.             public List<Node> neighbors;
  21.             public int posX;
  22.             public int posY;
  23.  
  24.             public float g = float.PositiveInfinity;
  25.             public float f = float.PositiveInfinity;
  26.             public Node previous = null;
  27.         }
  28.  
  29.         public static void Main()
  30.         {
  31.             for(int i=0; i<1000; ++i)
  32.             {
  33.                 performAStar();
  34.             }
  35.         }
  36.  
  37.         public static void performAStar()
  38.         {
  39.             string map = "S   XXX               X\n"
  40.                         +"XX      XXXXXXXXXXXXX X\n"
  41.                         +"       XX             X\n"
  42.                         +"X     XXX        XXXXXX\n"
  43.                         +"        XXXX   XXXXXXXX\n"
  44.                         +"XXXXXXX    X          X\n"
  45.                         +"XX         X   XXXXX  X\n"
  46.                         +"          XX     XX   X\n"
  47.                         +"XXXXXXXXXXX           X\n"
  48.                         +"XX          XXXXXXXXX X\n"
  49.                         +"       XXXXXX         X\n"
  50.                         +"X     XXX        XXXXXX\n"
  51.                         +"        XXXX   XXXXXXXX\n"
  52.                         +"XXXXXXX    X          X\n"
  53.                         +"XX         X   XXXXX  X\n"
  54.                         +"          XX     XX   X\n"
  55.                         +"XXXXX XXXXX           X\n"
  56.                         +"XX          XXXX XXXX X\n"
  57.                         +"       XXXXXX         X\n"
  58.                         +"X     XXX        XXXXXX\n"
  59.                         +"        XXXX   XXXXXXXX\n"
  60.                         +"XXXXX XX   X          X\n"
  61.                         +"XX         X   XXXXX  X\n"
  62.                         +"          XX     XX   X\n"
  63.                         +"XXXXX XXXXXXXXXXXX    X\n"
  64.                         +"              G XX    X\n"
  65.                         +"XXXXXXXXXXXXXXXXXXXXXXX";
  66.             string[] lines = map.Split ('\n');
  67.  
  68.             Node start = null;
  69.             Node goal = null;
  70.  
  71.             int width = lines [0].Length;
  72.             int height = lines.Length;
  73.  
  74.             Node[,] nodes = new Node[width,height];
  75.             //initialize the node grid with zeros
  76.             for (int x = 0; x < width; x++)
  77.                 for (int y = 0; y < height; y++)
  78.                     nodes [x,y] = null;
  79.  
  80.             //create the accessible nodes on the grid
  81.             for (int y = 0; y < height; y++)
  82.             {
  83.                 string line = lines [y];
  84.                 for (int x = 0; x < width; x++)
  85.                 {
  86.                     char c = line [x];
  87.  
  88.                     //accessible node?
  89.                     if (c != 'X')
  90.                     {
  91.                         Node node = new Node (x, y);
  92.  
  93.                         if (c == 'S')
  94.                             start = node;
  95.                         if (c == 'G')
  96.                             goal = node;
  97.  
  98.                         nodes [x,y] = node;
  99.                     }
  100.                 }
  101.             }
  102.  
  103.             //connect the nodes
  104.             for (int x = 0; x < width; x++)
  105.             {
  106.                 for (int y = 0; y < height; y++)
  107.                 {
  108.                     if(nodes [x,y] == null)
  109.                         continue;
  110.  
  111.                     //up
  112.                     if (y > 0 && nodes [x,y - 1] != null)
  113.                         nodes [x,y].neighbors.Add (nodes [x,y - 1]);
  114.                    
  115.                     //up right
  116.                     if (y > 0 && x < width - 1 && nodes [x + 1,y - 1] != null)
  117.                         nodes [x,y].neighbors.Add (nodes [x + 1,y - 1]);
  118.                    
  119.                     //right
  120.                     if (x < width - 1 && nodes [x + 1,y] != null)
  121.                         nodes [x,y].neighbors.Add (nodes [x + 1,y]);
  122.  
  123.                     //right down
  124.                     if (x < width - 1 && y < height && nodes [x + 1,y + 1] != null)
  125.                         nodes [x,y].neighbors.Add (nodes [x + 1,y + 1]);
  126.  
  127.                     //down
  128.                     if (y < height && nodes [x,y + 1] != null)
  129.                         nodes [x,y].neighbors.Add (nodes [x,y + 1]);
  130.                    
  131.                     //down left
  132.                     if (y < height && x > 0 && nodes [x - 1,y + 1] != null)
  133.                         nodes [x,y].neighbors.Add (nodes [x - 1,y + 1]);
  134.                    
  135.                     //left
  136.                     if (x > 0 && nodes [x - 1,y] != null)
  137.                         nodes [x,y].neighbors.Add (nodes [x - 1,y]);
  138.                    
  139.                     //left up
  140.                     if (x > 0 && y > 0 && nodes [x - 1,y - 1] != null)
  141.                         nodes [x,y].neighbors.Add (nodes [x - 1,y - 1]);
  142.                 }
  143.             }
  144.  
  145.             //perform the actal pathfinding
  146.             if (!aStar(start, goal))
  147.             {
  148.                 Console.Error.WriteLine("Unable to find a path.");
  149.             }
  150.  
  151.             //reconstruct the path
  152.             List<Node> path = new List<Node> ();
  153.             Node waypoint = goal;
  154.             path.Add (goal);
  155.             while (waypoint.previous != null)
  156.             {
  157.                 path.Add (waypoint.previous);
  158.                 waypoint = waypoint.previous;
  159.             }
  160.  
  161.             string result = "";
  162.  
  163.             for (int i = path.Count - 1; i >= 0; i--)
  164.             {
  165.                 Node p = path[i];
  166.                 result += "("+p.posX.ToString()+","+p.posY.ToString()+")";
  167.             }
  168.  
  169.             //make sure the compiler can't optimize away any important computations by saving the result:
  170.             resultLength = result.Length;
  171.             //Console.WriteLine(result);
  172.  
  173.         }
  174.  
  175.         private static bool aStar(Node start, Node goal)
  176.         {
  177.             List<Node> closedSet = new List<Node> ();
  178.             List<Node> openSet = new List<Node> ();
  179.  
  180.             openSet.Add (start);
  181.             start.g = 0.0f;
  182.             start.f = h (start, goal);
  183.  
  184.             while (openSet.Count > 0)
  185.             {
  186.                 Node current = findBest(openSet);
  187.  
  188.                 if(current == goal)
  189.                     return true;
  190.  
  191.                 openSet.Remove(current);
  192.                 closedSet.Add(current);
  193.  
  194.                 foreach(Node neighbor in current.neighbors)
  195.                 {
  196.                     if(closedSet.Contains(neighbor))
  197.                        continue;
  198.  
  199.                     float newG = current.g + h(current, neighbor);
  200.                     if(newG < neighbor.g)
  201.                     {
  202.                         neighbor.g = newG;
  203.                         neighbor.f = newG + h(neighbor, goal);
  204.                         neighbor.previous = current;
  205.                     }
  206.  
  207.                     if(!openSet.Contains(neighbor))
  208.                     {
  209.                         openSet.Add(neighbor);
  210.                     }
  211.                 }
  212.             }
  213.  
  214.             return false;
  215.         }
  216.  
  217.         private static float h(Node current, Node goal)
  218.         {
  219.             double dx = current.posX - goal.posX;
  220.             double dy = current.posY - goal.posY;
  221.             return (float)Math.Sqrt(dx*dx + dy*dy);
  222.         }
  223.  
  224.         private static Node findBest(List<Node> list)
  225.         {
  226.             Node best = null;
  227.             float bestF = float.PositiveInfinity;
  228.  
  229.             foreach (Node node in list)
  230.             {
  231.                 if(node.f < bestF)
  232.                 {
  233.                     best = node;
  234.                     bestF = node.f;
  235.                 }
  236.             }
  237.  
  238.             return best;
  239.         }
  240.     }
  241. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement