Curve Fitting: Add alternate 'refit' method
[blender-staging.git] / extern / curve_fit_nd / intern / generic_heap.c
1 /*
2  * Copyright (c) 2016, Blender Foundation.
3  *
4  * Redistribution and use in source and binary forms, with or without
5  * modification, are permitted provided that the following conditions are met:
6  *     * Redistributions of source code must retain the above copyright
7  *       notice, this list of conditions and the following disclaimer.
8  *     * Redistributions in binary form must reproduce the above copyright
9  *       notice, this list of conditions and the following disclaimer in the
10  *       documentation and/or other materials provided with the distribution.
11  *     * Neither the name of the <organization> nor the
12  *       names of its contributors may be used to endorse or promote products
13  *       derived from this software without specific prior written permission.
14  *
15  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
16  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
17  * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
18  * DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
19  * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
20  * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
21  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
22  * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
23  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
24  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
25  */
26
27 /** \file generic_heap.c
28  *  \ingroup curve_fit
29  */
30
31 #include <stdlib.h>
32 #include <string.h>
33 #include <stdbool.h>
34 #include <assert.h>
35
36 #include "generic_heap.h"
37
38 /* swap with a temp value */
39 #define SWAP_TVAL(tval, a, b)  {  \
40         (tval) = (a);                 \
41         (a) = (b);                    \
42         (b) = (tval);                 \
43 } (void)0
44
45 #ifdef __GNUC__
46 #  define UNLIKELY(x)     __builtin_expect(!!(x), 0)
47 #else
48 #  define UNLIKELY(x)     (x)
49 #endif
50
51
52 /***/
53
54 struct HeapNode {
55         void        *ptr;
56         double       value;
57         unsigned int index;
58 };
59
60 /* heap_* pool allocator */
61 #define TPOOL_IMPL_PREFIX  heap
62 #define TPOOL_ALLOC_TYPE   HeapNode
63 #define TPOOL_STRUCT       HeapMemPool
64 #include "generic_alloc_impl.h"
65 #undef TPOOL_IMPL_PREFIX
66 #undef TPOOL_ALLOC_TYPE
67 #undef TPOOL_STRUCT
68
69 struct Heap {
70         unsigned int size;
71         unsigned int bufsize;
72         HeapNode **tree;
73
74         struct HeapMemPool pool;
75 };
76
77 /** \name Internal Functions
78  * \{ */
79
80 #define HEAP_PARENT(i) (((i) - 1) >> 1)
81 #define HEAP_LEFT(i)   (((i) << 1) + 1)
82 #define HEAP_RIGHT(i)  (((i) << 1) + 2)
83 #define HEAP_COMPARE(a, b) ((a)->value < (b)->value)
84
85 #if 0  /* UNUSED */
86 #define HEAP_EQUALS(a, b) ((a)->value == (b)->value)
87 #endif
88
89 static void heap_swap(Heap *heap, const unsigned int i, const unsigned int j)
90 {
91
92 #if 0
93         SWAP(unsigned int,  heap->tree[i]->index, heap->tree[j]->index);
94         SWAP(HeapNode *,    heap->tree[i],        heap->tree[j]);
95 #else
96         HeapNode **tree = heap->tree;
97         union {
98                 unsigned int  index;
99                 HeapNode     *node;
100         } tmp;
101         SWAP_TVAL(tmp.index, tree[i]->index, tree[j]->index);
102         SWAP_TVAL(tmp.node,  tree[i],        tree[j]);
103 #endif
104 }
105
106 static void heap_down(Heap *heap, unsigned int i)
107 {
108         /* size won't change in the loop */
109         const unsigned int size = heap->size;
110
111         while (1) {
112                 const unsigned int l = HEAP_LEFT(i);
113                 const unsigned int r = HEAP_RIGHT(i);
114                 unsigned int smallest;
115
116                 smallest = ((l < size) && HEAP_COMPARE(heap->tree[l], heap->tree[i])) ? l : i;
117
118                 if ((r < size) && HEAP_COMPARE(heap->tree[r], heap->tree[smallest])) {
119                         smallest = r;
120                 }
121
122                 if (smallest == i) {
123                         break;
124                 }
125
126                 heap_swap(heap, i, smallest);
127                 i = smallest;
128         }
129 }
130
131 static void heap_up(Heap *heap, unsigned int i)
132 {
133         while (i > 0) {
134                 const unsigned int p = HEAP_PARENT(i);
135
136                 if (HEAP_COMPARE(heap->tree[p], heap->tree[i])) {
137                         break;
138                 }
139                 heap_swap(heap, p, i);
140                 i = p;
141         }
142 }
143
144 /** \} */
145
146
147 /** \name Public Heap API
148  * \{ */
149
150 /* use when the size of the heap is known in advance */
151 Heap *HEAP_new(unsigned int tot_reserve)
152 {
153         Heap *heap = malloc(sizeof(Heap));
154         /* ensure we have at least one so we can keep doubling it */
155         heap->size = 0;
156         heap->bufsize = tot_reserve ? tot_reserve : 1;
157         heap->tree = malloc(heap->bufsize * sizeof(HeapNode *));
158
159         heap_pool_create(&heap->pool, tot_reserve);
160
161         return heap;
162 }
163
164 void HEAP_free(Heap *heap, HeapFreeFP ptrfreefp)
165 {
166         if (ptrfreefp) {
167                 unsigned int i;
168
169                 for (i = 0; i < heap->size; i++) {
170                         ptrfreefp(heap->tree[i]->ptr);
171                 }
172         }
173
174         heap_pool_destroy(&heap->pool);
175
176         free(heap->tree);
177         free(heap);
178 }
179
180 void HEAP_clear(Heap *heap, HeapFreeFP ptrfreefp)
181 {
182         if (ptrfreefp) {
183                 unsigned int i;
184
185                 for (i = 0; i < heap->size; i++) {
186                         ptrfreefp(heap->tree[i]->ptr);
187                 }
188         }
189         heap->size = 0;
190
191         heap_pool_clear(&heap->pool);
192 }
193
194 HeapNode *HEAP_insert(Heap *heap, double value, void *ptr)
195 {
196         HeapNode *node;
197
198         if (UNLIKELY(heap->size >= heap->bufsize)) {
199                 heap->bufsize *= 2;
200                 heap->tree = realloc(heap->tree, heap->bufsize * sizeof(*heap->tree));
201         }
202
203         node = heap_pool_elem_alloc(&heap->pool);
204
205         node->ptr = ptr;
206         node->value = value;
207         node->index = heap->size;
208
209         heap->tree[node->index] = node;
210
211         heap->size++;
212
213         heap_up(heap, node->index);
214
215         return node;
216 }
217
218 bool HEAP_is_empty(Heap *heap)
219 {
220         return (heap->size == 0);
221 }
222
223 unsigned int HEAP_size(Heap *heap)
224 {
225         return heap->size;
226 }
227
228 HeapNode *HEAP_top(Heap *heap)
229 {
230         return heap->tree[0];
231 }
232
233 double HEAP_top_value(Heap *heap)
234 {
235         return heap->tree[0]->value;
236 }
237
238 void *HEAP_popmin(Heap *heap)
239 {
240         void *ptr = heap->tree[0]->ptr;
241
242         assert(heap->size != 0);
243
244         heap_pool_elem_free(&heap->pool, heap->tree[0]);
245
246         if (--heap->size) {
247                 heap_swap(heap, 0, heap->size);
248                 heap_down(heap, 0);
249         }
250
251         return ptr;
252 }
253
254 void HEAP_remove(Heap *heap, HeapNode *node)
255 {
256         unsigned int i = node->index;
257
258         assert(heap->size != 0);
259
260         while (i > 0) {
261                 unsigned int p = HEAP_PARENT(i);
262
263                 heap_swap(heap, p, i);
264                 i = p;
265         }
266
267         HEAP_popmin(heap);
268 }
269
270 double HEAP_node_value(HeapNode *node)
271 {
272         return node->value;
273 }
274
275 void *HEAP_node_ptr(HeapNode *node)
276 {
277         return node->ptr;
278 }