diff --git a/oocnn.c b/oocnn.c index 77c17f2..2f88045 100644 --- a/oocnn.c +++ b/oocnn.c @@ -1,492 +1,505 @@ #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, OOC_DataIdx recIdx, +static float OOC_PutNearest (OOC_SearchQueue *squeue, float d2, void *rec, +#ifdef OOC_NN_RECIDX + OOC_DataIdx recIdx, +#endif 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, rootRecIdx; /* Start with undefined max distance */ float d2max = -1; #ifdef PMAP_PATHFILT /* 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 OOC_NN_RECIDX sqn [kid].recIdx = sqn [root].recIdx; - +#endif #ifdef PMAP_PATHFILT 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; +#ifdef OOC_NN_RECIDX sqn [kid].recIdx = recIdx; +#endif memcpy(OOC_GetNearest(squeue, sqn [kid].idx), rec, squeue -> recSize); #ifdef PMAP_PATHFILT 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 OOC_NN_RECIDX rootRecIdx = sqn [root].recIdx; - +#endif #ifdef PMAP_PATHFILT 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 OOC_NN_RECIDX sqn [root].recIdx = sqn [kid].recIdx; - +#endif #ifdef PMAP_PATHFILT 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; +#ifdef OOC_NN_RECIDX sqn [root].recIdx = rootRecIdx; +#endif memcpy(OOC_GetNearest(squeue, sqn[root].idx), rec, squeue->recSize); #ifdef PMAP_PATHFILT 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_PATHFILT) && 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; if (!node) /* No current node -- shouldn't happen */ return -1; /* 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, 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) { +#ifdef OOC_NN_RECIDX if ((d2 = OOC_PutNearest(squeue, d2, rec, recIdx, attrib) +#else + if ((d2 = OOC_PutNearest(squeue, d2, rec, attrib) +#endif ) >= 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 } } } } } /* XXX: Note this will not correspond to the maximum distance in a partially filled search queue, since it hasn't been finalised; this distance should therefore be obtained from the search queue head by the caller when this func returns! */ 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, OOC_DataIdx *nnIdx, 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; if (!node) /* No current node -- shouldn't happen */ return -1; /* 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, nnIdx, 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)) /* If record passes filter, update closest record, its index, and max SQUARED distance to its key */ if ((d2 = dist2(key, oct -> key(rec))) < maxDist2) { memcpy(nnRec, rec, oct -> recSize); maxDist2 = d2; if (nnIdx) *nnIdx = recIdx; } } } return maxDist2; } #endif /* NIX / PMAP_OOC */ diff --git a/oocnn.h b/oocnn.h index 37bb3af..03ca714 100644 --- a/oocnn.h +++ b/oocnn.h @@ -1,126 +1,128 @@ /* ========================================================================= 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.h,v 1.2 2020/11/10 01:15:09 u-no-hoo Exp u-no-hoo $ */ #ifndef OOC_SEARCH_H #define OOC_SEARCH_H #include "oococt.h" /* Priority queue node for octree lookups */ typedef struct { float dist2; /* Record's distance^2 to query point */ unsigned idx; /* Record's index in queue buffer */ + #ifdef OOC_NN_RECIDX OOC_DataIdx recIdx; /* Record's absolute index */ + #endif } OOC_SearchQueueNode; /* Priority queue for octree lookups. Note that we explicitly store the * NN candidates in a local in-core buffer nnRec for later retrieval * without incurring additional disk I/O */ typedef struct { OOC_SearchQueueNode *node; unsigned len, tail, recSize; void *nnRec; } OOC_SearchQueue; /* Requires above typedefs */ #include "pmapfilt.h" 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 *queue, float maxDist2 ); /* Find k nearest neighbours (where k = queue -> len) within a maximum * SQUARED distance of maxDist2 around key. Returns the corresponding * data records with their SQUARED distances in the search queue * (queue -> node [0] .. queue -> node [queue -> tail - 1]), with the * furthest neighbour at the queue head and queue->tail <= queue->len. * * This is a recursive function requiring params for the current node: * DataIdx is the data offset for records in the current node, which is * necessary for implied addressing into the leaf file. Org and size are * the origin and size of the current node's bounding box. * * This function accepts two optional callbacks for selective search: * /SHALL MODE ON * * -- If filter != NULL, filter->filtFunc() is called for each candidate * nearest neighbour along with the encapsulated state data provided * by the caller; records SHALL be accepted if filter->filtFunc() * returns nonzero. * * -- If attrib != NULL, attrib->findFunc() is called for each candidate * nearest neighbour that has been accepted by filter->filtFunc() and * will therefore be added to the search queue. * This function SHALL map a record attribute to its search queue node * via a lookup table ("attribute map") accessible via attrib->state, * and SHALL return a modifiable pointer to the corresponding search * queue node, or NULL if the attribute is absent. * This callback SHALL support replacing a previously enqueued record * with the candidate if they share the same attribute and the latter * is closer to the key; this avoids accumulating records with * duplicate attributes in the queue, which may be desirable in some * applications. Or not. Maybe not. * This callback SHALL also support updating the attribute map entry * for each resorted record when restoring the priority queue property * during insertions; the search queue nodes referenced in the * attribute map are directly modified by dereferencing the returned * pointers. * In addition, the callbacks attrib->addFunc() and attrib->delFunc() * SHALL insert new entries into the attribute map, or delete evicted * ones once the search queue is full. * * /SHALL MODE OFF * We use the word "SHALL" a lot here 'cos Lars likes it and it sounds * very imposing and professional and stuff. * * This function returns -1 on failure, else a positive value on success. * The maximum (squared) distance to the found neighours can be obtained * from the head of the search queue, queue -> node [0].dist2. * (Note this will only coincide with the return value if the queue is * full, so this value should not be relied on!). */ 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, OOC_DataIdx *nnIdx, float maxDist2 ); /* Find single nearest neighbour within max SQUARED distance maxDist2 * around key and copy corresponding record in nnRec, and return its * index in nnIdx (if nnIdx != NULL). This is an optimised version of * OOC_FindNearest() without a search queue */ int OOC_InitNearest ( OOC_SearchQueue *squeue, unsigned len, unsigned recSize ); /* Initialise NN search queue of length len and local buffa for records * of size recSize; precondition to calling OOC_FindNearest() */ void *OOC_GetNearest (const OOC_SearchQueue *queue, unsigned idx); /* Return pointer to record at pos idx in search queue after calling * OOC_FindNearest() */ #endif