public void Execute()
{
// Resize containers
int graphSize = DepthMap.Count();
SelectedStatesByHorizon.Capacity = math.max(SelectedStatesByHorizon.Capacity, MaxDepth + 1);
PredecessorStates.Capacity = math.max(PredecessorStates.Capacity, graphSize);
HorizonStateList.Capacity = math.max(HorizonStateList.Capacity, graphSize);
var selectedStateKeys = SelectedStates.GetKeyArray(Allocator.Temp);
for (int i = 0; i < selectedStateKeys.Length; i++)
{
var stateKey = selectedStateKeys[i];
int stateDepth = int.MinValue;
SelectedStates.TryGetFirstValue(stateKey, out var selectedDepth, out var iterator);
do
{
stateDepth = math.max(stateDepth, selectedDepth);
}while (SelectedStates.TryGetNextValue(out selectedDepth, ref iterator));
// Update depth map
DepthMap[stateKey] = stateDepth;
SelectedStatesByHorizon.AddValueIfUnique(stateDepth, stateKey);
}
}
public void Execute()
{
var statesToUpdateLength = SelectedStates.Count();
var statesToUpdate = new NativeMultiHashMap <int, TStateKey>(statesToUpdateLength, Allocator.Temp);
var currentHorizon = new NativeList <TStateKey>(statesToUpdateLength, Allocator.Temp);
var nextHorizon = new NativeList <TStateKey>(statesToUpdateLength, Allocator.Temp);
var maxDepth = int.MinValue;
var selectedStateKeys = SelectedStates.GetKeyArray(Allocator.Temp);
for (int i = 0; i < selectedStateKeys.Length; i++)
{
var stateKey = selectedStateKeys[i];
int stateDepth = int.MinValue;
SelectedStates.TryGetFirstValue(stateKey, out var selectedDepth, out var iterator);
do
{
stateDepth = math.max(stateDepth, selectedDepth);
}while (SelectedStates.TryGetNextValue(out selectedDepth, ref iterator));
// Update depth map
DepthMap[stateKey] = stateDepth;
// Queue state and track max depth of backpropagation
statesToUpdate.AddValueIfUnique(stateDepth, stateKey);
maxDepth = math.max(maxDepth, stateDepth);
}
selectedStateKeys.Dispose();
var actionLookup = planGraph.ActionLookup;
var resultingStateLookup = planGraph.ResultingStateLookup;
var actionInfoLookup = planGraph.ActionInfoLookup;
var stateInfoLookup = planGraph.StateInfoLookup;
var stateTransitionInfoLookup = planGraph.StateTransitionInfoLookup;
var predecessorLookup = planGraph.PredecessorGraph;
var depth = maxDepth;
// Pull states from statesToUpdate
if (statesToUpdate.TryGetFirstValue(depth, out var stateToAdd, out var stateIterator))
{
do
{
currentHorizon.AddIfUnique(stateToAdd);
}while (statesToUpdate.TryGetNextValue(out stateToAdd, ref stateIterator));
}
// Update values from leaf state(s) to root
while (depth >= 0)
{
for (int i = 0; i < currentHorizon.Length; i++)
{
var stateKey = currentHorizon[i];
var updateState = true;
if (actionLookup.TryGetFirstValue(stateKey, out var actionKey, out var stateActionIterator))
{
// Expanded state. Only update if one or more actions have updated.
updateState = false;
// Update all actions
do
{
updateState |= UpdateCumulativeReward(new StateActionPair <TStateKey, TActionKey>(stateKey, actionKey), resultingStateLookup, stateInfoLookup, actionInfoLookup, stateTransitionInfoLookup);
}while (actionLookup.TryGetNextValue(out actionKey, ref stateActionIterator));
}
if (!updateState)
{
continue;
}
// Update state
if (UpdateStateValue(stateKey, actionLookup, stateInfoLookup, actionInfoLookup))
{
// If a change has occured, update predecessors
if (predecessorLookup.TryGetFirstValue(stateKey, out var predecessorStateKey, out var predecessorIterator))
{
do
{
nextHorizon.AddIfUnique(predecessorStateKey);
}while (predecessorLookup.TryGetNextValue(out predecessorStateKey, ref predecessorIterator));
}
}
}
var temp = currentHorizon;
currentHorizon = nextHorizon;
nextHorizon = temp;
nextHorizon.Clear();
depth--;
// pull out states from statesToUpdate
if (statesToUpdate.TryGetFirstValue(depth, out stateToAdd, out stateIterator))
{
do
{
currentHorizon.AddIfUnique(stateToAdd);
}while (statesToUpdate.TryGetNextValue(out stateToAdd, ref stateIterator));
}
}
// new: continue propagating complete flag changes
while (currentHorizon.Length > 0)
{
for (int i = 0; i < currentHorizon.Length; i++)
{
var stateKey = currentHorizon[i];
var updateState = false;
// Update all actions
actionLookup.TryGetFirstValue(stateKey, out var actionKey, out var stateActionIterator);
do
{
var stateActionPair = new StateActionPair <TStateKey, TActionKey>(stateKey, actionKey);
if (UpdateSubplanCompleteStatus(stateActionPair, out var updatedActionInfo))
{
updateState = true;
// Write back updated info
actionInfoLookup[stateActionPair] = updatedActionInfo;
}
}while (actionLookup.TryGetNextValue(out actionKey, ref stateActionIterator));
// Update state
if (updateState && UpdateSubplanCompleteStatus(stateKey, out var updatedStateInfo))
{
// Write back updated info
stateInfoLookup[stateKey] = updatedStateInfo;
// If a change has occured, update predecessors
if (predecessorLookup.TryGetFirstValue(stateKey, out var predecessorStateKey, out var predecessorIterator))
{
do
{
nextHorizon.AddIfUnique(predecessorStateKey);
}while (predecessorLookup.TryGetNextValue(out predecessorStateKey, ref predecessorIterator));
}
}
}
var temp = currentHorizon;
currentHorizon = nextHorizon;
nextHorizon = temp;
nextHorizon.Clear();
}
currentHorizon.Dispose();
nextHorizon.Dispose();
statesToUpdate.Dispose();
}
