1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace Engine.PathFinding
{
    /// <summary>
    /// AStar require it's nodes to implement certain functions (as defined by IAStarNode)
    /// If would be nice if this data could just be added to the node class then removed after AStar
    /// has done it's thing but I don't know a nice way of doing this.
    /// </summary>
    /// <typeparam name="T">The nodes AStar will operate on.</typeparam>
    public class AStar<T> where T : IAStarNode<T>, new()
    {
        public delegate double CostHeuristic(T start, T end);

        CostHeuristic   _costHeuristic;
        List<T>      _openList   = new List<T>();
        List<T>      _closeList  = new List<T>();
        T            _goal;
        List<T>      _path       = new List<T>();

        public List<T> Path
        {
            get{ return _path; }
        }


        public AStar(CostHeuristic costHeuristic)
        {
            _costHeuristic = costHeuristic;
        }

        public void FindPath(T root, T goal)
        {
            _goal = goal;
            _openList.Clear();
            _closeList.Clear();

            T rootNode = root;
            _openList.Add(rootNode);

            ProcessPath();
        }

            private void ProcessPath()
            {
                bool goalHasBeenReached = false;
                while (goalHasBeenReached == false)
                {
                    _openList.OrderByDescending(x => x.Cost);
                    T current = _openList.FirstOrDefault();


                    if (current.Equals(_goal))
                    {

                        // Backtrace and return path.
                        T trace = _goal;
                        while (trace != null)
                        {
                            _path.Add(trace);
                            trace = trace.ParentNode;
                        }
                        return;
                    }

                    _openList.RemoveAt(0);
                    _closeList.Add(current);

                    foreach (T neighbour in current.Neighbours)
                    {
                        System.Diagnostics.Debug.Assert(neighbour.Equals(current) == false, "No node should be it's own neighbour.");

                        double cost = current.Cost + _costHeuristic(neighbour, _goal);

                        if (_openList.Contains(neighbour) && neighbour.Cost > cost)
                        {
                            _openList.Remove(neighbour);
                        }

                        if (_closeList.Contains(neighbour) && neighbour.Cost > cost)
                        {
                            _closeList.Remove(neighbour);
                        }

                        if (_closeList.Contains(neighbour) == false &&
                            _openList.Contains(neighbour) == false)
                        {
                            neighbour.Cost = cost;
                            neighbour.ParentNode = current;
                            _openList.Add(neighbour);
                        }
                    }
                }
            }

        }
    
}