src/rt/oocnn.c

/* 
   =========================================================================
   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 1.30 2015/11/13 18:07:26 taschreg Exp taschreg $
*/


#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 < 7; 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 < 7; 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;
}

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