/***************************************************************************/
public NodePlanning(World.WorldState worldState, ActionPlanning action)
{
mWorldState = worldState;
mAction = action;
gCost = 0.0f;
hCost = 0.0f;
mParent = null;
}
/***************************************************************************/
public NodePlanning(WorldState worldState, PlanningAction action)
{
mWorldState = new WorldState(worldState);
mAction = action;
gCost = 0.0f;
hCost = 0.0f;
mParent = null;
}
/***************************************************************************/
float Heuristic(NodePlanning nodeA, NodePlanning nodeB)
{
// Heuristic function
float originalH = -World.PopulationCount((int)(nodeA.mWorldState | nodeB.mWorldState))
- World.PopulationCount((int)(nodeA.mWorldState & nodeB.mWorldState));
originalH -= 600 * Mathf.Sqrt(Mathf.Abs(originalH));
float newH = originalH;
return newH;
}
/***************************************************************************/
float GetDistance(NodePlanning nodeA, NodePlanning nodeB)
{
// Distance function
/*if(nodeA.mAction != null & nodeB.mAction != null)
{
Debug.LogWarning("DISTANCE COST: " + (nodeB.mAction.mCost - nodeA.mAction.mCost));
return nodeB.mAction.mCost - nodeA.mAction.mCost;
}*/
//Debug.LogWarning("Distance Value = " + nodeB.mAction.mCost);
return nodeB.mAction.mCost;
}
/***************************************************************************/
void RetracePlan(NodePlanning startNode, NodePlanning endNode)
{
List <NodePlanning> plan = new List <NodePlanning>();
NodePlanning currentNode = endNode;
while (currentNode != startNode)
{
plan.Add(currentNode);
currentNode = currentNode.mParent;
}
plan.Reverse();
mWorld.plan = plan;
}
public void ApplyBackwardEffects(NodePlanning node, WorldState world, PlanningAction action)
{
switch (action.mActionType)
{
case PlanningAction.ActionType.AT_PICK_UP:
world.ingredientsKept.Remove(action.mIngredient);
break;
case PlanningAction.ActionType.AT_GO_TO:
Vector3 ingredientPos = FindIngredientOfType(action.mIngredient);
node.mAction.mCost = (ingredientPos - mWorldState.cPos).magnitude;
mWorldState.cPos = ingredientPos;
world.ingredientsVisited.Remove(action.mIngredient);
break;
}
}
public List <NodePlanning> GetNeighboursBackward(NodePlanning node)
{
List <NodePlanning> neighbours = new List <NodePlanning>();
foreach (PlanningAction action in mActionList)
{
// If preconditions are met we can apply effects and the new state is valid
if (MeetConditions(node.mWorldState, action))
{
// Apply action and effects
NodePlanning newNodePlanning = new NodePlanning(node.mWorldState, action);
ApplyBackwardEffects(newNodePlanning, newNodePlanning.mWorldState, action);
neighbours.Add(newNodePlanning);
}
}
return(neighbours);
}
/***************************************************************************/
public List <NodePlanning> GetNeighbours(NodePlanning node)
{
List <NodePlanning> neighbours = new List <NodePlanning>();
foreach (ActionPlanning action in mActionList)
{
//Hacer en el bh para comparar
//WorldState completeWS = node.mWorldState | gym.mGymWorldState;
// If preconditions are met we can apply effects and the new state is valid
if ((node.mWorldState & action.mPreconditions) == action.mPreconditions)
{
// Apply action and effects
NodePlanning newNodePlanning = new NodePlanning(node.mWorldState | action.mEffects, action);
neighbours.Add(newNodePlanning);
}
}
return(neighbours);
}
/***************************************************************************/
#region Forward
public List <NodePlanning> GetNeighbours(NodePlanning node)
{
ingredients = FindObjectsOfType <Ingredient>().ToList();
List <NodePlanning> neighbours = new List <NodePlanning>();
foreach (PlanningAction action in mActionList)
{
// If preconditions are met we can apply effects and the new state is valid
if ((node.mWorldState.mask & action.mPreconditions) == action.mPreconditions && MeetsAdditionalPreconditions(node.mWorldState, action))
{
// Apply action and effects
NodePlanning newNodePlanning = new NodePlanning(node.mWorldState, action);
newNodePlanning.mWorldState.mask |= action.mEffects;
newNodePlanning.mWorldState.mask &= ~action.mNegEffects;
ApplyAdditionalEffects(newNodePlanning, newNodePlanning.mWorldState, action);
neighbours.Add(newNodePlanning);
}
}
return(neighbours);
}
public void ApplyAdditionalEffects(NodePlanning nodePlanning, WorldState mWorldState, PlanningAction action)
{
switch (action.mActionType)
{
case PlanningAction.ActionType.AT_GO_TO:
Vector3 ingredientPos = FindIngredientOfType(action.mIngredient);
nodePlanning.mAction.mCost = (ingredientPos - mWorldState.cPos).magnitude;
mWorldState.cPos = ingredientPos;
break;
case PlanningAction.ActionType.AT_PICK_UP:
mWorldState.ingredientsKept.Add(action.mIngredient);
break;
case PlanningAction.ActionType.AT_GO_TO_KITCHEN:
nodePlanning.mAction.mCost = (kitchen.transform.position - mWorldState.cPos).magnitude;
mWorldState.cPos = kitchen.transform.position;
mWorldState.finalRecipe.Add(RecipeCompleted(mWorldState));
break;
default:
break;
}
}
/***************************************************************************/
public bool Equals(NodePlanning other)
{
return(mWorldState == other.mWorldState);
}
/***************************************************************************/
public List <NodePlanning> FindPlan(WorldState startWorldState, WorldState targetWorldState)
{
mWorld.plan = new List <NodePlanning>();
targetWorldState.cPos = mWorld.kitchen.transform.position;
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.CompareFinal(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.CompareFinal(CurrentTargetNode.mWorldState))
{
// Open neighbours
foreach (NodePlanning neighbour in mWorld.GetNeighbours(node))
{
if (/*!neighbour.mWalkable ||*/ closedSet.Any(n => n.mWorldState.Compare(neighbour.mWorldState)))
{
continue;
}
float newCostToNeighbour = node.gCost + GetDistance(node, neighbour);
if (newCostToNeighbour < neighbour.gCost || !openSet.Any(n => n.mWorldState.Compare(neighbour.mWorldState)))
{
neighbour.gCost = newCostToNeighbour;
neighbour.hCost = Heuristic(neighbour, CurrentTargetNode);
neighbour.mParent = node;
if (!openSet.Any(n => n.mWorldState.Compare(neighbour.mWorldState)))
{
openSet.Add(neighbour);
mWorld.openSet = openSet;
}
else
{
// Find neighbour and replace
openSet[openSet.FindIndex(x => x.mWorldState.Compare(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;
CurrentTargetNode.mWorldState = node.mWorldState;
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
if (mWorld.plan != null && mWorld.plan.Count > 0)
{
Debug.Log("PLAN FOUND!");
}
else
{
Debug.Log("Not plan found");
}
for (int i = 0; i < mWorld.plan.Count; ++i)
{
//Debug.LogFormat("{0} Accumulated cost: {1}", mWorld.plan[i].mAction.mName, mWorld.plan[i].gCost);
}
return(mWorld.plan);
}
/***************************************************************************/
float Heuristic(NodePlanning nodeA, NodePlanning nodeB)
{
// Heuristic function
//return -World.PopulationCount( (int)(nodeA.mWorldState | nodeB.mWorldState) ) - World.PopulationCount( (int)(nodeA.mWorldState & nodeB.mWorldState) );
return(0);
}
/***************************************************************************/
float GetDistance(NodePlanning nodeA, NodePlanning nodeB)
{
// Distance function
return(nodeB.mAction.mCost);
}
