void SearchPath(Node curNode)
{
U.d("SearchPath", curNode);
curNode.Closed = true;
Collider[] colliders = NodeContainer.GetAroundBlocks(curNode);
if (colliders.Length == 0)
{
return;
}
Node minFitnessNode = colliders[0].GetComponent <Node>();
for (int i = 0; i < colliders.Length; i++)
{
Node node = colliders[i].gameObject.GetComponent <Node>();
if (node != null)
{
if (curNode == node || node.Closed == true)
{
continue;
}
var disFromCur = Vector3.Distance(curNode.transform.position, node.transform.position);
var disToEnd = Vector3.Distance(NodeContainer.EndNode.transform.position, node.transform.position);
node.goal = curNode.fitness + disFromCur;
node.heuristic = disToEnd;
node.fitness = node.goal + node.heuristic;
if (minFitnessNode.fitness > node.fitness)
{
minFitnessNode = node;
}
node.SetColor(Color.gray);
}
}
minFitnessNode.Closed = true;
minFitnessNode.SetColor(Color.blue);
_curNode = minFitnessNode;
if (_curNode == NodeContainer.EndNode)
{
_curNode.SetColor(Color.green);
_curNode = null;
return;
}
}
void SearchPath(Node curNode)
{
_openNodes.Remove(_curNode);
_closedNodes.Add(_curNode);
// 주변 노드를 찾아온다.
Collider[] colliders = NodeContainer.GetAroundBlocks(curNode);
if (colliders.Length == 0)
{
return;
}
for (int i = 0; i < colliders.Length; i++)
{
Node node = colliders[i].gameObject.GetComponent <Node>();
if (node != null)
{
// 현재 노드, 블록이라면 무시
if (curNode == node || node.IsBlock)
{
continue;
}
// 이미 탐색 완료된 노드라면 무시
bool isClosedNode = _closedNodes.Any <Node>((eleNode) => node == eleNode);
if (isClosedNode)
{
continue;
}
// 이미 오픈리스트에 있는 노드라면
bool isOpenNode = _openNodes.Any <Node>((eleNode) => node == eleNode);
if (isOpenNode)
{
// 누적 경로 비용을 현재 노드를 부모라 가정하고 다시 계산해서
// 비용이 더 낮다면 부모를 현재 노드로 변경하고 비용을 전부 갱신
var disFromCur = Vector3.Distance(curNode.transform.position, node.transform.position);
var disToEnd = Vector3.Distance(NodeContainer.EndNode.transform.position, node.transform.position);
if (node.goal > curNode.goal + disFromCur)
{
node.goal = curNode.goal + disFromCur;
node.heuristic = disToEnd;
node.fitness = node.goal + node.heuristic;
node.SetParent(curNode);
}
continue;
}
// 처음 발견한 노드라면
{
// 비용 계산
var disFromCur = Vector3.Distance(curNode.transform.position, node.transform.position);
var disToEnd = Vector3.Distance(NodeContainer.EndNode.transform.position, node.transform.position);
node.goal = curNode.goal + disFromCur;
node.heuristic = disToEnd;
node.fitness = node.goal + node.heuristic;
// 부모 설정
node.SetParent(curNode);
// 오픈 리스트에 추가
_openNodes.Add(node);
node.SetColor(Color.gray);
}
}
}
// 오픈 리스트에서 최적화 노드 찾기
Node fitnessNode = null;
foreach (Node node in _openNodes)
{
if (fitnessNode == null)
{
fitnessNode = node;
}
else if (fitnessNode.fitness > node.fitness)
{
fitnessNode = node;
}
}
if (fitnessNode == curNode || fitnessNode == null)
{
// 오픈 노드가 없다. 종료
U.d("Path Not Fount!!!");
_curNode = null;
return;
}
fitnessNode.SetColor(Color.blue);
_curNode = fitnessNode;
if (_curNode == NodeContainer.EndNode)
{
// 목표 지점에 도착했다.
_curNode.SetColor(Color.green);
_curNode = null;
// 목표지점에서 역으로 부모노드를 찾아가며 최종 경로 저장
SavePath();
return;
}
}