private void Swap(int a, int b)
{
PriorityNode temp = storage[a];
storage[a] = storage[b];
storage[b] = temp;
}
public static TreeNode GetAllTypeNode()
{
TypesN8Node nodeType = new TypesN8Node();
nodeType.Text = nodeType.KNXMainNumber + "." + nodeType.KNXSubNumber + " " + nodeType.DPTName;
nodeType.Nodes.Add(SCLOModeNode.GetTypeNode());
nodeType.Nodes.Add(BuildingModeNode.GetTypeNode());
nodeType.Nodes.Add(OccModeNode.GetTypeNode());
nodeType.Nodes.Add(PriorityNode.GetTypeNode());
nodeType.Nodes.Add(LightApplicationModeNode.GetTypeNode());
nodeType.Nodes.Add(ApplicationAreaNode.GetTypeNode());
nodeType.Nodes.Add(AlarmClassTypeNode.GetTypeNode());
nodeType.Nodes.Add(PSUModeNode.GetTypeNode());
nodeType.Nodes.Add(ErrorClassSystemNode.GetTypeNode());
nodeType.Nodes.Add(ErrorClassHVACNode.GetTypeNode());
nodeType.Nodes.Add(TimeDelayNode.GetTypeNode());
nodeType.Nodes.Add(BeaufortWindForceScaleNode.GetTypeNode());
nodeType.Nodes.Add(SensorSelectNode.GetTypeNode());
nodeType.Nodes.Add(ActuatorConnectTypeNode.GetTypeNode());
nodeType.Nodes.Add(FuelTypeNode.GetTypeNode());
nodeType.Nodes.Add(BurnerTypeNode.GetTypeNode());
nodeType.Nodes.Add(HVACModeNode.GetTypeNode());
nodeType.Nodes.Add(DHWModeNode.GetTypeNode());
nodeType.Nodes.Add(LoadPriorityNode.GetTypeNode());
nodeType.Nodes.Add(HVACContrModeNode.GetTypeNode());
nodeType.Nodes.Add(HVACEmergModeNode.GetTypeNode());
nodeType.Nodes.Add(ChangeoverModeNode.GetTypeNode());
nodeType.Nodes.Add(ValveModeNode.GetTypeNode());
nodeType.Nodes.Add(DamperModeNode.GetTypeNode());
nodeType.Nodes.Add(HeaterModeNode.GetTypeNode());
nodeType.Nodes.Add(FanModeNode.GetTypeNode());
nodeType.Nodes.Add(MasterSlaveModeNode.GetTypeNode());
nodeType.Nodes.Add(StatusRoomSetpNode.GetTypeNode());
nodeType.Nodes.Add(MeteringDeviceTypeNode.GetTypeNode());
nodeType.Nodes.Add(ADATypeNode.GetTypeNode());
nodeType.Nodes.Add(BackupModeNode.GetTypeNode());
nodeType.Nodes.Add(StartSynchronizationNode.GetTypeNode());
nodeType.Nodes.Add(BehaviourLockUnlockNode.GetTypeNode());
nodeType.Nodes.Add(BehaviourBusPowerUpDownNode.GetTypeNode());
nodeType.Nodes.Add(DALIFadeTimeNode.GetTypeNode());
nodeType.Nodes.Add(BlinkingModeNode.GetTypeNode());
nodeType.Nodes.Add(LightControlModeNode.GetTypeNode());
nodeType.Nodes.Add(SwitchPBModelNode.GetTypeNode());
nodeType.Nodes.Add(PBActionNode.GetTypeNode());
nodeType.Nodes.Add(DimmPBModelNode.GetTypeNode());
nodeType.Nodes.Add(SwitchOnModeNode.GetTypeNode());
nodeType.Nodes.Add(LoadTypeSetNode.GetTypeNode());
nodeType.Nodes.Add(LoadTypeDetectedNode.GetTypeNode());
nodeType.Nodes.Add(SABExceptBehaviourNode.GetTypeNode());
nodeType.Nodes.Add(SABBehaviourLockUnlockNode.GetTypeNode());
nodeType.Nodes.Add(SSSBModeNode.GetTypeNode());
nodeType.Nodes.Add(BlindsControlModeNode.GetTypeNode());
nodeType.Nodes.Add(CommModeNode.GetTypeNode());
nodeType.Nodes.Add(AddInfoTypesNode.GetTypeNode());
nodeType.Nodes.Add(RFModeSelectNode.GetTypeNode());
nodeType.Nodes.Add(RFFilterSelectNode.GetTypeNode());
return(nodeType);
}
/// <summary>
/// Finds where the node should be placed in the tree
/// </summary>
/// <param name="item"></param>
/// <param name="priority"></param>
/// <param name="node"></param>
public void FindNodePosition(T item, float priority, PriorityNode <T> node)
{
if (node.PriorityValue > priority)
{
// New node is of higher priority
if (node.Left != null)
{
FindNodePosition(item, priority, node.Left);
}
else
{
node.Left = new PriorityNode <T>(item, priority);
}
}
else
{
// New node is of lower or same priority
if (node.Right != null)
{
FindNodePosition(item, priority, node.Right);
}
else
{
node.Right = new PriorityNode <T>(item, priority);
}
}
}
public void Add(K key, V value)
{
data.Add(new PriorityNode(key, value));
// The index of the child (lower priority)
int child = data.Count - 1;
while (child > 0)
{
// The index of the parent (higher priority)
int parent = (child - 1) / 2;
// If the child's value is greater than the parent's value,
// everything is as it should be and we can stop
V cVal = data[child].value;
V pVal = data[parent].value;
if (cVal.CompareTo(pVal) >= 0)
{
break;
}
// If the child's value is less than the parent's value,
// we need to swap the two values
PriorityNode temp = data[parent];
data[parent] = data[child];
data[child] = temp;
child = parent;
}
}
public T Dequeue()
{
PriorityNode node = elements[elements.Count - 1];
elements.RemoveAt(elements.Count - 1);
return(node.Data);
}
public void Push(T item)
{
PriorityNode newNode = new PriorityNode(item, nextPriority);
nextPriority--; // pushed earlier -> greater value in min heap
pq.Add(newNode);
}
public PriorityNode(PriorityNode parent, TNode node, float realCost, float heuristicCost)
{
Parent = parent;
Node = node;
Priority = realCost + heuristicCost;
RealCost = realCost;
}
public K Remove()
{
// Store the element to be returned
K result = data[0].key;
// Set the first element to the last element
int last = data.Count - 1;
data[0] = data[last];
data.RemoveAt(last);
last--;
int parent = 0;
while (parent < last)
{
// Get the index of the smaller child
int child = 0;
int left = (parent * 2) + 1;
if (left > last)
{
// If there's no children we can stop
break;
}
else if (left == last)
{
// If there's only one child use that
child = left;
}
else
{
// If there's two children get the smaller
int right = (parent * 2) + 2;
V lValue = data[left].value;
V rValue = data[right].value;
child = lValue.CompareTo(rValue) <= 0 ? left : right;
}
// If the parent's value is less than the child's value,
// everything is as it should be and we can stop
V pValue = data[parent].value;
V cValue = data[child].value;
if (pValue.CompareTo(cValue) <= 0)
{
break;
}
// If the parent's value is greater than the child's value,
// we need to swap the two values
PriorityNode temp = data[child];
data[child] = data[parent];
data[parent] = temp;
parent = child;
}
return(result);
}
public void Enqueue(int priority, T item)
{
PriorityNode node = new PriorityNode();
node.Data = item;
node.Priority = priority;
elements.Add(node);
elements.Sort((x, y) => x.Priority.CompareTo(y.Priority));
}
/// <summary>
/// Add a new item to the pqueue, put it in priority order
/// </summary>
/// <param name="node"></param>
public void Enqueue(PriorityNode <T> node)
{
// Add to the end of the list
node.index = priorityQueue.Count;
priorityQueue.Add(node);
int current = priorityQueue.Count - 1;
RaisePriority(current);
}
/// <summary>
/// Change the priority of a node
/// </summary>
/// <param name="node"></param>
/// <param name="newPriority"></param>
public void ChangePriority(PriorityNode <T> node, double newPriority)
{
if (0 <= node.index && node.index < priorityQueue.Count)
{
node.priority = newPriority;
priorityQueue[node.index].priority = newPriority;
RaisePriority(node.index);
LowerPriority(node.index);
}
}
private BehaviorTree CreateTree()
{
Node mainNode =
new PriorityNode(
new SequenceNode(
new HasLittleHealth(),
new PriorityNode(
new SequenceNode(
new HasNoHealth(),
new Die()
),
new PriorityNode(
new SequenceNode(
new CanSeePlayer(),
new SequenceNode(
new IndicateEscapePoint(_container),
new MoveToTarget()
)
),
new SequenceNode(
new IndicateHealingPoint(_container),
new MoveToTarget(),
new Heal()
)
)
)
),
new SequenceNode(
new CanSeePlayer(),
new SequenceNode(
new SequenceNode(
new CanShootPlayer(),
new Shoot(_container)
),
new SequenceNode(
new IndicatePlayerPoint(_container),
new MoveToTarget()
)
)
),
new PriorityNode(
new SequenceNode(
new IndicatePatrolPoint(_container),
new MoveToTarget(),
new ChangeToIdle()
),
new Idle()
)
);
BehaviorTree tree = new BehaviorTree(mainNode);
return(tree);
}
private void CreateTree()
{
_board = new BlackBoard();
Node root = new PriorityNode(
new SequenceNode(
new HasNoHealthPoints(),
new Die()
),
new SequenceNode(
new IsClosePlayer(),
new IsSuffering(),
new FeelAgony()
),
new SequenceNode(
new IsNotHealthy(),
new MemSequnceNode(
new Escape(),
new GatherShopaholics(),
new ReturnOnLastPlaceWherePlayerSeen()
)
),
new PriorityNode(
new SequenceNode(
new CanSeePlayer(),
new PriorityNode(
new SequenceNode(
new IsClosePlayer(),
new AttackPlayer()
),
new SequenceNode(
new FollowPlayer(),
new CanSeeOtherShopaholic(),
new CallSeenShopaholic()
)
)
),
new SequenceNode(
new PriorityNode(
new SequenceNode(
new IsCalled(),
new FollowShopaholic()
),
new Idle()
)
)
)
);
_tree = new BehaviorTree(root);
return;
}
/// <summary>
/// Compare two items by priority
/// </summary>
/// <param name="item"></param>
/// <returns></returns>
public int CompareTo(Object item)
{
PriorityNode <V> i = (PriorityNode <V>)item;
if (i == null)
{
throw new Exception("ERROR: Cannot compare to " + item);
}
return(this.priority.CompareTo(i.priority));
}
public void Enqueue <T>(T value, int priority)
{
PriorityNode newNode = new PriorityNode(value, priority);
// Add new node as the next available leaf
storage.Add(newNode);
if (storage.Count > 2)
{
// Maintain the min-max ordering
var index = storage.Count - 1;
BubbleUp(index);
}
}
/// <summary>
/// Adds a node into its place based on priority
/// If the same priority, works like a regular queue
/// </summary>
/// <param name="item"></param>
/// <param name="priority"></param>
public void Enqueue(T item, float priority)
{
// Add item to queue
if (root == null)
{
root = new PriorityNode <T>(item, priority);
}
else
{
FindNodePosition(item, priority, root);
}
Count++;
}
/// <summary>
/// Returns the bottom left most node since it is highest priority
/// Will reconnect any child nodes of the node to its previous node
/// </summary>
/// <returns></returns>
public T Dequeue()
{
if (root == null)
{
return(default(T));
}
// Find the left most node (Highest priority)
PriorityNode <T> current = root;
PriorityNode <T> previous = null;
while (current.Left != null)
{
previous = current;
current = current.Left;
}
// Remove the node
if (previous == null)
{
// The root is being dequeued
// Switch root to right if it exists
if (current.Right != null)
{
root = current.Right;
}
else
{
root = null;
}
}
else
{
// If there exists a right node to the one being removed
// Move it to connect to the upper part of the tree
if (current.Right != null)
{
previous.Left = current.Right;
}
else
{
previous.Left = null;
}
}
Count--;
return(current.Value);
}
/// <summary>
/// Raise the priority of the node
/// </summary>
/// <param name="current"></param>
private void RaisePriority(int current)
{
PriorityNode <T> newItem = priorityQueue[current];
int parent = (current - 1) / 2;
// Percolate UP
while (current > 0 && newItem.CompareTo(priorityQueue[parent]) < 0)
{
// Copy parent down into current
priorityQueue[current] = priorityQueue[parent];
priorityQueue[current].index = current;
current = parent;
parent = (current - 1) / 2;
}
priorityQueue[current] = newItem;
priorityQueue[current].index = current;
}
/// <summary>
/// Lower the priority of a node
/// </summary>
/// <param name="current"></param>
private void LowerPriority(int current)
{
if (priorityQueue.Count >= 1)
{
PriorityNode <T> lastItem = priorityQueue[current];
// Percolate Down
int parent = current;
int left = parent * 2 + 1;
int right = parent * 2 + 2;
int swap = current;
bool swapped = true;
while (swapped && left < priorityQueue.Count)
{
// Assume we will swap with the left child
swap = left;
// If the right child exists and its priority is less than the left child,
// choose the right child as the "to swap" item
if (right < priorityQueue.Count && priorityQueue[right].CompareTo(priorityQueue[left]) < 0)
{
swap = right;
}
// If the "to swap" item is lower priority than the parent, swap them.
if (priorityQueue[swap].CompareTo(lastItem) < 0)
{
priorityQueue[parent] = priorityQueue[swap];
priorityQueue[parent].index = parent;
parent = swap;
left = parent * 2 + 1;
right = parent * 2 + 2;
}
else
{
swapped = false;
}
}
priorityQueue[parent] = lastItem;
priorityQueue[parent].index = parent;
}
}
/// <summary>
/// Remove an item from the pqueue, highest priority first
/// </summary>
/// <returns></returns>
public PriorityNode <T> Dequeue()
{
if (priorityQueue.Count == 0)
{
throw new Exception("Cannot dequeue from an empty queue");
}
// Store the first item, we will return it
PriorityNode <T> node = new PriorityNode <T>(priorityQueue[0]);
// Store the last item, remove it from the list
priorityQueue[0] = priorityQueue[priorityQueue.Count - 1];
priorityQueue[0].index = 0;
priorityQueue.RemoveAt(priorityQueue.Count - 1);
LowerPriority(0);
return(node);
}
public void Enqueue(T val, int priority)
{
var node = new PriorityNode <T>(val, priority);
if (queue == null)
{
this.queue = new LinkedList <T>(node);
}
else
{
queue.Reset();
PriorityNode <T> previous = null;
PriorityNode <T> current = queue.Head as PriorityNode <T>;
while (true)
{
if (node.Priority < current.Priority)
{
if (previous == null)
{
queue.AddFront(node);
}
else
{
previous.Next = node;
node.Next = current;
}
break;
}
else if (current.Next == null)
{
queue.AddBack(node);
break;
}
previous = current;
current = queue.Next() as PriorityNode <T>;
}
}
_count++;
}
public void enque(U priority, T item)
{
++size;
Node <PriorityNode> prev = null;
foreach (Node <PriorityNode> current in priorityQueue)
{
int cmp = priority.CompareTo(current.Item.Priority);
if (cmp == 1)
{
break;
}
else if (cmp == 0)
{
current.Item.Queue.enque(item);
return;
}
prev = current;
}
PriorityNode newPriorityNode = new PriorityNode(priority);
priorityQueue.enqueAfter(prev, newPriorityNode);
newPriorityNode.Queue.enque(item);
}
public int GetHighestPriority()
{
PriorityNode node = elements[elements.Count - 1];
return(node.Priority);
}
public T peek()
{
PriorityNode node = elements[elements.Count - 1];
return(node.Data);
}
public T Peek()
{
PriorityNode peeked = pq.Peek();
return(peeked.GetItem());
}
public T Pop()
{
PriorityNode popped = pq.Pop();
return(popped.GetItem());
}
/// <summary>
/// Clears the queue
/// </summary>
public void Clear()
{
root = null;
}
public int CompareTo(Object o)
{
PriorityNode otherNode = (PriorityNode)o;
return(priority - otherNode.priority);
}
/// <summary>
/// Copy constructor
/// </summary>
/// <param name="node"></param>
public PriorityNode(PriorityNode <V> node)
{
this.item = node.item;
this.priority = node.priority;
this.index = node.index;
}
public static IEnumerable <TNode> FindPath(
TNode start,
TNode end,
Func <TNode, IEnumerable <TNode> > getNeighbors,
Func <TNode, TNode, float> getCost,
Func <TNode, TNode, float> getHeuristicCost,
IEqualityComparer <TNode> equalityComparer,
int maxNodes = 1024)
{
var openList = new FastPriorityQueue <PriorityNode>(maxNodes);
var closedList = new HashSet <PriorityNode>(new PriorityNodeEqualityComparer(equalityComparer));
var startNode = new PriorityNode(start, getHeuristicCost(start, end));
openList.Enqueue(startNode, startNode.Priority);
while (openList.Any())
{
var currentNode = openList.Dequeue();
if (equalityComparer.Equals(currentNode.Node, end))
{
var path = new List <TNode>();
while (currentNode.Parent != null)
{
path.Add(currentNode.Node);
currentNode = currentNode.Parent;
}
return(((IEnumerable <TNode>)path).Reverse());
}
closedList.Add(currentNode);
foreach (var neighbor in getNeighbors(currentNode.Node))
{
var cost = currentNode.RealCost + getCost(currentNode.Node, neighbor);
if (float.IsInfinity(cost))
{
continue;
}
var openListNeighbor = openList.FirstOrDefault(x => equalityComparer.Equals(x.Node, neighbor));
if (openListNeighbor != null && cost < openListNeighbor.RealCost)
{
openList.Remove(openListNeighbor);
openListNeighbor = null;
}
var closedListNeighbor = closedList.FirstOrDefault(x => equalityComparer.Equals(x.Node, neighbor));
if (closedListNeighbor != null && cost < closedListNeighbor.RealCost)
{
closedList.Remove(closedListNeighbor);
closedListNeighbor = null;
}
if (openListNeighbor == null && closedListNeighbor == null)
{
if (openList.Count == openList.MaxSize)
{
continue;
}
var realCost = currentNode.RealCost + cost;
var heuristicCost = getHeuristicCost(neighbor, end);
var newNode = new PriorityNode(currentNode, neighbor, realCost, heuristicCost);
openList.Enqueue(newNode, newNode.Priority);
}
}
}
return(new List <TNode>());
}
