///<summary>
///ダイクストラ法を用いた探索
/// </summary>
///
///<remarks>再起関数</remarks>
///
///<param name="nodes">移動候補先すべてのノード</param>
///<param name="fixedNode"> 確定ノードの保存先</param>
public void DoDijikstra(List <Node> nodes, ref Node fixedNode,
AgentBase agent, List <AgentBase> agents, bool narrow = false)
{
//自分を含めると無限ループの可能性がある
foreach (var node in nodes)
{
if (this == node)
{
continue;
}
//確定ノードの場合はさらにその先まで探索する
else if (node.NodeStatus == NodeKind.Determined)
{
if (node.NextNodes.Count() == 0)
{
break;
}
//確定したルート通りでないと探索が終わらない
if (node.PreviousNode != this)
{
continue;
}
//ダイクストラで探索
node.DoDijikstra(node.NextNodes, ref fixedNode, agent, agents);
//狭い通路の探索
node.DoDijikstra(node.NextNodesWithNarrow, ref fixedNode, agent, agents, true);
}
//確定ノードではないときはコスト計算を行う
else
{
//コスト計算
// TODO 距離以外も用いた計算の実装
double temporaryCost;
if (narrow)
{
temporaryCost = CalculateCostNarrow(node, agent);
}
else
{
temporaryCost = CalculateCost(node);
//ノード間の距離を水平に保つように回転させる
//Atan2では水平での角度を見ているため戻す場合はマイナスする
var theta = -Math.Atan2(this.Y - node.Y, this.X - node.X);
var Point = Rotate2D(node, this, theta);
Point.Y += 25;
var rect = new Rect(new Point(this.X, this.Y - 25), Point);
int count = 0;
foreach (var anotherAgent in agents)
{
if (anotherAgent == agent)
{
continue;
}
if (rect.Contains(Rotate2D(anotherAgent.Node, this, theta)))
{
count++;
}
}
if ((count * 100) / this.DistanceFromNode(node) > 2)
{
temporaryCost += count * 100;
}
}
if (fixedNode == null)
{
fixedNode = node;
node.DistanceCost = temporaryCost;
node.PreviousNode = this;
}
else if (fixedNode.DistanceCost > temporaryCost)
{
fixedNode = node;
node.DistanceCost = temporaryCost;
node.PreviousNode = this;
}
else if (node.DistanceCost > temporaryCost)
{
node.DistanceCost = temporaryCost;
node.PreviousNode = this;
}
}
}
}
