/// <summary>
/// 最短経路を探す.
/// </summary>
/// <returns>The saitan.</returns>
/// <param name="nodeMap">Node map.</param>
/// <param name="startKey">Start key.</param>
/// <param name="goalKey">Goal key.</param>
/// <param name="loopout">計算を諦めるしきい値</param>
/// <param name="costDict">Cost dict.</param>
/// <param name="route">Route.</param>
public static int GetShortestRoute(NodeMap nodeMap, string startKey, string goalKey, int loopout, out CostDict costDict, out LinkedList <string> route)
{
costDict = new CostDict(nodeMap.nodeMap.Count);
var stack = new SortedSet <CostDataForStack>(new CostDataForStack.myReverserClass());
var startNode = nodeMap.nodeMap[startKey];
var goalNode = nodeMap.nodeMap[goalKey];
{
var openNodeCostData = new CostData
{
cost = 0.0f,
heuristicCost = Vector3.Distance(startNode.Position, goalNode.Position),
parent = null
};
costDict[startKey] = openNodeCostData;
CostDataForStack costDataForStack = new CostDataForStack
{
cost = openNodeCostData.cost,
score = openNodeCostData.cost + openNodeCostData.heuristicCost,
nodeName = startKey
};
stack.Add(costDataForStack);
}
while (stack.Count != 0 && 0 < loopout)
{
loopout--;
var parent = stack.First();
stack.Remove(parent);
var parentName = parent.nodeName;
var parentNode = nodeMap.nodeMap[parentName];
var parentCost = costDict[parentName];
foreach (var openNodeName in parentNode.Graph)
{
if (costDict.ContainsKey(openNodeName))
{
continue;
}
var openNode = nodeMap.nodeMap[openNodeName];
var openNodeCost = parentCost.cost + Vector3.Distance(openNode.Position, parentNode.Position);
var hCost = Vector3.Distance(openNode.Position, goalNode.Position);
var openNodeCostData = new CostData
{
cost = openNodeCost,
heuristicCost = hCost,
parent = parentName
};
var costDataForStack = new CostDataForStack
{
cost = openNodeCostData.cost,
score = openNodeCostData.cost + openNodeCostData.heuristicCost,
nodeName = openNodeName
};
costDict[openNodeName] = openNodeCostData;
stack.Add(costDataForStack);
CostDataForStack.Dump(costDataForStack);
}
if (costDict.ContainsKey(goalKey))
{
break;
}
}
if (costDict.ContainsKey(goalKey))
{
route = new LinkedList <string>();
var node = costDict[goalKey];
while (node.parent != null)
{
route.AddFirst(node.parent);
node = costDict[node.parent];
}
return(route.Count);
}
else
{
route = null;
return(0);
}
}
public static void Dump(CostDataForStack costDataForStack)
{
// Debug.Log(string.Format("Name: {0}, Cost: {1}, score:{2}", costDataForStack.nodeName, costDataForStack.cost, costDataForStack.score));
}
