/***************************************************************************/
public List<NodePlanning> ExecutePlanner()
{
World.WorldState CompleteWS = mWorld.mWorldState | mGymWorld.mGymWorldState;
FindPlan(CompleteWS, World.WorldState.WORLD_STATE_EXIT_GYM);
return mWorld.plan;
}
/***************************************************************************/
public NodePlanning(World.WorldState worldState, ActionPlanning action)
{
mWorldState = worldState;
mAction = action;
gCost = 0.0f;
hCost = 0.0f;
mParent = null;
}
public byte[] SerializeWorld(World.WorldState worldState)
{
return(ME.ECS.Serializer.Serializer.Pack(worldState));
}
/// <summary>
/// Deactivate a WorldState in our World.
/// </summary>
/// <param name="ws"></param>
public void UnsetWorldState(World.WorldState ws)
{
mGymWorldState = (mGymWorldState & (~ws));
}
/// <summary>
/// Check if there is a specific WorldState active in our World.
/// </summary>
/// <param name="ws"></param>
/// <returns></returns>
public bool IsWorldState(World.WorldState ws)
{
return((mGymWorldState & ws) != 0);
}
/// <summary>
/// Set the desired WorldState in our World.
/// </summary>
/// <param name="ws"></param>
public void SetWorldState(World.WorldState ws)
{
mGymWorldState = mGymWorldState | ws;
}
/***************************************************************************/
public List<NodePlanning> FindPlan(World.WorldState startWorldState, World.WorldState targetWorldState)
{
CurrentStartNode = new NodePlanning(startWorldState, null);
CurrentTargetNode = new NodePlanning(targetWorldState, null);
List<NodePlanning> openSet = new List<NodePlanning>();
HashSet<NodePlanning> closedSet = new HashSet<NodePlanning>();
openSet.Add(CurrentStartNode);
mWorld.openSet = openSet;
NodePlanning node = CurrentStartNode;
while (openSet.Count > 0 && ((node.mWorldState & CurrentTargetNode.mWorldState) != CurrentTargetNode.mWorldState))
{
// Select best node from open list
node = openSet[0];
for (int i = 1; i < openSet.Count; i++)
{
if (openSet[i].fCost < node.fCost || (openSet[i].fCost == node.fCost && openSet[i].hCost < node.hCost))
{
node = openSet[i];
}
}
// Manage open/closed list
openSet.Remove(node);
closedSet.Add(node);
mWorld.openSet = openSet;
mWorld.closedSet = closedSet;
// Check destination
if (((node.mWorldState & CurrentTargetNode.mWorldState) != CurrentTargetNode.mWorldState))
{
// Open neighbours
foreach (NodePlanning neighbour in mWorld.GetNeighbours(node))
{
if ( /*!neighbour.mWalkable ||*/ closedSet.Any(n => n.mWorldState == neighbour.mWorldState))
{
continue;
}
float newCostToNeighbour = node.gCost + GetDistance(node, neighbour);
if (newCostToNeighbour < neighbour.gCost || !openSet.Any(n => n.mWorldState == neighbour.mWorldState))
{
neighbour.gCost = newCostToNeighbour;
neighbour.hCost = Heuristic(neighbour, CurrentTargetNode);
neighbour.mParent = node;
if (!openSet.Any(n => n.mWorldState == neighbour.mWorldState))
{
openSet.Add(neighbour);
mWorld.openSet = openSet;
}
else
{
// Find neighbour and replace
openSet[openSet.FindIndex(x => x.mWorldState == neighbour.mWorldState)] = neighbour;
}
}
}
}
else
{
// Path found!
// End node must be copied
CurrentTargetNode.mParent = node.mParent;
CurrentTargetNode.mAction = node.mAction;
CurrentTargetNode.gCost = node.gCost;
CurrentTargetNode.hCost = node.hCost;
RetracePlan(CurrentStartNode, CurrentTargetNode);
Debug.Log("Statistics:");
Debug.LogFormat("Total nodes: {0}", openSet.Count + closedSet.Count);
Debug.LogFormat("Open nodes: {0}", openSet.Count);
Debug.LogFormat("Closed nodes: {0}", closedSet.Count);
}
}
// Log plan
//Debug.Log("PLAN FOUND!");
for (int i = 0; i < mWorld.plan.Count; ++i)
{
Debug.LogFormat("{0} Accumulated cost: {1}, Cost of the action: {2}", mWorld.plan[i].mAction.mName, mWorld.plan[i].gCost, mWorld.plan[i].mAction.mCost);
}
return mWorld.plan;
}
