src/rt/oocnn.c

#ifndef lint
static const char RCSid[] = "$Id: oocnn.c,v 2.2 2017/08/14 21:12:10 rschregle Exp $";
#endif


/* 
   =========================================================================
   k-nearest neighbour lookup routines for out-of-core octree data structure
   
   Roland Schregle (roland.schregle@{hslu.ch, gmail.com})
   (c) Lucerne University of Applied Sciences and Arts,
       supported by the Swiss National Science Foundation (SNSF, #147053)
   =========================================================================
   
   $Id: oocnn.c,v 2.2 2017/08/14 21:12:10 rschregle Exp $
*/


#if !defined(_WIN32) && !defined(_WIN64) || defined(PMAP_OOC)
/* No Windoze support for now */

#include "oocnn.h"
#include "oocsort.h"
#include <stdlib.h>
#include <string.h>
#include <math.h>


#ifdef DEBUG_OOC_NN
static int OOC_SearchQueueCheck (OOC_SearchQueue *queue, const FVECT key,
                                 RREAL *(*keyFunc)(const void*), 
                                 unsigned root)
/* Priority queue sanity check */
{
   unsigned                   kid;
   const OOC_SearchQueueNode  *qn = queue -> node;
   void                       *rec;
   float                      d2;
   
   if (root < queue -> tail) {
      rec = OOC_GetNearest(queue, qn [root].idx);
      d2 = dist2(keyFunc(rec), key);
      
      if (qn [root].dist2 != d2)
         return -1;
      
      if ((kid = (root << 1) + 1) < queue -> tail) {
         if (qn [kid].dist2 > qn [root].dist2)
            return -1;
         else return OOC_SearchQueueCheck(queue, key, keyFunc, kid);
      }
         
      if (++kid < queue -> tail) {
         if (qn [kid].dist2 > qn [root].dist2)
            return -1;
         else return OOC_SearchQueueCheck(queue, key, keyFunc, kid);
      }
   }
   
   return 0;
}
#endif


static float OOC_PutNearest (OOC_SearchQueue *queue, float d2, void *rec)
/* Insert data record with SQUARED distance to query point into search
 * priority queue, maintaining the most distant record at the queue head. 
 * If the queue is full, the new record is only inserted if it is closer
 * than the queue head.
 * Returns the new maximum SQUARED distance at the head if the queue is
 * full.  Otherwise returns -1, indicating a maximum for the entire queue is
 * as yet undefined
 * The data record is copied into the queue's local record buffa for
 * post-search retrieval to minimise redundant disk access. Note that it
 * suffices to only rearrange the corresponding indices in the queue nodes
 * when restoring the priority queue after every insertion, rather than
 * moving the actual records. */
{
   OOC_SearchQueueNode  *qn = queue -> node;
   unsigned             root, kid, kid2, rootIdx;
   float                d2max = -1;    /* Undefined max distance ^2 */
   
   /* The queue is represented as a linearised binary tree with the root
    * corresponding to the queue head, and the tail corresponding to the
    * last leaf */
   if (queue -> tail < queue -> len) {
      /* Enlarge queue if not full, insert at tail and resort towards head */
      kid = queue -> tail++;
   
      while (kid) {
         root = (kid - 1) >> 1;
      
         /* Compare with parent and swap if necessary, else terminate */
         if (d2 > qn [root].dist2) {
            qn [kid].dist2 = qn [root].dist2;
            qn [kid].idx  = qn [root].idx;
            kid = root;
         }
         else break;
      }
      
      /* Assign tail position as linear index into record buffa 
       * queue -> nnRec and append record */
      qn [kid].dist2 = d2;
      qn [kid].idx   = queue -> tail - 1;
      memcpy(OOC_GetNearest(queue, qn [kid].idx), rec, queue -> recSize);
   }
   else if (d2 < qn [0].dist2) {
      /* Queue full and new record closer than maximum at head; replace head
       * and resort towards tail */
      root = 0;
      rootIdx = qn [root].idx;
      
      while ((kid = (root << 1) + 1) < queue -> tail) {
         /* Compare with larger kid & swap if necessary, else terminate */
         if ((kid2 = (kid + 1)) < queue -> tail && 
             qn [kid2].dist2 > qn [kid].dist2)
            kid = kid2;
         
         if (d2 < qn [kid].dist2) {
            qn [root].dist2 = qn [kid].dist2;
            qn [root].idx  = qn [kid].idx;
         }
         else break;
         
         root = kid;
      }
      
      /* Reassign head's previous buffa index and overwrite corresponding
       * record */
      qn [root].dist2   = d2;
      qn [root].idx     = rootIdx;
      memcpy(OOC_GetNearest(queue, qn [root].idx), rec, queue -> recSize);
      
      /* Update SQUARED maximum distance from head node */
      d2max = qn [0].dist2;
   }

   return d2max;
}


int OOC_InitNearest (OOC_SearchQueue *squeue, 
                     unsigned len, unsigned recSize)
{
   squeue -> len = len;
   squeue -> recSize = recSize;
   squeue -> node = calloc(len, sizeof(OOC_SearchQueueNode));
   squeue -> nnRec = calloc(len, recSize); 
   if (!squeue -> node || !squeue -> nnRec) {
      perror("OOC_InitNearest: failed search queue allocation");
      return -1;
   }
   
   return 0;
}


void *OOC_GetNearest (const OOC_SearchQueue *squeue, unsigned idx)
{
   return squeue -> nnRec + idx * squeue -> recSize;
}


static float OOC_BBoxDist2 (const FVECT bbOrg, RREAL bbSiz, const FVECT key)
/* Return minimum *SQUARED* distance between key and bounding box defined by
 * bbOrg and bbSiz; a distance of 0 implies the key lies INSIDE the bbox */
{
   /* Explicit comparison with bbox corners */
   int   i;
   FVECT d;
   
   for (i = 0; i < 3; i++) {
      d [i] = key [i] - bbOrg [i];
      d [i] = d [i] < 0 ? -d [i] : d [i] - bbSiz;
      
      /* Set to 0 if inside bbox */
      if (d [i] < 0)
         d [i] = 0;
   }
   
   return DOT(d, d);
}


float OOC_FindNearest (OOC_Octree *oct, OOC_Node *node, OOC_DataIdx dataIdx,
                       const FVECT org, float size, const FVECT key, 
                       const OOC_SearchFilter *filter, 
                       OOC_SearchQueue *queue, float maxDist2)
{
   const float kidSize = size * 0.5;
   unsigned    i, kid, kid0;
   float       d2;
   char        rec [oct -> recSize];
   FVECT       kidOrg;
   OOC_DataIdx kidDataIdx, recIdx;
   OOC_Node    *kidNode;
   
   /* Start with suboctant closest to key */
   for (kid0 = 0, i = 0; i < 3; i++)
      kid0 |= (key [i] > org [i] + kidSize) << i;
      
   for (i = 0; i < 8; i++) {
      kid = kid0 ^ i;
      kidNode = node;
      kidDataIdx = dataIdx + OOC_GetKid(oct, &kidNode, kid);
      
      /* Prune empty suboctant */
      if ((!kidNode && !OOC_ISLEAF(node)) ||
          (OOC_ISLEAF(node) && !node -> leaf.num [kid]))
         continue;
         
      /* Set up suboctant */
      VCOPY(kidOrg, org);
      OOC_OCTORIGIN(kidOrg, kid, kidSize);
    
      /* Prune suboctant if not overlapped by maxDist2 */
      if (OOC_BBoxDist2(kidOrg, kidSize, key) > maxDist2)
         continue;
         
      if (kidNode) {
         /* Internal node; recurse into non-empty suboctant */
         maxDist2 = OOC_FindNearest(oct, kidNode, kidDataIdx, kidOrg, 
                                    kidSize, key, filter, queue, maxDist2);
         if (maxDist2 < 0) 
            /* Bail out on error */
            break;
      }                
      else if (OOC_ISLEAF(node))
         /* Leaf node; do linear check of all records in suboctant */
         for (recIdx = kidDataIdx; 
              recIdx < kidDataIdx + node -> leaf.num [kid]; recIdx++) {
            if (OOC_GetData(oct, recIdx, rec))
               return -1;
               
            if (!filter || filter -> func(rec, filter -> data))
               /* Insert record in search queue SQUARED dist to key below
                * maxDist2 and passes filter */
               if ((d2 = dist2(key, oct -> key(rec))) < maxDist2) {
                  if ((d2 = OOC_PutNearest(queue, d2, rec)) >= 0)
                     /* Update maxDist2 if queue is full */
                     maxDist2 = d2;
#ifdef DEBUG_OOC_NN
                     if (OOC_SearchQueueCheck(queue, key, oct -> key, 0)) {
                        fprintf(stderr, "OOC_SearchPush: priority queue "
                                "inconsistency\n");
                        return -1;
                     }
#endif
               }
         }
   }
   
   return maxDist2;
}


float OOC_Find1Nearest (OOC_Octree *oct, OOC_Node *node, OOC_DataIdx dataIdx,
                        const FVECT org, float size, const FVECT key,  
                        const OOC_SearchFilter *filter, void *nnRec, 
                        float maxDist2)
{
   const float kidSize = size * 0.5;
   unsigned    i, kid, kid0;
   float       d2;
   char        rec [oct -> recSize];
   FVECT       kidOrg;
   OOC_DataIdx kidDataIdx, recIdx;
   OOC_Node    *kidNode;
   
   /* Start with suboctant closest to key */
   for (kid0 = 0, i = 0; i < 3; i++)
      kid0 |= (key [i] > org [i] + kidSize) << i;
      
   for (i = 0; i < 8; i++) {
      kid = kid0 ^ i;
      kidNode = node;
      kidDataIdx = dataIdx + OOC_GetKid(oct, &kidNode, kid);
      
      /* Prune empty suboctant */
      if ((!kidNode && !OOC_ISLEAF(node)) ||
          (OOC_ISLEAF(node) && !node -> leaf.num [kid]))
         continue;
         
      /* Set up suboctant */
      VCOPY(kidOrg, org);
      OOC_OCTORIGIN(kidOrg, kid, kidSize);
    
      /* Prune suboctant if not overlapped by maxDist2 */
      if (OOC_BBoxDist2(kidOrg, kidSize, key) > maxDist2)
         continue;
         
      if (kidNode) {
         /* Internal node; recurse into non-empty suboctant */
         maxDist2 = OOC_Find1Nearest(oct, kidNode, kidDataIdx, kidOrg, 
                                     kidSize, key, filter, nnRec, maxDist2);
         if (maxDist2 < 0) 
            /* Bail out on error */
            break;                                     
      }
      else if (OOC_ISLEAF(node))
         /* Leaf node; do linear check of all records in suboctant */
         for (recIdx = kidDataIdx; 
              recIdx < kidDataIdx + node -> leaf.num [kid]; recIdx++) {
            if (OOC_GetData(oct, recIdx, rec))
               return -1;
               
            if (!filter || filter -> func(rec, filter -> data))
               /* Update closest record and max SQUARED dist to key if it
                * passes filter */
               if ((d2 = dist2(key, oct -> key(rec))) < maxDist2) {
                  memcpy(nnRec, rec, oct -> recSize);
                  maxDist2 = d2;
               }
         }
   }
   
   return maxDist2;
}

#endif /* NIX / PMAP_OOC */
Revision:	2.3
Committed:	Tue Mar 23 00:16:38 2021 UTC (4 years, 7 months ago) by rschregle
Content type:	text/plain
Branch:	MAIN
CVS Tags:	rad5R4, rad6R0, HEAD
Changes since 2.2:	+4 -4 lines
Log Message:	fix(rtrace): Fixed ancient THT00PID bug that omitted an octant in out-of-core pmap lookups! UUAARGH!
#	User	Rev	Content
1	rschregle	2.2	#ifndef lint
2	rschregle	2.3	static const char RCSid[] = "$Id: oocnn.c,v 2.2 2017/08/14 21:12:10 rschregle Exp $";
3	rschregle	2.2	#endif
4
5
6	rschregle	2.1	/*
7			=========================================================================
8			k-nearest neighbour lookup routines for out-of-core octree data structure
9
10			Roland Schregle (roland.schregle@{hslu.ch, gmail.com})
11			(c) Lucerne University of Applied Sciences and Arts,
12			supported by the Swiss National Science Foundation (SNSF, #147053)
13			=========================================================================
14
15	rschregle	2.3	$Id: oocnn.c,v 2.2 2017/08/14 21:12:10 rschregle Exp $
16	rschregle	2.1	*/
17
18
19	rschregle	2.2	#if !defined(_WIN32) && !defined(_WIN64) \|\| defined(PMAP_OOC)
20			/* No Windoze support for now */
21	rschregle	2.1
22			#include "oocnn.h"
23			#include "oocsort.h"
24			#include <stdlib.h>
25			#include <string.h>
26			#include <math.h>
27
28
29
30			#ifdef DEBUG_OOC_NN
31			static int OOC_SearchQueueCheck (OOC_SearchQueue *queue, const FVECT key,
32			RREAL (keyFunc)(const void*),
33			unsigned root)
34			/* Priority queue sanity check */
35			{
36			unsigned kid;
37			const OOC_SearchQueueNode *qn = queue -> node;
38			void *rec;
39			float d2;
40
41			if (root < queue -> tail) {
42			rec = OOC_GetNearest(queue, qn [root].idx);
43			d2 = dist2(keyFunc(rec), key);
44
45			if (qn [root].dist2 != d2)
46			return -1;
47
48			if ((kid = (root << 1) + 1) < queue -> tail) {
49			if (qn [kid].dist2 > qn [root].dist2)
50			return -1;
51			else return OOC_SearchQueueCheck(queue, key, keyFunc, kid);
52			}
53
54			if (++kid < queue -> tail) {
55			if (qn [kid].dist2 > qn [root].dist2)
56			return -1;
57			else return OOC_SearchQueueCheck(queue, key, keyFunc, kid);
58			}
59			}
60
61			return 0;
62			}
63			#endif
64
65
66
67			static float OOC_PutNearest (OOC_SearchQueue queue, float d2, void rec)
68			/* Insert data record with SQUARED distance to query point into search
69			* priority queue, maintaining the most distant record at the queue head.
70			* If the queue is full, the new record is only inserted if it is closer
71			* than the queue head.
72			* Returns the new maximum SQUARED distance at the head if the queue is
73			* full. Otherwise returns -1, indicating a maximum for the entire queue is
74			* as yet undefined
75			* The data record is copied into the queue's local record buffa for
76			* post-search retrieval to minimise redundant disk access. Note that it
77			* suffices to only rearrange the corresponding indices in the queue nodes
78			* when restoring the priority queue after every insertion, rather than
79			* moving the actual records. */
80			{
81			OOC_SearchQueueNode *qn = queue -> node;
82			unsigned root, kid, kid2, rootIdx;
83			float d2max = -1; /* Undefined max distance ^2 */
84
85			/* The queue is represented as a linearised binary tree with the root
86			* corresponding to the queue head, and the tail corresponding to the
87			* last leaf */
88			if (queue -> tail < queue -> len) {
89			/* Enlarge queue if not full, insert at tail and resort towards head */
90			kid = queue -> tail++;
91
92			while (kid) {
93			root = (kid - 1) >> 1;
94
95			/* Compare with parent and swap if necessary, else terminate */
96			if (d2 > qn [root].dist2) {
97			qn [kid].dist2 = qn [root].dist2;
98			qn [kid].idx = qn [root].idx;
99			kid = root;
100			}
101			else break;
102			}
103
104			/* Assign tail position as linear index into record buffa
105			* queue -> nnRec and append record */
106			qn [kid].dist2 = d2;
107			qn [kid].idx = queue -> tail - 1;
108			memcpy(OOC_GetNearest(queue, qn [kid].idx), rec, queue -> recSize);
109			}
110			else if (d2 < qn [0].dist2) {
111			/* Queue full and new record closer than maximum at head; replace head
112			* and resort towards tail */
113			root = 0;
114			rootIdx = qn [root].idx;
115
116			while ((kid = (root << 1) + 1) < queue -> tail) {
117			/* Compare with larger kid & swap if necessary, else terminate */
118			if ((kid2 = (kid + 1)) < queue -> tail &&
119			qn [kid2].dist2 > qn [kid].dist2)
120			kid = kid2;
121
122			if (d2 < qn [kid].dist2) {
123			qn [root].dist2 = qn [kid].dist2;
124			qn [root].idx = qn [kid].idx;
125			}
126			else break;
127
128			root = kid;
129			}
130
131			/* Reassign head's previous buffa index and overwrite corresponding
132			* record */
133			qn [root].dist2 = d2;
134			qn [root].idx = rootIdx;
135			memcpy(OOC_GetNearest(queue, qn [root].idx), rec, queue -> recSize);
136
137			/* Update SQUARED maximum distance from head node */
138			d2max = qn [0].dist2;
139			}
140
141			return d2max;
142			}
143
144
145
146			int OOC_InitNearest (OOC_SearchQueue *squeue,
147			unsigned len, unsigned recSize)
148			{
149			squeue -> len = len;
150			squeue -> recSize = recSize;
151			squeue -> node = calloc(len, sizeof(OOC_SearchQueueNode));
152			squeue -> nnRec = calloc(len, recSize);
153			if (!squeue -> node \|\| !squeue -> nnRec) {
154			perror("OOC_InitNearest: failed search queue allocation");
155			return -1;
156			}
157
158			return 0;
159			}
160
161
162
163			void OOC_GetNearest (const OOC_SearchQueue squeue, unsigned idx)
164			{
165			return squeue -> nnRec + idx * squeue -> recSize;
166			}
167
168
169
170			static float OOC_BBoxDist2 (const FVECT bbOrg, RREAL bbSiz, const FVECT key)
171			/* Return minimum SQUARED distance between key and bounding box defined by
172			* bbOrg and bbSiz; a distance of 0 implies the key lies INSIDE the bbox */
173			{
174			/* Explicit comparison with bbox corners */
175			int i;
176			FVECT d;
177
178			for (i = 0; i < 3; i++) {
179			d [i] = key [i] - bbOrg [i];
180			d [i] = d [i] < 0 ? -d [i] : d [i] - bbSiz;
181
182			/* Set to 0 if inside bbox */
183			if (d [i] < 0)
184			d [i] = 0;
185			}
186
187			return DOT(d, d);
188			}
189
190
191
192			float OOC_FindNearest (OOC_Octree oct, OOC_Node node, OOC_DataIdx dataIdx,
193			const FVECT org, float size, const FVECT key,
194			const OOC_SearchFilter *filter,
195			OOC_SearchQueue *queue, float maxDist2)
196			{
197			const float kidSize = size * 0.5;
198			unsigned i, kid, kid0;
199			float d2;
200			char rec [oct -> recSize];
201			FVECT kidOrg;
202			OOC_DataIdx kidDataIdx, recIdx;
203			OOC_Node *kidNode;
204
205			/* Start with suboctant closest to key */
206			for (kid0 = 0, i = 0; i < 3; i++)
207			kid0 \|= (key [i] > org [i] + kidSize) << i;
208
209	rschregle	2.3	for (i = 0; i < 8; i++) {
210	rschregle	2.1	kid = kid0 ^ i;
211			kidNode = node;
212			kidDataIdx = dataIdx + OOC_GetKid(oct, &kidNode, kid);
213
214			/* Prune empty suboctant */
215			if ((!kidNode && !OOC_ISLEAF(node)) \|\|
216			(OOC_ISLEAF(node) && !node -> leaf.num [kid]))
217			continue;
218
219			/* Set up suboctant */
220			VCOPY(kidOrg, org);
221			OOC_OCTORIGIN(kidOrg, kid, kidSize);
222
223			/* Prune suboctant if not overlapped by maxDist2 */
224			if (OOC_BBoxDist2(kidOrg, kidSize, key) > maxDist2)
225			continue;
226
227			if (kidNode) {
228			/* Internal node; recurse into non-empty suboctant */
229			maxDist2 = OOC_FindNearest(oct, kidNode, kidDataIdx, kidOrg,
230			kidSize, key, filter, queue, maxDist2);
231			if (maxDist2 < 0)
232			/* Bail out on error */
233			break;
234			}
235			else if (OOC_ISLEAF(node))
236			/* Leaf node; do linear check of all records in suboctant */
237			for (recIdx = kidDataIdx;
238			recIdx < kidDataIdx + node -> leaf.num [kid]; recIdx++) {
239			if (OOC_GetData(oct, recIdx, rec))
240			return -1;
241
242			if (!filter \|\| filter -> func(rec, filter -> data))
243			/* Insert record in search queue SQUARED dist to key below
244			* maxDist2 and passes filter */
245			if ((d2 = dist2(key, oct -> key(rec))) < maxDist2) {
246			if ((d2 = OOC_PutNearest(queue, d2, rec)) >= 0)
247			/* Update maxDist2 if queue is full */
248			maxDist2 = d2;
249			#ifdef DEBUG_OOC_NN
250			if (OOC_SearchQueueCheck(queue, key, oct -> key, 0)) {
251			fprintf(stderr, "OOC_SearchPush: priority queue "
252			"inconsistency\n");
253			return -1;
254			}
255			#endif
256			}
257			}
258			}
259
260			return maxDist2;
261			}
262
263
264
265			float OOC_Find1Nearest (OOC_Octree oct, OOC_Node node, OOC_DataIdx dataIdx,
266			const FVECT org, float size, const FVECT key,
267			const OOC_SearchFilter filter, void nnRec,
268			float maxDist2)
269			{
270			const float kidSize = size * 0.5;
271			unsigned i, kid, kid0;
272			float d2;
273			char rec [oct -> recSize];
274			FVECT kidOrg;
275			OOC_DataIdx kidDataIdx, recIdx;
276			OOC_Node *kidNode;
277
278			/* Start with suboctant closest to key */
279			for (kid0 = 0, i = 0; i < 3; i++)
280			kid0 \|= (key [i] > org [i] + kidSize) << i;
281
282	rschregle	2.3	for (i = 0; i < 8; i++) {
283	rschregle	2.1	kid = kid0 ^ i;
284			kidNode = node;
285			kidDataIdx = dataIdx + OOC_GetKid(oct, &kidNode, kid);
286
287			/* Prune empty suboctant */
288			if ((!kidNode && !OOC_ISLEAF(node)) \|\|
289			(OOC_ISLEAF(node) && !node -> leaf.num [kid]))
290			continue;
291
292			/* Set up suboctant */
293			VCOPY(kidOrg, org);
294			OOC_OCTORIGIN(kidOrg, kid, kidSize);
295
296			/* Prune suboctant if not overlapped by maxDist2 */
297			if (OOC_BBoxDist2(kidOrg, kidSize, key) > maxDist2)
298			continue;
299
300			if (kidNode) {
301			/* Internal node; recurse into non-empty suboctant */
302			maxDist2 = OOC_Find1Nearest(oct, kidNode, kidDataIdx, kidOrg,
303			kidSize, key, filter, nnRec, maxDist2);
304			if (maxDist2 < 0)
305			/* Bail out on error */
306			break;
307			}
308			else if (OOC_ISLEAF(node))
309			/* Leaf node; do linear check of all records in suboctant */
310			for (recIdx = kidDataIdx;
311			recIdx < kidDataIdx + node -> leaf.num [kid]; recIdx++) {
312			if (OOC_GetData(oct, recIdx, rec))
313			return -1;
314
315			if (!filter \|\| filter -> func(rec, filter -> data))
316			/* Update closest record and max SQUARED dist to key if it
317			* passes filter */
318			if ((d2 = dist2(key, oct -> key(rec))) < maxDist2) {
319			memcpy(nnRec, rec, oct -> recSize);
320			maxDist2 = d2;
321			}
322			}
323			}
324
325			return maxDist2;
326			}
327
328	rschregle	2.2	#endif /* NIX / PMAP_OOC */