bate's blog

調べたこと実装したことなどを取りとめもなく書きます。

unityで最短経路

A*を使った最短経路のサンプルを作った。
空のGameObjectにMain.csを追加して、ライトを追加する。

前に作ったA*のコードを少しだけ改良した。

Node.cs

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

public class Node : MonoBehaviour {

    public int id;
    public List<int> nearList;
    public Node parent;
    public float f;
    public float g;
    public float h;
    public bool done;
    public bool isBlock;

    private void Awake()
    {
	nearList = new List<int>();
        parent = null;
        f = g = h = 0f;
        done = false;
        isBlock = false;
    }

	public void Clear()
	{
		parent = null;
		f = g = h = 0f;
		done = isBlock = false;
	}

	public void SetBlock()
	{
		isBlock = true;
		GetComponent<Renderer>().material.color = Color.green;
	}
	public void SetPass()
	{
		GetComponent<Renderer>().material.color = Color.yellow;
	}
}

PathFinding.cs

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

public class PathFinding {

	public Dictionary<int, Node> nodeDictionary;
	public List<Node> openList;
	public  List<Node> closeList;
	public Node start;
	public Node goal;

	public PathFinding()
	{
		openList = new List<Node>();
		closeList = new List<Node>();
	}

	public void Clear()
	{
		openList.Clear();
		closeList.Clear();
		start = goal = null;
	}

	public bool Search(int startId, int goalId)
	{
		if (nodeDictionary == null || nodeDictionary.Count == 0)
		{
			return false;
		}
		if (startId == goalId)
		{
			return false;
		}
		Clear();
		start = nodeDictionary[startId];
		if (start == null)
		{
			return false;
		}
		goal = nodeDictionary[goalId];
		if (goal == null)
		{
			return false;
		}
		openList.Add(start);
		start.g = 0;
		start.h = Heuristic(start, goal);
		start.f = start.h;

		while (true)
		{
			if (openList.Count == 0)
			{
				Debug.Log("path not found");
				return false;
			}
			float minCost = float.MaxValue;
			Node minCostNode = null;
			foreach (Node searchNode in openList)
			{
				if (searchNode.done || searchNode.isBlock)
				{
					continue;
				}
				if (searchNode.f < minCost)
				{
					minCost = searchNode.f;
					minCostNode = searchNode;
				}
			}
			if (minCostNode == null)
			{
				return false;
			}
			if (minCostNode == goal)
			{
				return true;
			}
			minCostNode.done = true;
			closeList.Add(minCostNode);
			openList.Remove(minCostNode);
			UpdateCost(minCostNode);
		}
		return false;
	}
	private float Heuristic(Node a, Node b)
	{
		return Vector3.Distance(a.transform.position, b.transform.position);
	}
	private void UpdateCost(Node node)
	{
		foreach(int id in node.nearList)
		{
			Node nearNode = nodeDictionary[id];
			if (nearNode == null || nearNode.done || nearNode.isBlock)
			{
				continue;
			}
			CalculateCost(node, nearNode);
		}
	}
	private void CalculateCost(Node node, Node near)
	{
		float nearH = Heuristic(near, goal);
		float nearG = node.g + Heuristic(node, near);
		float nearF = nearH + nearG;
		if (openList.Contains(near))
		{
			if (nearF < near.f)
			{
				near.f = nearF;
				near.g = nearG;
				near.h = nearH;
				near.parent = node;
			}
		}
		else if (closeList.Contains(near))
		{
			if (nearF < near.f)
			{
				near.f = nearF;
				near.g = nearG;
				near.h = nearH;
				near.parent = node;
				openList.Add (near);
				closeList.Remove(near);
			}
		}
		else
		{
			near.f = nearF;
			near.g = nearG;
			near.h = nearH;
			near.parent = node;
			openList.Add (near);
			closeList.Remove(near);
		}
	}
}

Main.cs

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

public class Main : MonoBehaviour {

	PathFinding pathFinding = new PathFinding();
	Dictionary<int, Node> nodeDictionary = new Dictionary<int, Node>();
	List<GameObject> lineList = new List<GameObject>();

	// Use this for initialization
	void Start ()
	{
		BuildNode();
		BuildLine();
		pathFinding.nodeDictionary = nodeDictionary;
		if (!pathFinding.Search(1, 6))
		{
			Debug.Log("not found");
		}
		else
		{
			Node node = pathFinding.goal;
			while (node != null)
			{
				node.SetPass();
				node = node.parent;
			}
		}
	}

	private void BuildNode()
	{
		GameObject prefab = GameObject.CreatePrimitive(PrimitiveType.Cube);
		GameObject go = GameObject.Instantiate(prefab);
		go.transform.position = new Vector3(-8, 4, 0);
		Node node = go.AddComponent<Node>();
		node.id = 1;
		node.nearList.Add (2);
		node.nearList.Add (4);
		node.nearList.Add (5);
		nodeDictionary.Add(node.id, node);
		go.name = node.id.ToString();
		
		go = GameObject.Instantiate(prefab);
		go.transform.position = new Vector3(-4, 4, 0);
		node = go.AddComponent<Node>();
		node.id = 2;
		node.nearList.Add (1);
		node.nearList.Add (3);
		node.nearList.Add (5);
		nodeDictionary.Add(node.id, node);
		go.name = node.id.ToString();
		
		go = GameObject.Instantiate(prefab);
		go.transform.position = new Vector3(0, 4, 0);
		node = go.AddComponent<Node>();
		node.id = 3;
		node.nearList.Add (2);
		node.nearList.Add (5);
		node.nearList.Add (6);
		nodeDictionary.Add(node.id, node);
		go.name = node.id.ToString();
		
		go = GameObject.Instantiate(prefab);
		go.transform.position = new Vector3(-8, -4, 0);
		node = go.AddComponent<Node>();
		node.id = 4;
		node.nearList.Add (1);
		node.nearList.Add (5);
		nodeDictionary.Add(node.id, node);
		go.name = node.id.ToString();
		
		go = GameObject.Instantiate(prefab);
		go.transform.position = new Vector3(-4, -4, 0);
		node = go.AddComponent<Node>();
		node.id = 5;
		node.nearList.Add (1);
		node.nearList.Add (2);
		node.nearList.Add (3);
		node.nearList.Add (4);
		node.nearList.Add (6);
		nodeDictionary.Add(node.id, node);
		go.name = node.id.ToString();

		node.SetBlock();
		
		go = GameObject.Instantiate(prefab);
		go.transform.position = new Vector3(0, -4, 0);
		node = go.AddComponent<Node>();
		node.id = 6;
		node.nearList.Add (3);
		node.nearList.Add (5);
		nodeDictionary.Add(node.id, node);
		go.name = node.id.ToString();
		
		go = GameObject.Instantiate(prefab);
		go.transform.position = new Vector3(4, -4, 0);
		node = go.AddComponent<Node>();
		node.id = 7;
		node.nearList.Add (6);
		nodeDictionary.Add(node.id, node);
		go.name = node.id.ToString();

		GameObject.Destroy(prefab);
	}

	private void BuildLine()
	{
		int i = 1;
		GameObject prefab = new GameObject();
		GameObject go = GameObject.Instantiate(prefab);
		LineRenderer lr = go.AddComponent<LineRenderer>();
		lr.SetPosition(0, nodeDictionary[1].transform.position);
		lr.SetPosition(1, nodeDictionary[2].transform.position);
		lineList.Add(go);
		go.name = "Line"+i.ToString();

		++i;
		go = GameObject.Instantiate(prefab);
		lr = go.AddComponent<LineRenderer>();
		lr.SetPosition(0, nodeDictionary[1].transform.position);
		lr.SetPosition(1, nodeDictionary[4].transform.position);
		lineList.Add(go);
		go.name = "Line"+i.ToString();

		++i;
		go = GameObject.Instantiate(prefab);
		lr = go.AddComponent<LineRenderer>();
		lr.SetPosition(0, nodeDictionary[1].transform.position);
		lr.SetPosition(1, nodeDictionary[5].transform.position);
		lineList.Add(go);
		go.name = "Line"+i.ToString();

		++i;
		go = GameObject.Instantiate(prefab);
		lr = go.AddComponent<LineRenderer>();
		lr.SetPosition(0, nodeDictionary[2].transform.position);
		lr.SetPosition(1, nodeDictionary[5].transform.position);
		lineList.Add(go);
		go.name = "Line"+i.ToString();

		++i;
		go = GameObject.Instantiate(prefab);
		lr = go.AddComponent<LineRenderer>();
		lr.SetPosition(0, nodeDictionary[4].transform.position);
		lr.SetPosition(1, nodeDictionary[5].transform.position);
		lineList.Add(go);
		go.name = "Line"+i.ToString();

		++i;
		go = GameObject.Instantiate(prefab);
		lr = go.AddComponent<LineRenderer>();
		lr.SetPosition(0, nodeDictionary[2].transform.position);
		lr.SetPosition(1, nodeDictionary[3].transform.position);
		lineList.Add(go);
		go.name = "Line"+i.ToString();

		++i;
		go = GameObject.Instantiate(prefab);
		lr = go.AddComponent<LineRenderer>();
		lr.SetPosition(0, nodeDictionary[3].transform.position);
		lr.SetPosition(1, nodeDictionary[5].transform.position);
		lineList.Add(go);
		go.name = "Line"+i.ToString();

		++i;
		go = GameObject.Instantiate(prefab);
		lr = go.AddComponent<LineRenderer>();
		lr.SetPosition(0, nodeDictionary[3].transform.position);
		lr.SetPosition(1, nodeDictionary[6].transform.position);
		lineList.Add(go);
		go.name = "Line"+i.ToString();

		++i;
		go = GameObject.Instantiate(prefab);
		lr = go.AddComponent<LineRenderer>();
		lr.SetPosition(0, nodeDictionary[5].transform.position);
		lr.SetPosition(1, nodeDictionary[6].transform.position);
		lineList.Add(go);
		go.name = "Line"+i.ToString();

		++i;
		go = GameObject.Instantiate(prefab);
		lr = go.AddComponent<LineRenderer>();
		lr.SetPosition(0, nodeDictionary[6].transform.position);
		lr.SetPosition(1, nodeDictionary[7].transform.position);
		lineList.Add(go);
		go.name = "Line"+i.ToString();

		GameObject.Destroy(prefab);
	}
	
	// Update is called once per frame
	void Update () {
	
	}
}