///<summary>Initialize new instance of <see cref="CTNode{T}"/></summary>
///<param name="left">Left child</param>
///<param name="right">Right child</param>
///<param name="value">Value of current node</param>
public CTNode(CTNode <T> left, CTNode <T> right, CTNode <T> parent, T value)
{
Left = left;
Right = right;
Value = value;
Parent = parent;
}
/* get min cost node
* from list of nodes */
static CTNode GetMinCostNode(List <CTNode> nodes)
{
CTNode node = nodes[0];
for (int i = 1; i < nodes.Count; i++)
{
if (nodes[i].cost < node.cost)
{
node = nodes[i];
}
else if (nodes[i].cost == node.cost &&
nodes[i].GetConstraintsCount() < node.GetConstraintsCount())
{
node = nodes[i];
}
}
return(node);
}
///<summary>Set right child</summary>
///<param name="left"><see cref="CTNode{T}"/> to be assing as a <see cref="Right"/> child</param>
public void SetRight(CTNode <T> right)
{
Right = right;
}
///<summary>Set left child</summary>
///<param name="left"><see cref="CTNode{T}"/> to be assing as a <see cref="Left"/> child</param>
public void SetLeft(CTNode <T> left)
{
Left = left;
}
///<summary>Set parent of current node</summary>
///<param name="parent"><see cref="CTNode{T}"/> to be assigned as a <see cref="Parent"/> of current node</param>
public void SetParent(CTNode <T> parent)
{
Parent = parent;
}
IEnumerator StepCBS()
{
grid.paths = null;
List <CTNode> OPEN = new List <CTNode> ();
List <Conflict> curConflicts = new List <Conflict>();
/* empty constraints */
List <State>[] emptyCstr = new List <State> [nAgents];
for (int i = 0; i < emptyCstr.Length; i++)
{
emptyCstr[i] = new List <State>();
}
List <List <Node> > soln = GetSolution(emptyCstr);
int cost = GetSolutionCost(soln);
/* add root node */
CTNode curNode = new CTNode(emptyCstr, soln, cost);
OPEN.Add(curNode);
int nodesTraversed = 0;
while (OPEN.Count > 0)
{
nodesTraversed++;
curNode = GetMinCostNode(OPEN);
OPEN.Remove(curNode);
grid.paths = curNode.soln;
curConflicts = GetConflicts(curNode.soln);
// Debug.Log(curNode.cost);
yield return(0);
if (curConflicts.Count == 0)
{
Debug.Log("final->");
Debug.Log(curNode.cost);
break;
}
else if (curConflicts.Count > 0)
{
foreach (Conflict conflict in curConflicts)
{
foreach (int agentID in conflict.agents)
{
// copy constraints
List <State>[] newConstraints = new List <State> [nAgents];
for (int i = 0; i < newConstraints.Length; i++)
{
newConstraints[i] = new List <State>(curNode.cstr[i]);
}
// add new constraint
newConstraints[agentID].Add(new State(conflict.node, conflict.time));
// solve with new constraints
List <List <Node> > newSoln = GetSolution(newConstraints, curNode.soln, agentID);
int newCost = GetSolutionCost(newSoln);
// add new node
CTNode newNode = new CTNode(newConstraints, newSoln, newCost);
OPEN.Add(newNode);
}
}
}
}
if (testing && logTestResults)
{
Log(nodesTraversed.ToString() + ", " + curNode.cost);
}
if (AStar.Mode == AStar.MODE_VH)
{
AStar.Mode = AStar.MODE_N;
StartCoroutine(StepCBS());
}
else if (AStar.Mode == AStar.MODE_N)
{
StartCoroutine(FlyDrones());
}
}
