int CompareCost(ARAstarNode n1, ARAstarNode n2)
{
float[] n1Key = n1.Key(parentPlanner.inflationFactor);
float[] n2Key = n2.Key(parentPlanner.inflationFactor);
if (n1.highPriority > 0 && n2.highPriority <= 0)
{
return(-1);
}
else if (n1.highPriority <= 0 && n2.highPriority > 0)
{
return(1);
}
else
{
if (n1Key[0] < n2Key[0])
{
return(-1);
}
else if (n1Key[0] > n2Key[0])
{
return(1);
}
else
{
if (n1Key[1] < n2Key[1])
{
return(-1);
}
else
{
return(1);
}
}
}
}
public void Insert(ARAstarNode node)
{
if (openDictionary.ContainsKey(node.action.state))
{
if (openDictionary[node.action.state].g > node.g)
{
if (useHeap)
{
openHeap.Remove(openDictionary[node.action.state]);
openHeap.Insert(node);
}
else
{
openList.Remove(openDictionary[node.action.state]);
openList.Add(node);
}
openDictionary[node.action.state] = node;
}
}
else
{
openDictionary[node.action.state] = node;
if (useHeap)
{
openHeap.Insert(node);
}
else
{
openList.Add(node);
}
}
}
public void UpdateList(ARAstarNode currentState)
{
startState = currentState.action.state;
Queue <ARAstarNode> queue = new Queue <ARAstarNode> ();
queue.Enqueue(currentState);
int n = 0;
while (queue.Count > 0)
{
ARAstarNode state = queue.Dequeue();
UpdateState(ref state, ref queue);
n++;
}
foreach (ARAstarNode node in dictionary.Values)
{
node.isDirty = false;
node.touched = false;
node.updated = false;
dictionary[node.action.state].isDirty = false;
dictionary[node.action.state].touched = false;
dictionary[node.action.state].updated = false;
}
}
private void SwapNodes(int i, int j)
{
ARAstarNode temp = heap[i].node;
heap[i].node = heap[j].node;
heap[j].node = temp;
}
public void Insert(ARAstarNode node)
{
ARAstarHeapNode newHeapNode = new ARAstarHeapNode(node, heap.Count);
heap.Add(newHeapNode);
HeapUp(heap.Count - 1);
}
public ARAstarHeapNode(ARAstarNode _node, int _index)
{
node = _node;
index = _index;
leftChild = 2 * index + 1;
rightChild = 2 * index + 2;
parent = index == 0 ? -1 : index / 2;
}
public void Insert(ARAstarNode node)
{
if(close.ContainsKey(node.action.state)){
if(close[node.action.state].g > node.g)
close[node.action.state] = node;
}
else{
close[node.action.state] = node;
}
}
public ARAstarNode ExtractMax()
{
if (heap.Count == 0)
{
throw new InvalidOperationException("Empty heap.");
}
ARAstarNode max = heap[0].node;
heap[0].node = heap[heap.Count - 1].node;
heap.RemoveAt(heap.Count - 1);
HeapDown(0);
return(max);
}
public void Remove(ARAstarNode node)
{
foreach(ARAstarHeapNode n in heap)
{
if(n.node == node){
int index = heap.IndexOf(n);
SwapNodes(index, heap.Count-1);
heap.RemoveAt(heap.Count-1);
HeapDown(index);
break;
}
}
}
public void Remove(ARAstarNode node)
{
foreach (ARAstarHeapNode n in heap)
{
if (n.node == node)
{
int index = heap.IndexOf(n);
SwapNodes(index, heap.Count - 1);
heap.RemoveAt(heap.Count - 1);
HeapDown(index);
break;
}
}
}
public void Insert(ARAstarNode node)
{
if (close.ContainsKey(node.action.state))
{
if (close[node.action.state].g > node.g)
{
close[node.action.state] = node;
}
}
else
{
close[node.action.state] = node;
}
}
public void Insert(ARAstarNode node)
{
if (incons.ContainsKey(node.action.state))
{
if (incons[node.action.state].g > node.g)
{
incons[node.action.state] = node;
}
}
else
{
incons[node.action.state] = node;
}
}
int compareKey(ARAstarNode firstNode, ARAstarNode secondNode)
{
float[] firstKey = firstNode.Key(currentWeight);
float[] secondKey = secondNode.Key(currentWeight);
if(firstKey[0] < secondKey[0])
return 1;
else if(firstKey[0] > secondKey[0])
return -1;
else{
if(firstKey[1] < secondKey[1])
return 1;
else
return -1;
}
}
private void HeapUp(int i)
{
ARAstarNode node = heap[i].node;
while (true)
{
int parent = heap[i].parent;
if (parent < 0 || compareKey(heap[parent].node, node) == 1)
{
break;
}
SwapNodes(i, parent);
i = parent;
}
}
void UpdateCost(ref ARAstarNode node, DefaultState successor)
{
if (!dictionary[successor].touched)
{
dictionary[successor].g =
node.g + Vector3.Distance((node.action.state as ARAstarState).state, (successor as ARAstarState).state);
dictionary[successor].touched = true;
dictionary[successor].g = dictionary[successor].rhs;
}
else if (dictionary[successor].g >
node.g + Vector3.Distance((node.action.state as ARAstarState).state, (successor as ARAstarState).state))
{
dictionary[successor].g =
node.g + Vector3.Distance((node.action.state as ARAstarState).state, (successor as ARAstarState).state);
dictionary[successor].g = dictionary[successor].rhs;
}
}
void UpdateState(ref ARAstarNode state, ref Queue <ARAstarNode> queue)
{
List <DefaultAction> neighborsList = new List <DefaultAction>();
domain.generatePredecessors(state.action.state, ref neighborsList);
foreach (DefaultAction action in neighborsList)
{
if (dictionary.ContainsKey(action.state) && !dictionary[action.state].updated)
{
UpdateCost(ref state, action.state);
UpdateReference(action.state);
if (!dictionary[action.state].updated)
{
dictionary[action.state].updated = true;
}
queue.Enqueue(dictionary[action.state]);
}
}
}
public void UpdateHeuristic(DefaultState goal)
{
if (useHeap)
{
foreach (ARAstarHeapNode heapNode in openHeap.heap)
{
ARAstarNode n = heapNode.node;
float newh = Vector3.Distance((n.action.state as ARAstarState).state, (goal as ARAstarState).state);
n.h = newh;
openDictionary[n.action.state].h = newh;
}
}
else
{
foreach (ARAstarNode n in openList)
{
float newh = Vector3.Distance((n.action.state as ARAstarState).state, (goal as ARAstarState).state);
n.h = newh;
openDictionary[n.action.state].h = newh;
}
}
}
int compareKey(ARAstarNode firstNode, ARAstarNode secondNode)
{
float[] firstKey = firstNode.Key(currentWeight);
float[] secondKey = secondNode.Key(currentWeight);
if (firstKey[0] < secondKey[0])
{
return(1);
}
else if (firstKey[0] > secondKey[0])
{
return(-1);
}
else
{
if (firstKey[1] < secondKey[1])
{
return(1);
}
else
{
return(-1);
}
}
}
public void Insert(ARAstarNode node)
{
if (incons.ContainsKey (node.action.state)) {
if (incons[node.action.state].g > node.g) {
incons[node.action.state] = node;
}
} else {
incons[node.action.state] = node;
}
}
public void insertNode(ref ARAstarNode node)
{
dictionary[node.action.state] = node;
}
public void InsertNode(ref DefaultState st, ref ARAstarNode node)
{
plan[st] = node;
}
void UpdateState(ref ARAstarNode state, ref Queue<ARAstarNode> queue)
{
List<DefaultAction> neighborsList = new List<DefaultAction>();
domain.generatePredecessors(state.action.state, ref neighborsList);
foreach (DefaultAction action in neighborsList) {
if(dictionary.ContainsKey(action.state) && !dictionary[action.state].updated)
{
UpdateCost (ref state, action.state);
UpdateReference (action.state);
if(!dictionary[action.state].updated)
dictionary[action.state].updated = true;
queue.Enqueue (dictionary[action.state]);
}
}
}
void UpdateCost(ref ARAstarNode node, DefaultState successor)
{
if(!dictionary[successor].touched)
{
dictionary[successor].g =
node.g + Vector3.Distance((node.action.state as ARAstarState).state,(successor as ARAstarState).state);
dictionary[successor].touched = true;
dictionary[successor].g = dictionary[successor].rhs;
}
else if(dictionary[successor].g >
node.g + Vector3.Distance((node.action.state as ARAstarState).state, (successor as ARAstarState).state))
{
dictionary[successor].g =
node.g + Vector3.Distance((node.action.state as ARAstarState).state, (successor as ARAstarState).state);
dictionary[successor].g = dictionary[successor].rhs;
}
}
public void UpdateList(ARAstarNode currentState)
{
startState = currentState.action.state;
Queue<ARAstarNode> queue = new Queue<ARAstarNode> ();
queue.Enqueue (currentState);
int n = 0;
while (queue.Count > 0) {
ARAstarNode state = queue.Dequeue ();
UpdateState (ref state, ref queue);
n++;
}
foreach (ARAstarNode node in dictionary.Values) {
node.isDirty = false;
node.touched = false;
node.updated = false;
dictionary[node.action.state].isDirty = false;
dictionary[node.action.state].touched = false;
dictionary[node.action.state].updated = false;
}
}
public void insertNode(ref ARAstarNode node)
{
dictionary[node.action.state] = node;
}
int CompareCost(ARAstarNode n1, ARAstarNode n2)
{
float[] n1Key = n1.Key(parentPlanner.inflationFactor);
float[] n2Key = n2.Key(parentPlanner.inflationFactor);
if(n1.highPriority > 0 && n2.highPriority <= 0)
return -1;
else if(n1.highPriority <= 0 && n2.highPriority > 0)
return 1;
else{
if (n1Key[0] < n2Key[0])
return -1;
else if(n1Key[0] > n2Key[0])
return 1;
else {
if(n1Key[1] < n2Key[1])
return -1;
else
return 1;
}
}
}
public void Insert(ARAstarNode node)
{
ARAstarHeapNode newHeapNode = new ARAstarHeapNode(node, heap.Count);
heap.Add(newHeapNode);
HeapUp(heap.Count-1);
}
public void InsertNode(ref DefaultState st, ref ARAstarNode node)
{
plan[st] = node;
}
public void Insert(ARAstarNode node)
{
if (openDictionary.ContainsKey (node.action.state)) {
if (openDictionary[node.action.state].g > node.g) {
if(useHeap){
openHeap.Remove(openDictionary[node.action.state]);
openHeap.Insert (node);
} else {
openList.Remove(openDictionary[node.action.state]);
openList.Add(node);
}
openDictionary[node.action.state] = node;
}
} else {
openDictionary[node.action.state] = node;
if(useHeap)
openHeap.Insert (node);
else
openList.Add(node);
}
}
public ARAstarHeapNode(ARAstarNode _node, int _index)
{
node = _node;
index = _index;
leftChild = 2*index+1;
rightChild = 2*index+2;
parent = index == 0 ? -1 : index/2;
}
