gk_mkpqueue.h
Go to the documentation of this file.
1 
11 #ifndef _GK_MKPQUEUE_H
12 #define _GK_MKPQUEUE_H
13 
14 
15 #define GK_MKPQUEUE(FPRFX, PQT, KVT, KT, VT, KVMALLOC, KMAX, KEY_LT)\
16 /*************************************************************************/\
17 \
18 /**************************************************************************/\
19 PQT *FPRFX ## Create(size_t maxnodes)\
20 {\
21  PQT *queue; \
22 \
23  queue = (PQT *)gk_malloc(sizeof(PQT), "gk_pqCreate: queue");\
24  FPRFX ## Init(queue, maxnodes);\
25 \
26  return queue;\
27 }\
28 \
29 \
30 /*************************************************************************/\
31 \
32 /**************************************************************************/\
33 void FPRFX ## Init(PQT *queue, size_t maxnodes)\
34 {\
35  queue->nnodes = 0;\
36  queue->maxnodes = maxnodes;\
37 \
38  queue->heap = KVMALLOC(maxnodes, "gk_PQInit: heap");\
39  queue->locator = gk_idxsmalloc(maxnodes, -1, "gk_PQInit: locator");\
40 }\
41 \
42 \
43 /*************************************************************************/\
44 \
45 /**************************************************************************/\
46 void FPRFX ## Reset(PQT *queue)\
47 {\
48  gk_idx_t i;\
49  gk_idx_t *locator=queue->locator;\
50  KVT *heap=queue->heap;\
51 \
52  for (i=queue->nnodes-1; i>=0; i--)\
53  locator[heap[i].val] = -1;\
54  queue->nnodes = 0;\
55 }\
56 \
57 \
58 /*************************************************************************/\
59 \
60 /**************************************************************************/\
61 void FPRFX ## Free(PQT *queue)\
62 {\
63  if (queue == NULL) return;\
64  gk_free((void **)&queue->heap, &queue->locator, LTERM);\
65  queue->maxnodes = 0;\
66 }\
67 \
68 \
69 /*************************************************************************/\
70 \
72 /**************************************************************************/\
73 void FPRFX ## Destroy(PQT *queue)\
74 {\
75  if (queue == NULL) return;\
76  FPRFX ## Free(queue);\
77  gk_free((void **)&queue, LTERM);\
78 }\
79 \
80 \
81 /*************************************************************************/\
82 \
83 /**************************************************************************/\
84 size_t FPRFX ## Length(PQT *queue)\
85 {\
86  return queue->nnodes;\
87 }\
88 \
89 \
90 /*************************************************************************/\
91 \
92 /**************************************************************************/\
93 int FPRFX ## Insert(PQT *queue, VT node, KT key)\
94 {\
95  gk_idx_t i, j;\
96  gk_idx_t *locator=queue->locator;\
97  KVT *heap=queue->heap;\
98 \
99  ASSERT2(FPRFX ## CheckHeap(queue));\
100 \
101  ASSERT(locator[node] == -1);\
102 \
103  i = queue->nnodes++;\
104  while (i > 0) {\
105  j = (i-1)>>1;\
106  if (KEY_LT(key, heap[j].key)) {\
107  heap[i] = heap[j];\
108  locator[heap[i].val] = i;\
109  i = j;\
110  }\
111  else\
112  break;\
113  }\
114  ASSERT(i >= 0);\
115  heap[i].key = key;\
116  heap[i].val = node;\
117  locator[node] = i;\
118 \
119  ASSERT2(FPRFX ## CheckHeap(queue));\
120 \
121  return 0;\
122 }\
123 \
124 \
125 /*************************************************************************/\
126 \
127 /**************************************************************************/\
128 int FPRFX ## Delete(PQT *queue, VT node)\
129 {\
130  gk_idx_t i, j, nnodes;\
131  KT newkey, oldkey;\
132  gk_idx_t *locator=queue->locator;\
133  KVT *heap=queue->heap;\
134 \
135  ASSERT(locator[node] != -1);\
136  ASSERT(heap[locator[node]].val == node);\
137 \
138  ASSERT2(FPRFX ## CheckHeap(queue));\
139 \
140  i = locator[node];\
141  locator[node] = -1;\
142 \
143  if (--queue->nnodes > 0 && heap[queue->nnodes].val != node) {\
144  node = heap[queue->nnodes].val;\
145  newkey = heap[queue->nnodes].key;\
146  oldkey = heap[i].key;\
147 \
148  if (KEY_LT(newkey, oldkey)) { /* Filter-up */\
149  while (i > 0) {\
150  j = (i-1)>>1;\
151  if (KEY_LT(newkey, heap[j].key)) {\
152  heap[i] = heap[j];\
153  locator[heap[i].val] = i;\
154  i = j;\
155  }\
156  else\
157  break;\
158  }\
159  }\
160  else { /* Filter down */\
161  nnodes = queue->nnodes;\
162  while ((j=(i<<1)+1) < nnodes) {\
163  if (KEY_LT(heap[j].key, newkey)) {\
164  if (j+1 < nnodes && KEY_LT(heap[j+1].key, heap[j].key))\
165  j++;\
166  heap[i] = heap[j];\
167  locator[heap[i].val] = i;\
168  i = j;\
169  }\
170  else if (j+1 < nnodes && KEY_LT(heap[j+1].key, newkey)) {\
171  j++;\
172  heap[i] = heap[j];\
173  locator[heap[i].val] = i;\
174  i = j;\
175  }\
176  else\
177  break;\
178  }\
179  }\
180 \
181  heap[i].key = newkey;\
182  heap[i].val = node;\
183  locator[node] = i;\
184  }\
185 \
186  ASSERT2(FPRFX ## CheckHeap(queue));\
187 \
188  return 0;\
189 }\
190 \
191 \
192 /*************************************************************************/\
193  \
194 /**************************************************************************/\
195 void FPRFX ## Update(PQT *queue, VT node, KT newkey)\
196 {\
197  gk_idx_t i, j, nnodes;\
198  KT oldkey;\
199  gk_idx_t *locator=queue->locator;\
200  KVT *heap=queue->heap;\
201 \
202  oldkey = heap[locator[node]].key;\
203 \
204  ASSERT(locator[node] != -1);\
205  ASSERT(heap[locator[node]].val == node);\
206  ASSERT2(FPRFX ## CheckHeap(queue));\
207 \
208  i = locator[node];\
209 \
210  if (KEY_LT(newkey, oldkey)) { /* Filter-up */\
211  while (i > 0) {\
212  j = (i-1)>>1;\
213  if (KEY_LT(newkey, heap[j].key)) {\
214  heap[i] = heap[j];\
215  locator[heap[i].val] = i;\
216  i = j;\
217  }\
218  else\
219  break;\
220  }\
221  }\
222  else { /* Filter down */\
223  nnodes = queue->nnodes;\
224  while ((j=(i<<1)+1) < nnodes) {\
225  if (KEY_LT(heap[j].key, newkey)) {\
226  if (j+1 < nnodes && KEY_LT(heap[j+1].key, heap[j].key))\
227  j++;\
228  heap[i] = heap[j];\
229  locator[heap[i].val] = i;\
230  i = j;\
231  }\
232  else if (j+1 < nnodes && KEY_LT(heap[j+1].key, newkey)) {\
233  j++;\
234  heap[i] = heap[j];\
235  locator[heap[i].val] = i;\
236  i = j;\
237  }\
238  else\
239  break;\
240  }\
241  }\
242 \
243  heap[i].key = newkey;\
244  heap[i].val = node;\
245  locator[node] = i;\
246 \
247  ASSERT2(FPRFX ## CheckHeap(queue));\
248 \
249  return;\
250 }\
251 \
252 \
253 /*************************************************************************/\
254 \
256 /**************************************************************************/\
257 VT FPRFX ## GetTop(PQT *queue)\
258 {\
259  gk_idx_t i, j;\
260  gk_idx_t *locator;\
261  KVT *heap;\
262  VT vtx, node;\
263  KT key;\
264 \
265  ASSERT2(FPRFX ## CheckHeap(queue));\
266 \
267  if (queue->nnodes == 0)\
268  return -1;\
269 \
270  queue->nnodes--;\
271 \
272  heap = queue->heap;\
273  locator = queue->locator;\
274 \
275  vtx = heap[0].val;\
276  locator[vtx] = -1;\
277 \
278  if ((i = queue->nnodes) > 0) {\
279  key = heap[i].key;\
280  node = heap[i].val;\
281  i = 0;\
282  while ((j=2*i+1) < queue->nnodes) {\
283  if (KEY_LT(heap[j].key, key)) {\
284  if (j+1 < queue->nnodes && KEY_LT(heap[j+1].key, heap[j].key))\
285  j = j+1;\
286  heap[i] = heap[j];\
287  locator[heap[i].val] = i;\
288  i = j;\
289  }\
290  else if (j+1 < queue->nnodes && KEY_LT(heap[j+1].key, key)) {\
291  j = j+1;\
292  heap[i] = heap[j];\
293  locator[heap[i].val] = i;\
294  i = j;\
295  }\
296  else\
297  break;\
298  }\
299 \
300  heap[i].key = key;\
301  heap[i].val = node;\
302  locator[node] = i;\
303  }\
304 \
305  ASSERT2(FPRFX ## CheckHeap(queue));\
306  return vtx;\
307 }\
308 \
309 \
310 /*************************************************************************/\
311 \
313 /**************************************************************************/\
314 VT FPRFX ## SeeTopVal(PQT *queue)\
315 {\
316  return (queue->nnodes == 0 ? -1 : queue->heap[0].val);\
317 }\
318 \
319 \
320 /*************************************************************************/\
321 \
323 /**************************************************************************/\
324 KT FPRFX ## SeeTopKey(PQT *queue)\
325 {\
326  return (queue->nnodes == 0 ? KMAX : queue->heap[0].key);\
327 }\
328 \
329 \
330 /*************************************************************************/\
331 \
332 /**************************************************************************/\
333 KT FPRFX ## SeeKey(PQT *queue, VT node)\
334 {\
335  gk_idx_t *locator;\
336  KVT *heap;\
337 \
338  heap = queue->heap;\
339  locator = queue->locator;\
340 \
341  return heap[locator[node]].key;\
342 }\
343 \
344 \
345 /*************************************************************************/\
346 \
349 /**************************************************************************/\
350 /*\
351 VT FPRFX ## SeeConstraintTop(PQT *queue, KT maxwgt, KT *wgts)\
352 {\
353  gk_idx_t i;\
354 \
355  if (queue->nnodes == 0)\
356  return -1;\
357 \
358  if (maxwgt <= 1000)\
359  return FPRFX ## SeeTopVal(queue);\
360 \
361  for (i=0; i<queue->nnodes; i++) {\
362  if (queue->heap[i].key > 0) {\
363  if (wgts[queue->heap[i].val] <= maxwgt)\
364  return queue->heap[i].val;\
365  }\
366  else {\
367  if (queue->heap[i/2].key <= 0)\
368  break;\
369  }\
370  }\
371 \
372  return queue->heap[0].val;\
373 \
374 }\
375 */\
376 \
377 \
378 /*************************************************************************/\
379 \
380 /**************************************************************************/\
381 int FPRFX ## CheckHeap(PQT *queue)\
382 {\
383  gk_idx_t i, j;\
384  size_t nnodes;\
385  gk_idx_t *locator;\
386  KVT *heap;\
387 \
388  heap = queue->heap;\
389  locator = queue->locator;\
390  nnodes = queue->nnodes;\
391 \
392  if (nnodes == 0)\
393  return 1;\
394 \
395  ASSERT(locator[heap[0].val] == 0);\
396  for (i=1; i<nnodes; i++) {\
397  ASSERT(locator[heap[i].val] == i);\
398  ASSERT(!KEY_LT(heap[i].key, heap[(i-1)/2].key));\
399  }\
400  for (i=1; i<nnodes; i++)\
401  ASSERT(!KEY_LT(heap[i].key, heap[0].key));\
402 \
403  for (j=i=0; i<queue->maxnodes; i++) {\
404  if (locator[i] != -1)\
405  j++;\
406  }\
407  ASSERTP(j == nnodes, ("%jd %jd\n", (intmax_t)j, (intmax_t)nnodes));\
408 \
409  return 1;\
410 }\
411 
412 
413 #define GK_MKPQUEUE_PROTO(FPRFX, PQT, KT, VT)\
414  PQT * FPRFX ## Create(size_t maxnodes);\
415  void FPRFX ## Init(PQT *queue, size_t maxnodes);\
416  void FPRFX ## Reset(PQT *queue);\
417  void FPRFX ## Free(PQT *queue);\
418  void FPRFX ## Destroy(PQT *queue);\
419  size_t FPRFX ## Length(PQT *queue);\
420  int FPRFX ## Insert(PQT *queue, VT node, KT key);\
421  int FPRFX ## Delete(PQT *queue, VT node);\
422  void FPRFX ## Update(PQT *queue, VT node, KT newkey);\
423  VT FPRFX ## GetTop(PQT *queue);\
424  VT FPRFX ## SeeTopVal(PQT *queue);\
425  KT FPRFX ## SeeTopKey(PQT *queue);\
426  KT FPRFX ## SeeKey(PQT *queue, VT node);\
427  VT FPRFX ## SeeConstraintTop(PQT *queue, KT maxwgt, KT *wgts);\
428  int FPRFX ## CheckHeap(PQT *queue);\
429 
430 
431 /* This is how these macros are used
432 GK_MKPQUEUE(gk_dkvPQ, gk_dkvPQ_t, double, gk_idx_t, gk_dkvmalloc, DBL_MAX)
433 GK_MKPQUEUE_PROTO(gk_dkvPQ, gk_dkvPQ_t, double, gk_idx_t)
434 */
435 
436 
437 #endif

gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:34:18