Commits

Robin Harper  committed 51b0565

Added a grid class to assist in finding the adjacent nodes from a parent node. Also added a merge sort to the class. Updated AstarPathfinder to use the new Grid collection along with removing the need for a helper class to switch between locations and gridnodes

  • Participants
  • Parent commits 6b4cecf

Comments (0)

Files changed (4)

File AstarPath/AstarPath.csproj

   </ItemGroup>
   <ItemGroup>
     <Compile Include="AstarPathfinder.cs" />
+    <Compile Include="Grid.cs" />
     <Compile Include="GridNode.cs" />
     <Compile Include="Location.cs" />
     <Compile Include="PathfinderUtilities.cs" />

File AstarPath/AstarPathfinder.cs

     {
         private const double Alpha = 0.4999;
         private const double DefaultCost = 1.0;
-        private const int MaxEstimatedPathLength = 1500;
+        private const int MaxEstimatedPathLength = 2000;
         private readonly int DirectionalMove = 1;
         private const double Nudge = DefaultCost / MaxEstimatedPathLength;
-        public readonly int[,] Obstacles;
-        public readonly List<GridNode> Grid;
+        //public readonly int[,] Obstacles;
+        //public readonly List<GridNode> Grid;
+        public readonly Grid Grid;
 
         public readonly int BitmapHeight;
         public readonly int BitmapWidth;
 
         public readonly PathfinderUtilities.PathfinderType CurrentPathfinderType;
 
+        private readonly int _size = 0;
+
         /// <summary>
         /// 
         /// </summary>
             BitmapHeight = map.Height;
             BitmapWidth = map.Width;
             CurrentPathfinderType = type;
-            Obstacles = PathfinderUtilities.GetObstacles(map);
-            Grid = PathfinderUtilities.CreateGrid(map, Obstacles, size);
+            _size = size;
+            //Obstacles = PathfinderUtilities.GetObstacles(map);
+            //Grid = PathfinderUtilities.CreateGrid(map, Obstacles, size);
+            Grid = PathfinderUtilities.CreateGrid(map, size);
 
             switch (type)
             {
         }
 
         /// <summary>
-        /// 
+        /// The optimal choices for inputs are:
+        /// 11 for grid size and AdvancedManhattan for the path finding
+        /// heuristic
         /// </summary>
         /// <param name="inputStart"></param>
         /// <param name="inputGoal"></param>
         /// <returns></returns>
         public List<GridNode> GridSearch(Point inputStart, Point inputGoal)
         {
-            List<GridNode> opened = new List<GridNode>();
-            List<GridNode> closed = new List<GridNode>();
+            //List<GridNode> opened = new List<GridNode>();
+            //List<GridNode> closed = new List<GridNode>();
+            var opened = new Grid();
+            var closed = new Grid();
             var start = Grid.FirstOrDefault(t => t.PointsInsideGrid.Contains(inputStart));
             var goal = Grid.FirstOrDefault(t => t.PointsInsideGrid.Contains(inputGoal));
 
                     start.IsObstacle = false;
                 }
 
-                start = UpdateHeuristics(start, start, start, goal);
+                //start = UpdateHeuristics(start, start, start, goal);
+                start = CalculateHeuristics(start, start, start, goal);
                 opened.Add(start);
 
                 while (opened.Count > 0)
                 {
-                    opened = GridNode.MergeSort(opened);
+                    //opened = GridNode.MergeSort(opened);
+                    opened = opened.MergeSort(opened);
 
                     var parent = opened[0];
                     closed.Add(parent);
                     opened.RemoveAt(0);
 
-                    if ((parent.GridBorder == goal.GridBorder) || opened.Count >= MaxEstimatedPathLength)
+                    if (parent.GridBorder == goal.GridBorder)
                     {
                         goal = parent;
                         opened.Clear();
                         break;
                     }
+                    else if (opened.Count >= MaxEstimatedPathLength)
+                    {
+                        opened.Clear();
+                        break;
+                    }
                     else
                     {
                         var adjacentNodes = PathfinderUtilities.FindAdjacentNodes(
-                            Grid, 
-                            parent, 
-                            CurrentPathfinderType,
-                            Obstacles,
-                            BitmapHeight, 
-                            BitmapHeight);
+                            Grid,
+                            parent,
+                            CurrentPathfinderType);
+                            //Obstacles,
+                            //BitmapHeight,
+                            //BitmapHeight);
 
                         adjacentNodes = UpdateHeuristics(adjacentNodes, parent, start, goal);
 
             var updated = new List<GridNode>();
             foreach(var localGridBlock in adjacentNodes)
             {
-                updated.Add(UpdateHeuristics(localGridBlock, parent, start, goal));
+                //updated.Add(UpdateHeuristics(localGridBlock, parent, start, goal));
+                updated.Add(CalculateHeuristics(localGridBlock, parent, start, goal));
             }
 
             return updated;
         /// <param name="start"></param>
         /// <param name="goal"></param>
         /// <returns></returns>
-        private GridNode UpdateHeuristics(GridNode neighboor, GridNode parent, GridNode start, GridNode goal)
-        {
-            var center = CalculateHeuristics(neighboor.Center, parent.Center, start.Center, goal.Center);
-            var newNeighboor = new GridNode(neighboor.GridBorder, neighboor.IsObstacle, center, parent);
+        //private GridNode UpdateHeuristics(GridNode neighboor, GridNode parent, GridNode start, GridNode goal)
+        //{
+        //    var center = CalculateHeuristics(neighboor.Center, parent.Center, start.Center, goal.Center);
+        //    var newNeighboor = new GridNode(neighboor.GridBorder, neighboor.IsObstacle, center, parent);
 
-            return newNeighboor;
-        }
+        //    return newNeighboor;
+        //}
 
         /// <summary>
         /// A method for grouping all the heuristic calculations together
         /// <param name="parent"></param>
         /// <param name="goal"></param>
         /// <returns></returns>
-        private Location CalculateHeuristics(Location neighboor, Location parent, Location start, Location goal)
+        private GridNode CalculateHeuristics(GridNode neighboor, GridNode parent, GridNode start, GridNode goal)
         {
             var g = CalculateG(parent, neighboor);
             var h = 0.0;
 
              var f = (Alpha * g) + (((DefaultCost - Alpha) * h) / Math.Max(Alpha, (DefaultCost - Alpha)));
 
-             return new Location(neighboor.x, neighboor.y, g, f, h);
+             //return new Location(neighboor.x, neighboor.y, g, f, h);
+             return new GridNode(neighboor.GridBorder, neighboor.IsObstacle, new Location(neighboor.Center.x, neighboor.Center.y, g, f, h), parent);
         }
 
         /// <summary>
         /// <param name="start">The original start position on the map</param>
         /// <param name="goal">Our goal position on the map</param>
         /// <returns></returns>
-        private double CalculateHWithBasicManhattan(Location neighboor, Location parent, Location start, Location goal)
+        private double CalculateHWithBasicManhattan(GridNode neighboor, GridNode parent, GridNode start, GridNode goal)
         {
-            var h = GetCost(parent, neighboor) * ((Math.Abs(neighboor.x - goal.x) + Math.Abs(neighboor.y - goal.y)));
+            var h = GetCost(parent, neighboor) * ((Math.Abs(neighboor.Center.x - goal.Center.x) + Math.Abs(neighboor.Center.y - goal.Center.y)));
             return CalculateSimpleNudge(h);
         }
 
         /// <param name="start">The original start position on the map</param>
         /// <param name="goal">Our goal position on the map</param>
         /// <returns></returns>
-        private double CalculateHWithAdvancedManhattan(Location neighboor, Location parent, Location start, Location goal)
+        private double CalculateHWithAdvancedManhattan(GridNode neighboor, GridNode parent, GridNode start, GridNode goal)
         {
-            var h = GetCost(parent, neighboor) * Math.Max(Math.Abs(neighboor.x - goal.x), Math.Abs(neighboor.y - goal.y));
-            return CalculateComplexNudge(h, parent.ToPoint(), start.ToPoint(), goal.ToPoint());
+            var h = GetCost(parent, neighboor) * Math.Max(Math.Abs(neighboor.Center.x - goal.Center.x), Math.Abs(neighboor.Center.y - goal.Center.y));
+            return CalculateComplexNudge(h, parent.Center.ToPoint(), start.Center.ToPoint(), goal.Center.ToPoint());
         }
 
         /// <summary>
         /// <param name="start">The original start position on the map</param>
         /// <param name="goal">Our goal position on the map</param>
         /// <returns></returns>
-        private double CalculateHWithEuclidian(Location neighboor, Location parent, Location start, Location goal)
+        private double CalculateHWithEuclidian(GridNode neighboor, GridNode parent, GridNode start, GridNode goal)
         {
-            var h = GetCost(parent, neighboor) * (Math.Sqrt(Math.Pow((neighboor.x - goal.x), 2) + Math.Pow((neighboor.y - goal.y), 2)));
+            var h = GetCost(parent, neighboor) * (Math.Sqrt(Math.Sqrt((neighboor.Center.x - goal.Center.x)) + 
+                                                                Math.Sqrt((neighboor.Center.y - goal.Center.y))));
+
             return CalculateSimpleNudge(h);
         }
 
         /// <param name="parentLocation"></param>
         /// <param name="alpha"></param>
         /// <returns></returns>
-        private double CalculateG(Location parentLocation, Location currentLocation)
+        private double CalculateG(GridNode parentLocation, GridNode currentLocation)
         {
-            return GetCost(parentLocation, currentLocation) + parentLocation.g;
+            return GetCost(parentLocation, currentLocation) + parentLocation.Center.g;
         }
 
         /// <summary>
         /// <param name="start"></param>
         /// <param name="goal"></param>
         /// <returns></returns>
-        private double GetCost(Location start, Location goal)
+        private double GetCost(GridNode start, GridNode goal)
         {
-            return (Obstacles[start.x, start.y] + Obstacles[goal.x, goal.y]) * DirectionalMove;
+            var cost = 1.0;
+            //return (Obstacles[start.x, start.y] + Obstacles[goal.x, goal.y]) * DirectionalMove;
+            if (goal.IsObstacle)
+            {
+                cost += 1.0;
+            }
+
+            return cost * DirectionalMove;
         }
 
         /// <summary>

File AstarPath/Grid.cs

+using System;
+using System.Collections.Generic;
+using System.Linq;
+using System.Text;
+
+namespace AstarPath
+{
+    public class Grid : List<GridNode>
+    {
+        private int _w = 0;
+        private int _h = 0;
+        private int _size = 0;
+
+
+        public Grid() : base()
+        {
+            
+        }
+
+        public Grid(List<GridNode> list)
+            : base(list)
+        {
+        }
+
+        public GridNode this[int x, int y]
+        {
+            get 
+            {
+                if (this.Count > 0)
+                {
+                    if (x >= 0 && y >= 0)
+                    {
+                        var n = (from node in this
+                                 where node.X == x && node.Y == y
+                                 select node);
+
+                        if (n != null && n.Count() > 0)
+                        {
+                            return n.First();
+                        }
+                    }
+                }
+
+                return null;
+            }
+        }
+
+        public Grid MergeSort(Grid grid)
+        {
+            if (grid.Count <= 1)
+            {
+                return grid;
+            }
+
+            var left = new Grid();
+            var right = new Grid();
+
+            int middle = grid.Count / 2;
+
+            for (int i = 0; i < middle; i++)
+            {
+                left.Add(grid[i]);
+            }
+
+            for (int i = middle; i < grid.Count; i++)
+            {
+                right.Add(grid[i]);
+            }
+
+            left = MergeSort(left);
+            right = MergeSort(right);
+            return Merge(left, right);
+        }
+
+        private Grid Merge(List<GridNode> left, List<GridNode> right)
+        {
+            var result = new Grid();
+
+            while (left.Count > 0 || right.Count > 0)
+            {
+                if (left.Count > 0 && right.Count > 0)
+                {
+                    if (left[0].Center.f <= right[0].Center.f)
+                    {
+                        result.Add(left[0]);
+                        left.RemoveAt(0);
+
+                        if (left.Count >= 1)
+                        {
+                            left = left.GetRange(0, left.Count);
+                        }
+                    }
+                    else
+                    {
+                        result.Add(right[0]);
+                        right.RemoveAt(0);
+
+                        if (right.Count >= 1)
+                        {
+                            right = right.GetRange(0, right.Count);
+                        }
+                    }
+                }
+                else if (left.Count > 0)
+                {
+                    result.Add(left[0]);
+                    left.RemoveAt(0);
+
+                    if (left.Count >= 1)
+                    {
+                        left = left.GetRange(0, left.Count);
+                    }
+                }
+                else if (right.Count > 0)
+                {
+                    result.Add(right[0]);
+                    right.RemoveAt(0);
+
+                    if (right.Count >= 1)
+                    {
+                        right = right.GetRange(0, right.Count);
+                    }
+                }
+            }
+
+            return result;
+        }
+    }
+}

File AstarPath/PathfinderUtilities.cs

             return localGrid;
         }
 
+        public static Grid CreateGrid(Bitmap map, int gridSize)
+        {
+            var _grid = new Grid();
+
+            int ycounter = 0;
+            int xcounter = 0;
+
+            for (int y = 0; y < map.Height; y += gridSize)
+            {
+                xcounter = 0;
+                for (int x = 0; x < map.Width; x += gridSize)
+                {
+                    var rectangle = new Rectangle(xcounter, ycounter, gridSize, gridSize);
+                    // set default obstacle status to false
+                    var cp = PathfinderUtilities.GetActualCenterPoint(x, y, gridSize);
+                    var center = new Location(cp);
+                    var gridBlock = new GridNode(rectangle, false, center, null);
+
+                    _grid.Add(gridBlock);
+
+                    xcounter++;
+                }
+
+                ycounter++;
+            }
+
+            _grid = FindObstacles(map, _grid);
+
+            return _grid;
+        }
+
+        public static Grid FindObstacles(Bitmap map, Grid grid)
+        {
+            for(int i = 0; i < grid.Count; i++)
+            {
+                foreach (var p in grid[i].PointsInsideGrid)
+                {
+                    if (p.X >= 0 && p.X < map.Width)
+                    {
+                        if (p.Y >= 0 && p.Y < map.Height)
+                        {
+                            var pix = map.GetPixel(p.X, p.Y);
+                            var r = pix.R; var g = pix.G; var b = pix.B;
+                            if (r <= Color.DarkGray.R && g <= Color.DarkGray.G && b <= Color.DarkGray.B)
+                            {
+                                // break back out to the next node
+                                grid[i].IsObstacle = true;
+                                break;
+                            }
+                        }
+                    }
+                }
+            }
+
+            return grid;
+        }
+
         /// <summary>
         /// 
         /// </summary>
                 {
                     var xList = grid.FindAll(t => t.X == searchPoint.X);
                     var gridBlock = xList.Find(t => t.Y == searchPoint.Y);
-                    /*
-                    GridNode gridBlock = grid.FirstOrDefault(t => 
-                        t.X == searchPoint.X && t.Y == searchPoint.Y);
-                     */
 
                     if (gridBlock != null && gridBlock != current)
                     {
             return adjacentNodes;
         }
 
+        public static List<GridNode> FindAdjacentNodes(Grid grid, GridNode current, PathfinderType pathfinderHeuristic)
+        {
+            var adjacentNodes = new List<GridNode>();
+            var searchList = CreateSearchList(1, pathfinderHeuristic);
+
+            foreach (var searchValue in searchList)
+            {
+                var node = grid[current.X + searchValue.x, current.Y + searchValue.y];
+
+                if (node != null && !node.IsObstacle)
+                {
+                    adjacentNodes.Add(node);
+                }
+            }
+
+            return adjacentNodes;
+        }
+
         /// <summary>
         /// Iterates over the bitmap to generate an obstacle map.
         /// 
         /// <returns></returns>
         public static Point GetActualCenterPoint(int x, int y, int gridSize)
         {
-            switch (gridSize)
+            if(gridSize > 0)
             {
-                default:
-                    return new Point(x + 1, y + 1);
-                case 5:
-                    return new Point(x + 2, y + 2);
-                case 7:
-                    return new Point(x + 3, y + 3);
-                case 9:
-                    return new Point(x + 4, y + 4);
-                case 11:
-                    return new Point(x + 5, y + 5);
-                case 13:
-                    return new Point(x + 6, y + 6);
-                case 15:
-                    return new Point(x + 7, y + 7);
+                var floor = Convert.ToInt32(Math.Floor(gridSize / 2.0));
+                return new Point(x + floor, y + floor);
             }
+            
+            return new Point(0, 0);
         }
 
         /// <summary>
         /// <returns></returns>
         public static Point GetTopLeftPoint(int centerX, int centerY, int gridSize)
         {
-            switch(gridSize)
+            if (gridSize > 0)
             {
-                default:
-                    return new Point(centerX - 1, centerY - 1);
-                case 5:
-                    return new Point(centerX - 2, centerY - 2);
-                case 7:
-                    return new Point(centerX - 3, centerY - 3);
-                case 9:
-                    return new Point(centerX - 4, centerY - 4);
-                case 11:
-                    return new Point(centerX - 5, centerY - 5);
-                case 13:
-                    return new Point(centerX - 6, centerY - 6);
-                case 15:
-                    return new Point(centerX - 7, centerY - 7);
+                var floor = Convert.ToInt32(Math.Floor(gridSize / 2.0));
+                return new Point(centerX - floor, centerY - floor);
             }
+
+            return new Point(0, 0);
         }
     }
 }