[Release] AStar Algorithm

[Mr_PoP]

Well , I was working on the AStar Algorithm , and am almost finished it.
the way I intended to design it with so I don't need anther struct/class for Node, but anyways it needs some Optimization ("like using Heap (data structure) instead of Dictionary") but so far it works fine.

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using MapData.Interfaces;

namespace MapData.Algorithm {
    public class AStar {
        IDMap map;
        List<Location2D> openset = new List<Location2D>(); // The set of tentative nodes to be evaluated, initially containing the start node.
        List<Location2D> closedset = new List<Location2D>(); // The set of nodes already evaluated.
        List<Location2D> path = new List<Location2D>(); // The path result.
        Dictionary<Location2D, Double> g_scores = new Dictionary<Location2D, Double>(); // Cost from start along best known path.
        Dictionary<Location2D, Double> h_scores = new Dictionary<Location2D, Double>(); // Estimated cost to get from a node to the goal.
        Dictionary<Location2D, Double> f_scores = new Dictionary<Location2D, Double>(); // Estimated total cost from start to goal.
        Dictionary<Location2D, Location2D> came_from = new Dictionary<Location2D, Location2D>(); // The navigated nodes.
        int[] ArrayX = { 0, -1, -1, -1, 0, 1, 1, 1 };
        int[] ArrayY = { 1, 1, 0, -1, -1, -1, 0, 1 };

        /// <summary>
        /// Initing the Algorithm with MapInfo
        /// </summary>
        /// <param name="map">DMap info.</param>
        public AStar(IDMap map) {
            this.map = map;
        }

        /// <summary>
        /// Start searching untile it's find it's goal or return false.
        /// </summary>
        /// <param name="start">Starting Node.</param>
        /// <param name="goal">Ending Node.</param>
        public bool Find( Location2D start, Location2D goal) {
            #region Clear
            openset.Clear();
            closedset.Clear();
            path.Clear();
            g_scores.Clear();
            h_scores.Clear();
            f_scores.Clear();
            came_from.Clear();
            #endregion

            openset.Add(start);
            g_scores[start] = 0;
            h_scores[start] = GetHValue(start, goal);
            f_scores[start] = g_scores[start] + h_scores[start];

            while (openset.Count != 0) { // while openset is not empty

                Location2D current = CheckBestNode(); //the node in openset having the lowest f_score[] value
                

                if (current.Equals(goal)) {
                    ReconstructPathRecursive(current);
                    return true;
                }

                Location2D neighbor = new Location2D();

                for (int i = 0; i < 8; i++) { // neighbor nodes.
                    neighbor.X = (ushort)(current.X + ArrayX[i]);
                    neighbor.Y = (ushort)(current.Y + ArrayY[i]);

                    //TODO: check walkeable areas!

                    g_scores[neighbor] = NeighborDistance(current, neighbor);
                    h_scores[neighbor] = GetHValue(neighbor, goal);
                    f_scores[neighbor] = g_scores[neighbor] + h_scores[neighbor];

                    bool tentative_is_better = false;
                    Double tentative_g_score = 0;

                    if (closedset.Contains(neighbor)) continue;

                    if (!openset.Contains(neighbor)) {
                        openset.Add(neighbor);
                        tentative_g_score = g_scores[neighbor] + GetHValue(neighbor, goal);
                        tentative_is_better = true;
                    } else if (tentative_g_score < g_scores[neighbor]) {
                        tentative_is_better = true;
                    } else {
                        tentative_is_better = false;
                    }

                    if (tentative_is_better) {
                        came_from[neighbor] = current;
                        g_scores[neighbor] = tentative_g_score;
                        h_scores[neighbor] = GetHValue(neighbor, goal);
                        f_scores[neighbor] = g_scores[neighbor] + h_scores[neighbor];
                    }
                }
            }
            return false;
        }

        /// <summary>
        /// Check the best node that has the smallest f value;
        /// </summary>
        /// <returns>The best Node.</returns>
        Location2D CheckBestNode() {
            var BestNode = f_scores.OrderBy(x => x.Value).First().Key;

            openset.Remove(BestNode);
            closedset.Add(BestNode);
            return BestNode;
        }

        private void ReconstructPathRecursive(Location2D current_node) {
            Location2D temp;
            if (came_from.TryGetValue(current_node, out temp)) {
                ReconstructPathRecursive(temp);
                path.Add(temp);
            }
        }

        int GetHValue(Location2D start, Location2D goal) {
            int nDx = Math.Abs(start.X - goal.X);
            int nDy = Math.Abs(start.Y - goal.Y);
            if (nDx > nDy)
                return 10 * nDx + 6 * nDy;
            return 10 * nDy + 6 * nDx;
        }

        readonly Double SQRT_2 = Math.Sqrt(2);

        protected  Double NeighborDistance(Location2D inStart, Location2D inEnd) {
            int diffX = Math.Abs(inStart.X - inEnd.X);
            int diffY = Math.Abs(inStart.Y - inEnd.Y);

            switch (diffX + diffY) {
                case 1: return 1;
                case 2: return SQRT_2;
                case 0: return 0;
                default:
                    throw new ApplicationException();
            }
        }

        public List<Location2D> Path {
            get {
                return path;
            }
        }
    }
}

for more info check A* search algorithm - Wikipedia, the free encyclopedia

how to use it , simply change Location2D to a Point or if you have your own location struct use it , so every map should have a PatherFinder , and so you can use it , I hope it help you guys , any suggestion to Improve it would be cool ;P.

[injection illusion logic]

im fine with struct and classes for nodes but it still awesome =) +k