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

もうちょっと複雑な経路にしてみました。

navi5からNavi6に行きます。画像の球にナンバーが振ってないので分かりにくいですね(´・ω・`)

反復深化６回目から解が得られてますね。dist=60.6

初め、下から回っていく経路を先に見つけて、次に真ん中の道を通る経路を見つけて、こっちが近いので確定
Start:5-> 9-> 8-> 2-> 10-> 11-> 6:Goal

反復深化９回目で、dist=60.648
６回目以降変わりなし

こうゆう経路の方が分岐数が増えますね。

pointを空中に増やしてみた。
壁を飛び越えたパスを最短経路として発見。
自明といえば自明な感じですが(´・ω・`)

任意の場所から任意の場所に行くには、まず一番近いポイントから、ゴールに一番近いポイントへの経路を見つけて
近いポイントに移動→経路を移動→ゴールに近いポイントからゴールまでは徒歩

みたいな、電車に乗って、駅からは歩きますみたいな感じですが、ここで問題点は、一番近いポイントから経路という電車に乗るのが実は遠回りになる場合があります。
最短距離のポイントではなく、ちょっと離れたポイントの方がゴールに近い場合は、最寄りのゴールに向かうのは逆戻りになるパターンです。
これは現在地からポイントへの距離も含めて探索すればOKかな？

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

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

    void Start () {
        wp = new List<GameObject>();

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

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

        string sou = "Navi (5)";
        string dis = "Navi (6)";
        ret = "search " + sou + " ->  " + dis + " \n";

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

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

            ret += "#"+i+" result:";
            for (int j = 0; j < 100; j++)
            {
                if (goalHistory[j] == -1) break;
                if (goalHistory[j] == dic[dis])
                {
                    ret += wp[goalHistory[j]].name+":GOAL dist=" + minDist;
                    break;
                }
                else
                    ret += wp[goalHistory[j]].name + "->";
            }
            ret += "\n\n";
        }
        GameObject.Find("/Canvas/Text").GetComponent<Text>().text = ret;
    }

    void Update () {
	}

    void search(int sp,int dp,int depth,int limit)
    {
        if (depth >= limit) { return; }// indent(depth); ret += "->limit\n"; return; }

        history[depth] = sp;
        if (path[sp, dp] == true)
        {
            history[depth + 1] = dp;
            float dist = CalcDistance(depth + 1);
//            ret += wp[sp].name+"->" + wp[dp].name + ":GOAL dist="+dist+"\n";
            if (dist < minDist)
            {
                for (int j = 0; j <= depth + 1; j++) goalHistory[j] = history[j];
                minDist = dist;
                ret += "    ";
                for (int j = 0; j < depth+1; j++) ret += wp[goalHistory[j]].name+"->";
                ret += ":GOAL dist="+minDist+"\n";
            }
            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;

//                ret += wp[sp].name+"->";
//                ret += " !";
//                for (int j = 0; j < depth+1; j++) ret += wp[history[j]].name+"|";
                search(i, dp,depth+1,limit);
//                indent(depth);
            }
        }
//        ret += "none\n";
    }
    void indent(int depth)
    {
        ret += "  ";
        for (int i = 0; i < depth; i++) ret += "    ";
    }
    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);
            //ret += wp[history[j - 1]].name;
        }
        return dist;
    }

    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;
                Vector3 dir = (dp-sp).normalized;
//              if (!Physics.SphereCast(sp,0.5f,dir,out hit, 100f,layerMask))
                if (!Physics.Linecast(sp, dp, out hit, layerMask))
                {
                    Debug.Log(wp[i].name + "->" + wp[j].name + "=OK");
                    path[i, j] = path[j, i] = true;
                    GameObject gm =Instantiate((GameObject)Resources.Load("point"), sp, Quaternion.identity) as GameObject;
                    LineRenderer LR;
                    LR = gm.GetComponent<LineRenderer>();
                    LR.enabled = true;
                    LR.SetWidth(.2f, .2f);
                    LR.SetVertexCount(2);
                    LR.SetPosition(0, sp);
                    LR.SetPosition(1, dp);
                }
                else
                {
                    path[i, j] = path[j, i] = false;
                    Debug.Log(wp[i].name + "->" + wp[j].name + "=NG");
                }
            }
        }
    }
}