Merge branch 'master' into blender2.8
[blender.git] / source / blender / blenlib / intern / BLI_heap.c
1 /*
2  * ***** BEGIN GPL LICENSE BLOCK *****
3  *
4  * This program is free software; you can redistribute it and/or
5  * modify it under the terms of the GNU General Public License
6  * as published by the Free Software Foundation; either version 2
7  * of the License, or (at your option) any later version.
8  *
9  * This program is distributed in the hope that it will be useful,
10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
12  * GNU General Public License for more details.
13  *
14  * You should have received a copy of the GNU General Public License
15  * along with this program; if not, write to the Free Software Foundation,
16  * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
17  *
18  * Contributor(s): Brecht Van Lommel
19  *
20  * ***** END GPL LICENSE BLOCK *****
21  */
22
23 /** \file blender/blenlib/intern/BLI_heap.c
24  *  \ingroup bli
25  *
26  * A min-heap / priority queue ADT.
27  */
28
29 #include <stdlib.h>
30 #include <string.h>
31
32 #include "MEM_guardedalloc.h"
33
34 #include "BLI_utildefines.h"
35 #include "BLI_heap.h"
36 #include "BLI_strict_flags.h"
37
38 /***/
39
40 struct HeapNode {
41         float value;
42         uint  index;
43         void *ptr;
44 };
45
46 struct HeapNode_Chunk {
47         struct HeapNode_Chunk *prev;
48         uint size;
49         uint bufsize;
50         struct HeapNode buf[0];
51 };
52
53 /**
54  * Number of nodes to include per #HeapNode_Chunk when no reserved size is passed,
55  * or we allocate past the reserved number.
56  *
57  * \note Optimize number for 64kb allocs.
58  * \note keep type in sync with tot_nodes in heap_node_alloc_chunk.
59  */
60 #define HEAP_CHUNK_DEFAULT_NUM \
61         ((uint)((MEM_SIZE_OPTIMAL((1 << 16) - sizeof(struct HeapNode_Chunk))) / sizeof(HeapNode)))
62
63 struct Heap {
64         uint size;
65         uint bufsize;
66         HeapNode **tree;
67
68         struct {
69                 /* Always keep at least one chunk (never NULL) */
70                 struct HeapNode_Chunk *chunk;
71                 /* when NULL, allocate a new chunk */
72                 HeapNode *free;
73         } nodes;
74 };
75
76 /** \name Internal Functions
77  * \{ */
78
79 #define HEAP_PARENT(i) (((i) - 1) >> 1)
80 #define HEAP_LEFT(i)   (((i) << 1) + 1)
81 #define HEAP_RIGHT(i)  (((i) << 1) + 2)
82 #define HEAP_COMPARE(a, b) ((a)->value < (b)->value)
83
84 #if 0  /* UNUSED */
85 #define HEAP_EQUALS(a, b) ((a)->value == (b)->value)
86 #endif
87
88 BLI_INLINE void heap_swap(Heap *heap, const uint i, const uint j)
89 {
90 #if 1
91         HeapNode **tree = heap->tree;
92         HeapNode *pi = tree[i], *pj = tree[j];
93         pi->index = j;
94         tree[j] = pi;
95         pj->index = i;
96         tree[i] = pj;
97 #elif 0
98         SWAP(uint,  heap->tree[i]->index, heap->tree[j]->index);
99         SWAP(HeapNode *,    heap->tree[i],        heap->tree[j]);
100 #else
101         HeapNode **tree = heap->tree;
102         union {
103                 uint  index;
104                 HeapNode     *node;
105         } tmp;
106         SWAP_TVAL(tmp.index, tree[i]->index, tree[j]->index);
107         SWAP_TVAL(tmp.node,  tree[i],        tree[j]);
108 #endif
109 }
110
111 static void heap_down(Heap *heap, uint i)
112 {
113         /* size won't change in the loop */
114         HeapNode **const tree = heap->tree;
115         const uint size = heap->size;
116
117         while (1) {
118                 const uint l = HEAP_LEFT(i);
119                 const uint r = HEAP_RIGHT(i);
120                 uint smallest = i;
121
122                 if (LIKELY(l < size) && HEAP_COMPARE(tree[l], tree[smallest])) {
123                         smallest = l;
124                 }
125                 if (LIKELY(r < size) && HEAP_COMPARE(tree[r], tree[smallest])) {
126                         smallest = r;
127                 }
128
129                 if (UNLIKELY(smallest == i)) {
130                         break;
131                 }
132
133                 heap_swap(heap, i, smallest);
134                 i = smallest;
135         }
136 }
137
138 static void heap_up(Heap *heap, uint i)
139 {
140         HeapNode **const tree = heap->tree;
141
142         while (LIKELY(i > 0)) {
143                 const uint p = HEAP_PARENT(i);
144
145                 if (HEAP_COMPARE(tree[p], tree[i])) {
146                         break;
147                 }
148                 heap_swap(heap, p, i);
149                 i = p;
150         }
151 }
152
153 /** \} */
154
155
156 /** \name Internal Memory Management
157  * \{ */
158
159 static struct HeapNode_Chunk *heap_node_alloc_chunk(
160         uint tot_nodes, struct HeapNode_Chunk *chunk_prev)
161 {
162         struct HeapNode_Chunk *chunk = MEM_mallocN(
163                 sizeof(struct HeapNode_Chunk) + (sizeof(HeapNode) * tot_nodes), __func__);
164         chunk->prev = chunk_prev;
165         chunk->bufsize = tot_nodes;
166         chunk->size = 0;
167         return chunk;
168 }
169
170 static struct HeapNode *heap_node_alloc(Heap *heap)
171 {
172         HeapNode *node;
173
174         if (heap->nodes.free) {
175                 node = heap->nodes.free;
176                 heap->nodes.free = heap->nodes.free->ptr;
177         }
178         else {
179                 struct HeapNode_Chunk *chunk = heap->nodes.chunk;
180                 if (UNLIKELY(chunk->size == chunk->bufsize)) {
181                         chunk = heap->nodes.chunk = heap_node_alloc_chunk(HEAP_CHUNK_DEFAULT_NUM, chunk);
182                 }
183                 node = &chunk->buf[chunk->size++];
184         }
185
186         return node;
187 }
188
189 static void heap_node_free(Heap *heap, HeapNode *node)
190 {
191         node->ptr = heap->nodes.free;
192         heap->nodes.free = node;
193 }
194
195 /** \} */
196
197
198 /** \name Public Heap API
199  * \{ */
200
201 /**
202  * Creates a new heap. Removed nodes are recycled, so memory usage will not shrink.
203  *
204  * \note Use when the size of the heap is known in advance.
205  */
206 Heap *BLI_heap_new_ex(uint tot_reserve)
207 {
208         Heap *heap = MEM_mallocN(sizeof(Heap), __func__);
209         /* ensure we have at least one so we can keep doubling it */
210         heap->size = 0;
211         heap->bufsize = MAX2(1u, tot_reserve);
212         heap->tree = MEM_mallocN(heap->bufsize * sizeof(HeapNode *), "BLIHeapTree");
213
214         heap->nodes.chunk = heap_node_alloc_chunk((tot_reserve > 1) ? tot_reserve : HEAP_CHUNK_DEFAULT_NUM, NULL);
215         heap->nodes.free = NULL;
216
217         return heap;
218 }
219
220 Heap *BLI_heap_new(void)
221 {
222         return BLI_heap_new_ex(1);
223 }
224
225 void BLI_heap_free(Heap *heap, HeapFreeFP ptrfreefp)
226 {
227         if (ptrfreefp) {
228                 uint i;
229
230                 for (i = 0; i < heap->size; i++) {
231                         ptrfreefp(heap->tree[i]->ptr);
232                 }
233         }
234
235         struct HeapNode_Chunk *chunk = heap->nodes.chunk;
236         do {
237                 struct HeapNode_Chunk *chunk_prev;
238                 chunk_prev = chunk->prev;
239                 MEM_freeN(chunk);
240                 chunk = chunk_prev;
241         } while (chunk);
242
243         MEM_freeN(heap->tree);
244         MEM_freeN(heap);
245 }
246
247 void BLI_heap_clear(Heap *heap, HeapFreeFP ptrfreefp)
248 {
249         if (ptrfreefp) {
250                 uint i;
251
252                 for (i = 0; i < heap->size; i++) {
253                         ptrfreefp(heap->tree[i]->ptr);
254                 }
255         }
256         heap->size = 0;
257
258         /* Remove all except the last chunk */
259         while (heap->nodes.chunk->prev) {
260                 struct HeapNode_Chunk *chunk_prev = heap->nodes.chunk->prev;
261                 MEM_freeN(heap->nodes.chunk);
262                 heap->nodes.chunk = chunk_prev;
263         }
264         heap->nodes.chunk->size = 0;
265         heap->nodes.free = NULL;
266 }
267
268 /**
269  * Insert heap node with a value (often a 'cost') and pointer into the heap,
270  * duplicate values are allowed.
271  */
272 HeapNode *BLI_heap_insert(Heap *heap, float value, void *ptr)
273 {
274         HeapNode *node;
275
276         if (UNLIKELY(heap->size >= heap->bufsize)) {
277                 heap->bufsize *= 2;
278                 heap->tree = MEM_reallocN(heap->tree, heap->bufsize * sizeof(*heap->tree));
279         }
280
281         node = heap_node_alloc(heap);
282
283         node->ptr = ptr;
284         node->value = value;
285         node->index = heap->size;
286
287         heap->tree[node->index] = node;
288
289         heap->size++;
290
291         heap_up(heap, node->index);
292
293         return node;
294 }
295
296 /**
297  * Convenience function since this is a common pattern.
298  */
299 void BLI_heap_insert_or_update(Heap *heap, HeapNode **node_p, float value, void *ptr)
300 {
301         if (*node_p == NULL) {
302                 *node_p = BLI_heap_insert(heap, value, ptr);
303         }
304         else {
305                 BLI_heap_node_value_update_ptr(heap, *node_p, value, ptr);
306         }
307 }
308
309
310 bool BLI_heap_is_empty(const Heap *heap)
311 {
312         return (heap->size == 0);
313 }
314
315 uint BLI_heap_len(const Heap *heap)
316 {
317         return heap->size;
318 }
319
320 /**
321  * Return the top node of the heap.
322  * This is the node with the lowest value.
323  */
324 HeapNode *BLI_heap_top(const Heap *heap)
325 {
326         return heap->tree[0];
327 }
328
329 /**
330  * Return the value of top node of the heap.
331  * This is the node with the lowest value.
332  */
333 float BLI_heap_top_value(const Heap *heap)
334 {
335         BLI_assert(heap->size != 0);
336
337         return heap->tree[0]->value;
338 }
339
340 /**
341  * Pop the top node off the heap and return it's pointer.
342  */
343 void *BLI_heap_pop_min(Heap *heap)
344 {
345         BLI_assert(heap->size != 0);
346
347         void *ptr = heap->tree[0]->ptr;
348
349         heap_node_free(heap, heap->tree[0]);
350
351         if (--heap->size) {
352                 heap_swap(heap, 0, heap->size);
353                 heap_down(heap, 0);
354         }
355
356         return ptr;
357 }
358
359 void BLI_heap_remove(Heap *heap, HeapNode *node)
360 {
361         BLI_assert(heap->size != 0);
362
363         uint i = node->index;
364
365         while (i > 0) {
366                 uint p = HEAP_PARENT(i);
367                 heap_swap(heap, p, i);
368                 i = p;
369         }
370
371         BLI_heap_pop_min(heap);
372 }
373
374 /**
375  * Can be used to avoid #BLI_heap_remove, #BLI_heap_insert calls,
376  * balancing the tree still has a performance cost,
377  * but is often much less than remove/insert, difference is most noticable with large heaps.
378  */
379 void BLI_heap_node_value_update(Heap *heap, HeapNode *node, float value)
380 {
381         if (value < node->value) {
382                 node->value = value;
383                 heap_up(heap, node->index);
384         }
385         else if (value > node->value) {
386                 node->value = value;
387                 heap_down(heap, node->index);
388         }
389 }
390
391 void BLI_heap_node_value_update_ptr(Heap *heap, HeapNode *node, float value, void *ptr)
392 {
393         node->ptr = ptr; /* only difference */
394         if (value < node->value) {
395                 node->value = value;
396                 heap_up(heap, node->index);
397         }
398         else if (value > node->value) {
399                 node->value = value;
400                 heap_down(heap, node->index);
401         }
402 }
403
404 float BLI_heap_node_value(const HeapNode *node)
405 {
406         return node->value;
407 }
408
409 void *BLI_heap_node_ptr(const HeapNode *node)
410 {
411         return node->ptr;
412 }
413
414 static bool heap_is_minheap(const Heap *heap, uint root)
415 {
416         if (root < heap->size) {
417                 if (heap->tree[root]->index != root) {
418                         return false;
419                 }
420                 const uint l = HEAP_LEFT(root);
421                 if (l < heap->size) {
422                         if (HEAP_COMPARE(heap->tree[l], heap->tree[root]) || !heap_is_minheap(heap, l)) {
423                                 return false;
424                         }
425                 }
426                 const uint r = HEAP_RIGHT(root);
427                 if (r < heap->size) {
428                         if (HEAP_COMPARE(heap->tree[r], heap->tree[root]) || !heap_is_minheap(heap, r)) {
429                                 return false;
430                         }
431                 }
432         }
433         return true;
434 }
435 /**
436  * Only for checking internal errors (gtest).
437  */
438 bool BLI_heap_is_valid(const Heap *heap)
439 {
440         return heap_is_minheap(heap, 0);
441 }
442
443 /** \} */