簡単コードで光線飛ばして経路探索(3)

実際にクマさんを経路探索で見つけたパスにそって動かしました。

Vector3 next = Navi.GetNextPath(transform.position, goal);

現在地、ゴールを引数で渡すと、次のポイントの座標を返値で返してきます。
次のポイントにつくと、次のポイントを返します。
渡す座標はポイントの位置である必要はありません。
渡した座標をポイントとして、新たに経路探索します。

ここでは、MyStartを開始位置、myGoalをゴール位置にしている。startとgoalをいったりきたりする。

using UnityEngine;
using System.Collections;

public class Teddy : MonoBehaviour {

    Vector3 goal;
    Vector3 next;

    bool flag;
    void Start () {
        transform.position = GameObject.Find("myStart").transform.position;
        goal = GameObject.Find("myGoal").transform.position;
        next = transform.position;
        flag = true;
    }

    void Update () {
        if (Vector3.Distance(goal, transform.position) < 0.3f)
        {
            if (flag == true)
            {
                goal = GameObject.Find("myStart").transform.position; flag = false;
            }
            else
            {
                goal = GameObject.Find("myGoal").transform.position;flag = true;
            }

            next = Navi.GetNextPath(transform.position, goal);
        }
        if ( Vector3.Distance(next,transform.position) < 0.3f)
            next = Navi.GetNextPath(transform.position, goal);

        transform.rotation = Quaternion.LookRotation(next - transform.position);
        transform.position += transform.forward * Time.deltaTime *4f;
	}
}

LineRendereやCanvasへの情報出力などを除いた経路探索コード。
とりあえずポイントは100個まで。
探索深さ10までなので、スタートからゴールまで10point経由までは探索できるはず。
速度を求めるなら、解が見つかった時点で探索打ち切りでもいいと思う。
深さを１深くしても距離が変わらないなら打ち切りでもいいかも？
ただ、ノードの数と距離は無関係なので、ノードの数はめっちゃ増えるけど、物理的にすごく近い経路を真面目に見つけようと思うとそれなりに深さは必要と思う。
ま、最短経路じゃなくていいと思うけどね。
あと、障害物はLayerがWaterという前提でLayerMaskにしています。
現状、経路が見つからない場合は、nextの座標が現在と同じ座標を返す。

using UnityEngine;
using System.Collections;
using System.Collections.Generic;
using UnityEngine.UI;

public class Navi : MonoBehaviour {
    static List<GameObject> wp = new List<GameObject>();
    static Dictionary<string,int> dic = new Dictionary<string, int>();
    static bool[,] path = new bool[100, 100];
    static int[] history = new int[100];
    static int[] goalHistory = new int[100];
    static string ret;
    static float minDist = 999f;

    void Start () {
        int no =0;
        foreach (GameObject n in GameObject.FindGameObjectsWithTag("Finish"))
        {
            dic.Add( n.name,no++);
            wp.Add(n);
        }
    }

    static void CalcPathBase()
    {
        for (int i = 0; i < 100; i++) for (int j = 0; j < 100; j++) path[i, j] = false;
        makePath();

        string sou = "Now";
        string dis = "Goal";

        for (int i = 2; i < 10; i++)
        {
            minDist = 999f; goalHistory[0] = -1;

            search(dic[sou], dic[dis], 0, i);
        }
    }

    public static Vector3 GetNextPath(Vector3 pos,Vector3 goal)
    {
        if (Vector3.Distance(pos, goal) < 0.2f) return goal;

        GameObject.Find("Now").transform.position=pos;
        GameObject.Find("Goal").transform.position =goal;
        CalcPathBase();

        if (goalHistory[0] == -1) return pos;
        return wp[goalHistory[1]].transform.position;
    }

    static void search(int sp,int dp,int depth,int limit)
    {
        if (depth >= limit) { return; }

        history[depth] = sp;
        if (path[sp, dp] == true)
        {
            history[depth + 1] = dp;
            float dist = CalcDistance(depth + 1);
            if (dist < minDist)
            {
                for (int j = 0; j <= depth + 1; j++) goalHistory[j] = history[j];
                minDist = dist;
            }
            return;
        }

        for (int i=0;i<100;i++)
        {
            if (sp == i) continue;
            if( path[sp,i]==true)
            {
                bool flag = false;
                for (int j = 0; j < depth + 1; j++) if (history[j] == i) { flag = true; break; }
                if (flag) continue;
                search(i, dp,depth+1,limit);
            }
        }
    }
    static float CalcDistance(int depth)
    {
        float dist = 0;
        for (int j = 1; j <= depth; j++)
            dist += Vector3.Distance(wp[history[j-1]].transform.position, wp[history[j]].transform.position);
        return dist;
    }

    static void makePath()
    {
        RaycastHit hit;
        int layerMask = LayerMask.GetMask("Water");

        for (int i=0;i<wp.Count;i++)
        {
            for (int j = i+1; j < wp.Count; j++)
            {
                Vector3 sp = wp[i].transform.position;
                Vector3 dp = wp[j].transform.position;
                if (!Physics.Linecast(sp, dp, out hit, layerMask))
                    path[i, j] = path[j, i] = true;
            }
        }
    }
}

　反復深化を15まで増やして、１深くしても探索結果の経路距離が変わらなかった場合は反復深化を打ち切るように修正。

        for (int i = 4; i < 15; i++)
        {
            float oldDist = minDist;
            minDist = 999f; goalHistory[0] = -1;

            search(dic[sou], dic[dis], 0, i);
            if (minDist<999f && oldDist == minDist) break;
        }

実行時にリアルタイムに経路生成・探索を行っているので、実行時に壁を動かせるし、増やせる。経路ノードを動かしたり、追加したり、削除することもできるはず。

次のノードについたときに再探索するので、壁を動かした場合は、その場で再探索をしないと壁移動に気づかないタイミングができてしまう。
次のノードに移動中に、さらに次のノードに光線を飛ばして直接行けるなら、そっちに行くみたいな処理を入れれば、動的な障害変更に強い探索処理になると思う。