-
Notifications
You must be signed in to change notification settings - Fork 1
/
Heap.cs
208 lines (154 loc) · 5.39 KB
/
Heap.cs
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
using System;
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
namespace hutian.AI.PathFinding
{
//Heap from https://www.youtube.com/watch?v=3Dw5d7PlcTM
//T in this case will be Node
//T : IHeapItem<T> means that the Node has to implement the interface
public class Heap<T> where T : IHeapItem<T>
{
//The array that will hold the heap
private T[] items;
//How many nodes we have stored in the heap
private int currentItemCount;
//How many items can we have in the heap?
public Heap(int maxHeapSize)
{
items = new T[maxHeapSize];
}
//Add new item to the heap
public void Add(T item)
{
//Do we have room to add it?
if (currentItemCount + 1 > items.Length)
{
//Debug.Log("Cant add item to heap becuse it's full");
return;
}
item.HeapIndex = currentItemCount;
//Add the item to the end of the array
items[currentItemCount] = item;
//But it may belong to another position in the heap
SortUp(item);
currentItemCount += 1;
}
//Remove the first item from the heap, which is the node with the lowest f cost
public T RemoveFirst()
{
T firstItem = items[0];
currentItemCount -= 1;
//To resort the heap, we add the last item in the array to the first position in the array
items[0] = items[currentItemCount];
items[0].HeapIndex = 0;
//And then move the first item to where it belongs in the array
SortDown(items[0]);
return firstItem;
}
//How many items do we have in the heap?
public int Count
{
get
{
return currentItemCount;
}
}
//Does the heap contain this item?
public bool Contains(T item)
{
return Equals(items[item.HeapIndex], item);
}
//Update an item already in the heap, but we need to change its priority in the heap
public void UpdateItem(T item)
{
//This is for pathfinding so we only need to add better nodes and thus only need to sort up
SortUp(item); //更新后节点的总代价一定比之前更小,所以做上浮操作
}
//Clear the array
public void Clear()
{
Array.Clear(items, 0, items.Length);
currentItemCount = 0;
}
//
// Heap mechanics
//
//Sorts and item down in the array to the position where it belongs
private void SortDown(T item)
{
while (true)
{
//From heap index to array index
int childIndexLeft = item.HeapIndex * 2 + 1;
int childIndexRight = item.HeapIndex * 2 + 2;
int swapIndex = 0;
//Do we have a children to the left
if (childIndexLeft < currentItemCount)
{
swapIndex = childIndexLeft;
//But we also need to check if we have a children to the right
if (childIndexRight < currentItemCount)
{
//Compare the left and the right node, to find if we should swap with the left or the right node
if (items[childIndexLeft].CompareTo(items[childIndexRight]) < 0)//右子节点比左子节点更小,那么交换父元素与右子节点
{
swapIndex = childIndexRight;
}
}
if (item.CompareTo(items[swapIndex]) < 0)
{
Swap(item, items[swapIndex]);
}
else
{
return;
}
}
else
{
return;
}
}
}
//Sorts an item up in the array to the position where it belongs
private void SortUp(T item)
{
//From heap index to array index
int parentIndex = (item.HeapIndex - 1) / 2;
while (true)
{
T parentItem = items[parentIndex];
//If item has a lower f cost than the parent
if (item.CompareTo(parentItem) > 0)
{
Swap(item, parentItem);
}
else
{
break;
}
parentIndex = (item.HeapIndex - 1) / 2;
}
}
//Swap 2 items in the heap, which is the same as moving one item up (or down) and the other item down (or up)
private void Swap(T itemA, T itemB)
{
items[itemA.HeapIndex] = itemB;
items[itemB.HeapIndex] = itemA;
//We also need to swap the heap indexes
int itemAIndex = itemA.HeapIndex;
itemA.HeapIndex = itemB.HeapIndex;
itemB.HeapIndex = itemAIndex;
}
}
//Each node has to implement this, so both HeapIndex and CompareTo
public interface IHeapItem<T> : IComparable<T>
{
int HeapIndex
{
get;
set;
}
}
}