diff --git a/oocnn.c b/oocnn.c index 31c8e25..c2310ff 100644 --- a/oocnn.c +++ b/oocnn.c @@ -1,476 +1,476 @@ #ifndef lint static const char RCSid[] = "$Id: oocnn.c,v 1.2 2020/11/10 01:09:47 u-no-hoo Exp u-no-hoo $"; #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, "Daylight Redirecting Components") (c) Tokyo University of Science, supported by the JSPS Grants-in-Aid for Scientific Research (KAKENHI JP19KK0115, "Three-Dimensional Light Flow") ========================================================================= $Id: oocnn.c,v 1.2 2020/11/10 01:09:47 u-no-hoo Exp u-no-hoo $ */ #if !defined(_WIN32) && !defined(_WIN64) && defined(PMAP_OOC) /* No Windoze support for now */ #include "oocnn.h" #include "oocsort.h" #include #include #include /* ========================= SEARCH QUEUE STUFF ========================= */ #ifdef DEBUG_OOC_NN static int OOC_CheckSearchQueue ( OOC_SearchQueue *squeue, const FVECT key, RREAL *(*keyFunc)(const void*), unsigned root ) /* Priority queue sanity check */ { unsigned kid; const OOC_SearchQueueNode *sqn = squeue -> node; void *rec; float d2; if (root < squeue -> tail) { rec = OOC_GetNearest(squeue, sqn [root].idx); d2 = dist2(keyFunc(rec), key); if (sqn [root].dist2 != d2) return -1; if ((kid = (root << 1) + 1) < squeue -> tail) { if (sqn [kid].dist2 > sqn [root].dist2) return -1; else return OOC_CheckSearchQueue(squeue, key, keyFunc, kid); } if (++kid < squeue -> tail) { if (sqn [kid].dist2 > sqn [root].dist2) return -1; else return OOC_CheckSearchQueue(squeue, key, keyFunc, kid); } } return 0; } #endif static float OOC_PutNearest ( OOC_SearchQueue *squeue, float d2, void *rec, const OOC_SearchAttribCallback *attrib ) /* 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. * * The search priority queue is represented as a linearised binary tree with * the root corresponding to the queue head, and the tail corresponding to * the last leaf. * * The optional attrib callback maps record attributes to their queue nodes * by calling attrib->findFunc() if attrib != NULL. It enables finding a * previously queued data record and replacing it by rec if the latter's * SQUARED distance d2 is lower. * * The 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. * * This function returns the new maximum SQUARED distance at the head if the * search queue is full. Otherwise it returns -1, indicating a maximum for * the entire queue is as yet undefined. */ { void *tRec = NULL; OOC_SearchQueueNode *sqn = squeue -> node, **attribSqn = NULL, **tAttribSqn = NULL; unsigned root, kid, kid2, rootIdx; /* Start with undefined max distance */ float d2max = -1; #ifdef PMAP_PHOTONFLOW /* Find previously enqueued record with same attribute if callback defined */ #ifdef PMAP_DEBUGPATHS printf("-------- Enqueue --------\n"); #endif attribSqn = (OOC_SearchQueueNode**)( attrib ? attrib -> findFunc(rec, attrib -> state) : NULL ); #ifdef PMAP_DEBUGPATHS printf(attribSqn ? "found\n" : "not found\n"); #endif #endif if (!attribSqn && squeue -> tail < squeue -> len) { /* No duplicate attribute in queue, and queue not full; insert record at tail and resort towards head */ kid = squeue -> tail++; while (kid) { root = (kid - 1) >> 1; /* Compare with parent and swap if necessary (bubble up), else terminate */ if (d2 > sqn [root].dist2) { sqn [kid].dist2 = sqn [root].dist2; sqn [kid].idx = sqn [root].idx; #ifdef PMAP_PHOTONFLOW if (attrib) { /* Update root's attribute map entry to refer to kid */ tRec = OOC_GetNearest(squeue, sqn [root].idx); tAttribSqn = (OOC_SearchQueueNode**)( attrib -> findFunc(tRec, attrib -> state) ); #ifdef PMAP_DEBUGPATHS printf("update\n"); #endif if (!tAttribSqn) { /* Consistency error: a previously enqueued record must * have an entry in the attribute map! */ fputs("OOC_PutNearest: corrupt attribute map\n", stderr); exit(-1); } *tAttribSqn = &sqn [kid]; } #endif kid = root; } else break; } /* Priority queue restored; assign tail position as linear index into * record buffa squeue -> nnRec and append record */ sqn [kid].dist2 = d2; sqn [kid].idx = squeue -> tail - 1; memcpy(OOC_GetNearest(squeue, sqn [kid].idx), rec, squeue -> recSize); #ifdef PMAP_PHOTONFLOW if (attrib) /* Add new record to attribute map */ attrib -> addFunc(rec, &sqn [kid], attrib -> state); #endif } else { /* Search queue full or duplicate attribute in queue and new node may be closer; set root to replaced node, otherwise to queue head */ root = attribSqn ? *attribSqn - sqn : 0; if (d2 < sqn [root].dist2) { /* New record closer than maximum at root; replace and resort towards leaves (=search queue tail) */ rootIdx = sqn [root].idx; #ifdef PMAP_PHOTONFLOW if (attrib && !attribSqn) { /* New record will replace root and has unique attribute; delete * root from the attribute map */ tRec = OOC_GetNearest(squeue, sqn [root].idx); attrib -> delFunc(tRec, attrib -> state); } #endif while ((kid = (root << 1) + 1) < squeue -> tail) { /* Compare with larger kid & swap if necessary (bubble down), else terminate */ if ( (kid2 = (kid + 1)) < squeue -> tail && sqn [kid2].dist2 > sqn [kid].dist2 ) kid = kid2; if (d2 < sqn [kid].dist2) { sqn [root].dist2 = sqn [kid].dist2; sqn [root].idx = sqn [kid].idx; #ifdef PMAP_PHOTONFLOW if (attrib) { /* Update kid's attribute map entry to refer to root */ tRec = OOC_GetNearest(squeue, sqn [kid].idx); tAttribSqn = (OOC_SearchQueueNode**)( attrib -> findFunc(tRec, attrib -> state) ); #ifdef PMAP_DEBUGPATHS printf("update\n"); #endif if (!tAttribSqn) { /* Consistency error: a previously enqueued record must * have an entry in the attribute map! */ fputs("OOC_PutNearest: corrupt attribute map\n", stderr); exit(-1); } *tAttribSqn = &sqn [root]; } #endif } else break; root = kid; } /* Reassign head's / replaced node's previous buffa index and overwrite corresponding record */ sqn [root].dist2 = d2; sqn [root].idx = rootIdx; memcpy(OOC_GetNearest(squeue, sqn[root].idx), rec, squeue->recSize); #ifdef PMAP_PHOTONFLOW if (attrib) { if (!attribSqn) /* Add new record to attribute map */ attrib -> addFunc(rec, &sqn [root], attrib -> state); else { #ifdef PMAP_DEBUGPATHS attrib -> findFunc(rec, attrib -> state); printf("update\n"); #endif /* Update new record's existing attribute map entry */ *attribSqn = &sqn [root]; } } #endif } } if (squeue -> tail >= squeue -> len) /* Search queue full; update max distance^2 from head node */ d2max = sqn [0].dist2; #if defined(PMAP_PHOTONFLOW) && defined(PMAP_DEBUGPATHS) if (attrib) /* Run sanity check on attribute map */ attrib -> checkFunc(attrib -> state); #endif 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; } /* ============================ OCTREE STUFF ============================ */ 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_SearchFilterCallback *filter, const OOC_SearchAttribCallback *attrib, OOC_SearchQueue *squeue, 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++) { + + 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, attrib, squeue, 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 -> filtFunc(rec, filter -> state)) { /* Insert record in search queue if it passes filter * and its SQUARED dist to key is below maxDist2 */ if ((d2 = dist2(key, oct -> key(rec))) < maxDist2) { if ((d2 = OOC_PutNearest(squeue, d2, rec, attrib)) >= 0) /* Update maxDist2 if search queue is full */ maxDist2 = d2; #ifdef DEBUG_OOC_NN if (OOC_CheckSearchQueue(squeue, key, oct->key, 0)) { fprintf( stderr, "OOC_PutNearest: corrupt search queue\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_SearchFilterCallback *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++) { + 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 -> filtFunc(rec, filter -> state)) /* 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 */