Exemplo n.º 1
0
    public List <shota_NodePoint> Astar(shota_NodePoint StartNode, shota_NodePoint GoalNode)
    {
        int  cost    = 0;                                                 // 実コスト
        bool isGoal  = false;                                             // ゴールを発見しているか
        bool isStart = false;                                             // 開始地点まで遡れたか
        int  index   = 0;                                                 // 遡るための添字
        List <shota_NodePoint> astar_root = new List <shota_NodePoint>(); // 出た最短経路をこのリストに入れる
        List <shota_NodePoint> openlist   = new List <shota_NodePoint>(); // 今探査中のノードを格納するリスト

        // スタートノードのコスト設定
        StartNode.SetMoveToTargetcost(cost);
        // 通過コストと実コストを加算してスコアを算出
        StartNode.CalcTotalCost();
        // 探査範囲を広げる度にコストを増やす
        cost++;
        // 一通りの設定が完了し、探査範囲を広げる際自身の状態を探査終了に更新する
        StartNode.SetNodeStatus(shota_NodePoint.NodeStatus.CLOSE);

        // 隣接するノードの親に自身を設定しつつ、状態を探査中に変更し、探査中リストに入れる
        for (int i = StartNode.GetConnectNode().Count - 1; i >= 0; i--)
        {
            if (StartNode.GetNodePos() == GoalNode.GetNodePos())
            {
                // スタート地点がゴールならそのまま返す
                astar_root.Add(StartNode);
                return(astar_root);
                // 意味不明なので解説
                // 現在のルートの「次の目的地」を開始地点としているので、そこにゴール地点が設定されたらそのまま進ませる
            }

            StartNode.GetConnectNode()[i].SetParentNode(StartNode);

            // ゴールチェック
            if (GoalNode.GetNodePos() == StartNode.GetConnectNode()[i].GetNodePos())// ゴール地点だった場合
            {
                astar_root.Add(StartNode.GetConnectNode()[i]);
                isGoal = true;
                break;
            }

            StartNode.GetConnectNode()[i].SetNodeStatus(shota_NodePoint.NodeStatus.OPEN);
            StartNode.GetConnectNode()[i].SetMoveToTargetcost(cost);
            StartNode.GetConnectNode()[i].CalcTotalCost();
            openlist.Add(StartNode.GetConnectNode()[i]);
        }

        while (!isGoal)
        {
            // ループが逆順で回しているのでスコアソートも降順で行う
            openlist.Sort((a, b) => b.GetTotalCost() - a.GetTotalCost());
            for (int i = openlist.Count - 1; i >= 0; i--)
            {
                // ゴールチェック
                if (GoalNode.GetNodePos() == openlist[i].GetNodePos())// ゴール地点だった場合
                {
                    astar_root.Add(openlist[i]);
                    isGoal = true;
                    break;
                }
                else // 通常処理
                {
                    if (openlist[i].GetConnectNode() != null)
                    {
                        for (int j = openlist[i].GetConnectNode().Count - 1; j >= 0; j--)
                        {
                            // 未探査か確認
                            if (openlist[i].GetConnectNode()[j].GetNodeStatus() != shota_NodePoint.NodeStatus.CLOSE &&
                                openlist[i].GetConnectNode()[j].GetNodeStatus() != shota_NodePoint.NodeStatus.OPEN)
                            {
                                // 探査中に更新
                                openlist[i].GetConnectNode()[j].SetNodeStatus(shota_NodePoint.NodeStatus.OPEN);
                                // 実コストを設定しスコアを算出する
                                openlist[i].SetMoveToTargetcost(cost);
                                openlist[i].CalcTotalCost();
                                // 未探査であるなら親に自身を設定
                                openlist[i].GetConnectNode()[j].SetParentNode(openlist[i]);
                                // 探査中リストに追加
                                openlist.Add(openlist[i].GetConnectNode()[j]);
                            }
                        }
                    }
                    // 拡大完了次第自身の状態を探索済みに設定
                    openlist[i].SetNodeStatus(shota_NodePoint.NodeStatus.CLOSE);
                    // 設定後探査中リストから除外
                    openlist.RemoveAt(i);
                }
            }
            cost++;
        }

        // ゴールを見つけたので経路をリストに追加していく
        while (!isStart)
        {
            astar_root.Add(astar_root[index].GetParentNode());
            index++;
            // 開始ノードと確認出来たら(ここでは座標を比較)
            if (astar_root[index].GetNodePos() == StartNode.GetNodePos())
            {
                // ゴール地点 -> スタート地点 の順で追加されているので反転させる
                astar_root.Reverse();
                // 出た経路を返す
                return(astar_root);
            }
        }

        return(null); // 想定外のミスがある場合nullを投げる
    }
Exemplo n.º 2
0
 private void SetNodePropertie(shota_NodePoint node, int cost)
 {
     node.SetNodeStatus(shota_NodePoint.NodeStatus.OPEN);
     node.SetMoveToTargetcost(cost);
     node.CalcTotalCost();
 }