spatial-trees/pqueue.c at master · mtiberti/spatial-trees · GitHub

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
/*************************************************************************************/
/*  Code to implement the abstract data type priority queue for use in j nearest     */
/*  neighbor searching.  Actual implementation is done using heaps.                  */
/*                                                                                   */
/*  Adapted from Sedgewick's: Algorithms in C p. 148-160.                            */
/*************************************************************************************/

/*
   The heap data structure consists of two priority queues.  One for the j-smallest
   distances encountered, one to keep the indexes into the points array of the
   points corresponding to the j-smallest distances.
*/


/*************************************************************************************/

void PQupheap(DistArr,FoundArr,k)

/*************************************************************************************/

float *DistArr;  /* j-smallest distances encountered */

int *FoundArr,k;

{
float v;
int j;

v=DistArr[k]; DistArr[0] = 999999999999999.0;
j=FoundArr[k];

while(DistArr[k/2] <= v) {
  DistArr[k] = DistArr[k/2];
  FoundArr[k] = FoundArr[k/2];
  k=k/2;
}
DistArr[k] = v;
FoundArr[k] = j;
}

/*************************************************************************************/

void PQInsert(distance,index,DistArr,FoundArr)

/*************************************************************************************/

float distance,*DistArr;
int   index, *FoundArr;

{
  FoundArr[0]=FoundArr[0]+1;
  DistArr[FoundArr[0]] = distance;
  FoundArr[FoundArr[0]] = index;
  PQupheap(DistArr,FoundArr,FoundArr[0]);
}


/*************************************************************************************/

void PQdownheap(DistArr,FoundArr,k,index)

/*************************************************************************************/

float *DistArr;  /* j-smallest distances encountered */

int *FoundArr,k,index;

{

int j,N;
float v;

v=DistArr[k];

N = FoundArr[0];  /* tricky patch to maintain the data structure */
FoundArr[0]=index;

while (k <= N/2) {
  j=k+k;
  if (j < N && DistArr[j] <DistArr[j+1]) j++;
  if (v>=DistArr[j]) break;
  DistArr[k]=DistArr[j];
  FoundArr[k]=FoundArr[j];
  k=j;
}

DistArr[k] = v;
FoundArr[k]= index;
FoundArr[0]=N;  /* restore data struct */


}

/*************************************************************************************/

void PQreplace(distance,DistArr,FoundArr,index)

/*************************************************************************************/

float *DistArr,distance;
int *FoundArr;

{
  DistArr[0]=distance;
  PQdownheap(DistArr,FoundArr,0,index);
}