/***************************************************************************/ 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); }