bool OnlinePlanExpansionExecution::DestroyGoalSnippetIfExist(_In_ IOlcbpPlan::NodeID snippetGoalId)
{
    IOlcbpPlan::NodeQueue Q;
    IOlcbpPlan::NodeSerializedSet visitedNodes;
    IOlcbpPlan::NodeSerializedSet snippetRoots;

    // 1. Collect the snippet orphan nodes to start the BFS from
    GetSnippetOrphanNodes(snippetGoalId, snippetRoots);
    for (auto childNode : snippetRoots)
    {
        if (GetNodeData(childNode).SatisfyingGoal == snippetGoalId)
        {
            Q.push(childNode);
            visitedNodes.insert(childNode);
        }
    }

    // 2. Do a BFS on the snippet to collect its nodes
    while (!Q.empty())
    {
        IOlcbpPlan::NodeID currNodeId = Q.front();
        Q.pop();

        const IOlcbpPlan::NodeSerializedSet& currChildren = m_pOlcbpPlan->GetAdjacentNodes(currNodeId);

        for (auto currChildNodeId : currChildren)
        {
            if (visitedNodes.count(currChildNodeId) == 0)
            {
                Q.push(currChildNodeId);
                visitedNodes.insert(currChildNodeId);
            }
        }
    }

    // 3. Remove visited nodes from the plan
    for (auto visitedNodeId : visitedNodes)
    {
        auto pCurrNode = m_pOlcbpPlan->GetNode(visitedNodeId);

        if (IsActionNode(visitedNodeId))
        {
            ((Action*)pCurrNode)->Abort(*g_Game);
        }

        LogWarning("MEMORY LEAK detected, should delete plan node[%d]", visitedNodeId);
        // deleting currNode crashes the execution history logic, should fix
        m_pOlcbpPlan->RemoveNode(visitedNodeId);
        m_nodeData.erase(visitedNodeId);
        delete pCurrNode;
    }

    // 4. Since there is no plan destroyed, fast return with false
    if (visitedNodes.empty())
        return false;
    else
    {
        // 4. Since the plan structure changed, raise the flag
        m_planStructureChangedThisFrame = true;
        return true;
    }
}
void OnlinePlanExpansionExecution::ExpandGoal(_In_ IOlcbpPlan::NodeID expansionGoalNodeId, _In_ CaseEx *pExpansionCase)
{
    LogActivity(ExpandGoal);

    IOlcbpPlan *pCasePlan = pExpansionCase->Plan();
    IOlcbpPlan::NodeSerializedSet casePlanRoots = pCasePlan->GetOrphanNodes();
    IOlcbpPlan::NodeSerializedSet casePlanNodes = pCasePlan->GetNodes();
    IOlcbpPlan::NodeSerializedSet newExpansionPlanRoots;

    LogInfo("Expanding node[%d] with plan %s", expansionGoalNodeId, pExpansionCase->Plan()->ToString().c_str());

    // 1. Construct planner plan graph nodes from the expansion case
    SetGoalNodeBelongingCase(expansionGoalNodeId, pExpansionCase);

    // Cross mapping between node IDs in in the planner plan and case plan
    std::map<IOlcbpPlan::NodeID, IOlcbpPlan::NodeID> plannerToCasePlanNodeIdMap;
    std::map<IOlcbpPlan::NodeID, IOlcbpPlan::NodeID> casePlanToPlannerNodeIdMap;

    for(auto caseNodeId : casePlanNodes)
    {
        IOlcbpPlan::NodeValue pOriginalNode = pCasePlan->GetNode(caseNodeId);
        IOlcbpPlan::NodeValue pNode = static_cast<PlanStepEx*>(const_cast<PlanStepEx*>(pOriginalNode)->Clone());

        IOlcbpPlan::NodeID plannerNodeId = m_pOlcbpPlan->AddNode(pNode, pNode->Id());
        m_nodeData[plannerNodeId] = OlcbpPlanNodeData();
        m_nodeData[plannerNodeId].ID = plannerNodeId;

        // Add Data record for the new node
        SetNodeSatisfyingGoal(plannerNodeId, expansionGoalNodeId);

        if (casePlanRoots.count(caseNodeId) > 0)
            newExpansionPlanRoots.insert(plannerNodeId);

        if (IsActionNode(plannerNodeId))
        {
            m_clonedNodesMapping[pOriginalNode] = pNode;
        }
        else if (IsGoalNode(plannerNodeId))
        {
            OpenNode(plannerNodeId);
        }

        // Map node ID in in the planner plan with its counterpart in case plan and vice versa
        plannerToCasePlanNodeIdMap[plannerNodeId] = caseNodeId;
        casePlanToPlannerNodeIdMap[caseNodeId] = plannerNodeId;
    }

    // 2. Link the goal node with the roots of the expansion plan
    IOlcbpPlan::NodeQueue Q;
    IOlcbpPlan::NodeSerializedSet visitedNodes;

    for(auto rootNodeId : newExpansionPlanRoots)
    {
        // Cross link the goal node with the sub plan roots
        LinkNodes(expansionGoalNodeId, rootNodeId);

        // Enqueue the roots in a queue to expand them
        Q.push(rootNodeId);
        visitedNodes.insert(rootNodeId);
    }

    // 3. Continue construction of the sub plan tree with the rest of the plan graph nodes
    while(!Q.empty())
    {
        IOlcbpPlan::NodeID currPlannerNodeId = Q.front();
        Q.pop();

        IOlcbpPlan::NodeSerializedSet currCaseChildren = pCasePlan->GetAdjacentNodes(plannerToCasePlanNodeIdMap[currPlannerNodeId]);

        for (auto currCaseChildNodeId : currCaseChildren)
        {
            IOlcbpPlan::NodeID currPlannerChildNodeId = casePlanToPlannerNodeIdMap[currCaseChildNodeId];

            // If my child not visited, consider it for expansion
            if(!visitedNodes.count(currPlannerChildNodeId))
            {
                Q.push(currPlannerChildNodeId);
                visitedNodes.insert(currPlannerChildNodeId);
            }

            // Cross link the current node with its child
            LinkNodes(currPlannerNodeId, currPlannerChildNodeId);
        }
    }

    int nodeAddRemoveDelta = 0; //AdaptSnippet(expansionGoalNodeId);
    // Adaptation should leave at least 1 node in the snippet
    _ASSERTE((int)pCasePlan->Size() + nodeAddRemoveDelta > 0);
    size_t adaptedSnippetSize = (size_t)((int)pCasePlan->Size() + nodeAddRemoveDelta);

    CloseNode(expansionGoalNodeId);

    // 4. Since the plan structure changed, recompute the NotReadyParentsCount
    // for plan graph node
    ComputeFreshSnippetWaitOnParentsCount(expansionGoalNodeId);

    // 5. Record that the expanded goal node is waiting for N number of open nodes
    // If the number of those open nodes = 0, and the goal node is not satisfied, then
    // there is no way for the goal to succeed, and the goal should fail
    GetNodeData(expansionGoalNodeId).SetWaitOnChildrenCount(adaptedSnippetSize);

    m_planStructureChangedThisFrame = true;

    LogInfo("Plan after node[%d] expansion %s", expansionGoalNodeId, m_pOlcbpPlan->ToString().c_str());
}
Esempio n. 3
0
//----------------------------------------------------------------------------------------------
void IStrategizer::GraphScene::ComputeGraphLevels()
{
    IOlcbpPlan::NodeSerializedSet roots = m_pGraph->GetOrphanNodes();
    IOlcbpPlan::NodeSerializedSet visitedNodes;

    typedef unsigned NodeLevel;
    deque< pair<NodeID, NodeLevel> > Q;

    for (NodeID nodeId : roots)
    {
        Q.push_back(make_pair(nodeId, 0));
        visitedNodes.insert(nodeId);
    }

    for(unsigned i = 0; i < m_graphLevels.size(); ++i)
        m_graphLevels[i].clear();
    m_graphLevels.clear();

	list<NodeID> orderedCaseChildren;
	list<NodeID> orderedSnippetChildren;

    while(!Q.empty())
    {
        NodeID currNodeId = Q.front().first;
        NodeLevel nodeLevel = Q.front().second;

        Q.pop_front();

        if(nodeLevel >= m_graphLevels.size())
            m_graphLevels.resize(nodeLevel + 1);

		m_graphLevels[nodeLevel].push_back(currNodeId);

		IOlcbpPlan::NodeSerializedSet children = m_pGraph->GetAdjacentNodes(currNodeId);

		if (m_pPlanContext != nullptr)
		{
			orderedCaseChildren.clear();
			orderedSnippetChildren.clear();

			for (NodeID nodeId : children)
			{
				if (visitedNodes.count(nodeId) == 0)
				{
					visitedNodes.insert(nodeId);

					if (m_pPlanContext->Data.at(nodeId).SatisfyingGoal != currNodeId)
					{
						if (IsGoalNode(nodeId))
							orderedCaseChildren.push_front(nodeId);
						else
							orderedCaseChildren.push_back(nodeId);
					}
					else
					{
						if (IsGoalNode(nodeId))
							orderedSnippetChildren.push_front(nodeId);
						else
							orderedSnippetChildren.push_back(nodeId);
					}
				}
			}

			for (auto nodeId : orderedCaseChildren)
				Q.push_back(make_pair(nodeId, nodeLevel + 1));

			for (auto nodeId : orderedSnippetChildren)
				Q.push_back(make_pair(nodeId, nodeLevel + 1));
		}
		else
		{
			for (NodeID nodeId : children)
			{
				if (visitedNodes.count(nodeId) == 0)
				{
					visitedNodes.insert(nodeId);
					Q.push_back(make_pair(nodeId, nodeLevel + 1));
				}
			}
		}
    }
}