WSHCommon/Common/CommonAI/RTS/PathFinder.cs

using CommonLang;
using System;
using System.Collections.Generic;
using System.Text;

namespace CommonAI.RTS
{
    public interface ITerrain<T> : IDisposable where T : MapNode
    {
        void ForEachNodes(Action<T> action);
        int TotalNodeCount { get; }
    }

    public abstract class MapNode : IDisposable
    {
        internal TempMapNode tempNode;
        
        /// <summary>
        /// 临近节点
        /// </summary>
        public abstract MapNode[] Nexts { get; }

        /// <summary>
        /// 测试通过
        /// </summary>
        /// <param name="other"></param>
        /// <returns></returns>
        public abstract bool TestCross(MapNode other);

        /// <summary>
        /// g(n) 是在状态空间中从初始节点到n节点的实际代价。值越大，优先级越低。
        /// </summary>
        /// <param name="target"></param>
        /// <returns></returns>
        public abstract float GetG(MapNode target);

        /// <summary>
        /// h(n) 是从n到目标节点最佳路径的估计代价。值越大，优先级越低。
        /// </summary>
        /// <param name="father"></param>
        /// <returns></returns>
        public abstract float GetH(MapNode father);

        /// <summary>
        /// 销毁节点
        /// </summary>
        public abstract void Dispose();

    }

    //-----------------------------------------------------------------------------------------------------------------

    public abstract class WayPoint : IDisposable
    {
        public object Tag { get; set; }

        public MapNode node { get; private set; }
        public WayPoint next { get; private set; }
        public WayPoint prev { get; private set; }
        public WayPoint tail
        {
            get
            {
                WayPoint wp = this;
                while (wp.next != null) { wp = wp.next; }
                return wp;
            }
        }
        /// <summary>
        /// 从当前节点开始算总长
        /// </summary>
        public int count
        {
            get
            {
                int ret = 0;
                WayPoint wp = this;
                while (wp != null) { ret++; wp = wp.next; }
                return ret;
            }
        }

        public WayPoint(MapNode map_node)
        {
            this.node = map_node;
        }
        public virtual void Dispose()
        {
            this.node = null;
            this.next = null;
            this.prev = null;
        }
        public abstract WayPoint Clone();

        public void LinkNext(WayPoint n)
        {
            if (this.next != null)
            {
                this.next.prev = null;
            }
            this.next = n;
            if (n != null)
            {
                n.prev = this;
            }
        }
    }

    //-----------------------------------------------------------------------------------------------------------------

    /// <summary>
    /// 抽象的A*寻路算法，子类可以自定义实现如何寻路。
    ///  @author WAZA
    /// </summary>
    public abstract class RTSAstar<T> : Disposable where T : MapNode
    {
        private ITerrain<T> terrain;
        private TempMapNodeList.OpenList src_open_list;
        private TempMapNodeList.CloseList src_close_list;

        public int TotalNodeCount { get { return terrain.TotalNodeCount; } }

        public void Init(ITerrain<T> map)
        {
            this.terrain = map;
            this.src_open_list = new TempMapNodeList.OpenList();
            this.src_close_list = new TempMapNodeList.CloseList();
            terrain.ForEachNodes((node) =>
            {
                var temp = new TempMapNode(node);
                node.tempNode = temp;
            });
        }

        protected override void Disposing()
        {
            terrain.ForEachNodes((node) =>
            {
                node.tempNode.Dispose();
            });
            terrain.Dispose();
            terrain = null;
            src_open_list = null;
            src_close_list = null;
        }
        
        public abstract WayPoint GenWayPoint(MapNode node);
        
        public WayPoint findPath(MapNode src_node, MapNode dst_node)
        {
            return findPath(src_node.tempNode, dst_node.tempNode);
        }

        protected WayPoint findPath(TempMapNode src_node, TempMapNode dst_node)
        {
            WayPoint head = GenWayPoint(src_node.data);

            if (src_node.data.Equals(dst_node.data))
            {
                return head;
            }

            src_open_list.clear();
            src_close_list.clear();

            src_node.setFather(src_node, dst_node);
            src_open_list.add(src_node);

            do
            {
                // search min F
                TempMapNode cur_node = src_open_list.getMinF();
                // put the min F to closed
                src_open_list.remove(cur_node);
                src_close_list.add(cur_node);

                // find next node
                for (int i = cur_node.data.Nexts.Length - 1; i >= 0; --i)
                {
                    TempMapNode near = cur_node.data.Nexts[i].tempNode;
                    if (!near.data.TestCross(cur_node.data))
                    {
                        continue;
                    }
                    // ignore what if the block can not across or already in close table
                    if (src_close_list.contains(near))
                    {
                        continue;
                    }
                    // push and if is not in open table
                    if (!src_open_list.contains(near))
                    {
                        near.setFather(cur_node, dst_node);
                        src_open_list.add(near);
                    }
                }

                // stop when :
                // 1. dst node already in close list
                if (cur_node.data.Equals(dst_node.data))
                {
                    // finded the path
                    WayPoint end = null;//GenWayPoint(dst_node.data);
                    for (int i = terrain.TotalNodeCount - 1; i >= 0; i--)
                    {
                        // linked to head
                        if (cur_node.data.Equals(src_node.data) || (cur_node.s_father == cur_node))
                        {
                            head.LinkNext(end);
                            break;
                        }
                        else
                        {
                            WayPoint next = GenWayPoint(cur_node.data);
                            next.LinkNext(end);
                            end = next;
                        }
                        cur_node = cur_node.s_father;
                    }
                    break;
                }
                // 2. open list is empty
                if (src_open_list.isEmpty())
                {
                    // not find the path
                    break;
                }

            } while (true);

            return head;
        }

    }

    //---------------------------------------------------------------------------------------------------------
    public class TempMapNode : IDisposable
    {
        /**对应的地图数据*/
        public MapNode data { get; private set; }

        /**每次寻路时，暂存的上一个节点*/
        internal TempMapNode s_father = null;
        // 当前所在的列表 //
        internal TempMapNodeList m_curList = null;
        internal TempMapNode m_prev = null;
        internal TempMapNode m_next = null;

        private float s_g = 0;
        private float s_h = 0;
        private float s_f = 0;

        internal float F { get { return s_f; } }

        internal TempMapNode(MapNode data)
        {
            this.data = data;
        }
        /// <summary>
        /// F = G + H
        /// 这里：
        /// </summary>
        /// <param name="father"></param>
        /// <param name="target"></param>
        internal void setFather(TempMapNode father, TempMapNode target)
        {
            this.s_father = father;
            this.s_g = father.s_g + data.GetG(target.data);
            this.s_h = data.GetH(father.data);
            this.s_f = (s_g + s_h);
        }

        public void Dispose()
        {
            data = null;
            s_father = null;
            m_curList = null;
            m_prev = null;
            m_next = null;
        }

    }
    //---------------------------------------------------------------------------------------------------------
    internal abstract class TempMapNodeList
    {
        private TempMapNode head = null;
        private TempMapNode last = null;

        internal class OpenList : TempMapNodeList
        {
        }
        //--------------------------------------------------------------------------
        internal class CloseList : TempMapNodeList
        {
        }
        //--------------------------------------------------------------------------
        internal TempMapNodeList()
        {

        }
        internal bool isEmpty()
        {
            return head == null;
        }
        internal bool contains(TempMapNode node)
        {
            return node.m_curList == this;
        }
        internal virtual TempMapNode getMinF()
        {
            float min = float.MaxValue;
            TempMapNode ret = null;
            for (TempMapNode a = head; a != null; a = a.m_next)
            {
                //float v = a.F;
                if (min > a.F)
                {
                    ret = a;
                    min = a.F;
                }
            }
            return ret;
        }
        internal virtual void add(TempMapNode node)
        {
            if (node.m_curList == null)
            {
                if (last == null)
                {
                    head = last = node;
                }
                else
                {
                    last.m_next = node;
                    node.m_prev = last;
                    last = node;
                }
                node.m_curList = this;
            }
            else
            {
                throw new Exception("Node is already in a List !");
            }
        }
        internal virtual void remove(TempMapNode node)
        {
            if (node.m_curList == this)
            {
                if (head == node)
                {
                    head = node.m_next;
                }
                if (last == node)
                {
                    last = node.m_prev;
                }
                if (node.m_next != null)
                {
                    node.m_next.m_prev = node.m_prev;
                }
                if (node.m_prev != null)
                {
                    node.m_prev.m_next = node.m_next;
                }
                node.m_next = null;
                node.m_prev = null;
                node.m_curList = null;
            }
            else
            {
                throw new Exception("Node is not contains in this list !");
            }
        }
        internal virtual void clear()
        {
            if (head != null)
            {
                for (TempMapNode i = head; i != null; i = i.m_next)
                {
                    i.m_curList = null;
                }
                TempMapNode p = head;
                TempMapNode q = null;
                do
                {
                    q = p.m_next;
                    p.m_next = null;
                    p = q;
                }
                while (p != null);
                p = last;
                do
                {
                    q = p.m_prev;
                    p.m_prev = null;
                    p = q;
                }
                while (p != null);
                this.head = null;
                this.last = null;
            }
        }

    }
    //---------------------------------------------------------------------------------------------------------
}