1 |
greg |
2.1 |
/* |
2 |
|
|
================================================================== |
3 |
|
|
Out-of-core octree data structure |
4 |
|
|
|
5 |
|
|
Roland Schregle (roland.schregle@{hslu.ch, gmail.com}) |
6 |
|
|
(c) Fraunhofer Institute for Solar Energy Systems, |
7 |
|
|
Lucerne University of Applied Sciences & Arts |
8 |
|
|
================================================================== |
9 |
|
|
|
10 |
|
|
$Id$ |
11 |
|
|
*/ |
12 |
|
|
|
13 |
|
|
|
14 |
|
|
#include <stdlib.h> |
15 |
|
|
#include "oococt.h" |
16 |
|
|
#include "oocsort.h" |
17 |
|
|
|
18 |
|
|
|
19 |
|
|
static OOC_Sort_Key zInterleave (OOC_Sort_Key k) |
20 |
|
|
/* Interleave lower 21 bits of k with 00, resulting in 63 bits. |
21 |
|
|
This code taken from |
22 |
|
|
http://www.forceflow.be/2013/10/07/ |
23 |
|
|
morton-encodingdecoding-through-bit-interleaving-implementations/ */ |
24 |
|
|
{ |
25 |
|
|
/* Mask out lower 21 bits, the rest is cryptic */ |
26 |
|
|
OOC_Sort_Key i = k & 0x1fffff; |
27 |
|
|
|
28 |
|
|
i = (i | i << 32) & 0x001f00000000ffff; |
29 |
|
|
i = (i | i << 16) & 0x001f0000ff0000ff; |
30 |
|
|
i = (i | i << 8) & 0x100f00f00f00f00f; |
31 |
|
|
i = (i | i << 4) & 0x10c30c30c30c30c3; |
32 |
|
|
i = (i | i << 2) & 0x1249249249249249; |
33 |
|
|
|
34 |
|
|
return i; |
35 |
|
|
} |
36 |
|
|
|
37 |
|
|
|
38 |
|
|
static OOC_Sort_Key Key2zOrder (OOC_Octree *oct, FVECT key) |
39 |
|
|
/* Compute 63-bit Morton (Z-order) index for 3D key */ |
40 |
|
|
{ |
41 |
|
|
int i; |
42 |
|
|
OOC_Sort_Key k [3]; |
43 |
|
|
|
44 |
|
|
/* Normalise key and map to 21-bit int */ |
45 |
|
|
for (i = 0; i < 3; i++) |
46 |
|
|
k [i] = (((OOC_Sort_Key)1 << 21) - 1) / oct -> size * |
47 |
|
|
(key [i] - oct -> org [i]); |
48 |
|
|
|
49 |
|
|
/* Interleave each dim with zeros and merge */ |
50 |
|
|
return zInterleave(k [0]) | zInterleave(k [1]) << 1 | |
51 |
|
|
zInterleave(k [2]) << 2; |
52 |
|
|
} |
53 |
|
|
|
54 |
|
|
|
55 |
|
|
static OOC_Node *NewNode (OOC_Octree *oct) |
56 |
|
|
/* Allocate new octree node, enlarge array if necessary. |
57 |
|
|
Return pointer to new node or NULL if failed. */ |
58 |
|
|
{ |
59 |
|
|
OOC_Node *n = NULL; |
60 |
|
|
|
61 |
|
|
if (++oct -> numNodes > oct -> maxNodes) { |
62 |
|
|
/* Need to allocate root or enlarge array */ |
63 |
|
|
oct -> maxNodes += OOC_BLKSIZE / sizeof(OOC_Node); |
64 |
|
|
realloc(oct -> root, oct -> maxNodes * sizeof(OOC_Node)); |
65 |
|
|
if (!oct -> root) |
66 |
|
|
return NULL; |
67 |
|
|
} |
68 |
|
|
|
69 |
|
|
n = oct -> root + oct -> numNodes - 1; |
70 |
|
|
n -> num = n -> kid = 0; |
71 |
|
|
} |
72 |
|
|
|
73 |
|
|
|
74 |
|
|
int OOC_Init (OOC_Octree *oct, unsigned recSize, unsigned leafMax, |
75 |
|
|
unsigned maxDepth) |
76 |
|
|
{ |
77 |
|
|
ooc -> org [0] = oct -> org [1] = oct -> org [2] = FHUGE; |
78 |
|
|
ooc -> size = oct -> maxNodes = oct -> lastNode = 0; |
79 |
|
|
ooc -> recSize = recSize; |
80 |
|
|
ooc -> leafMax = leafMax; |
81 |
|
|
ooc -> maxDepth = maxDepth; |
82 |
|
|
ooc -> root = NULL; |
83 |
|
|
|
84 |
|
|
return NewNode(oct) ? 0 : -1; |
85 |
|
|
} |
86 |
|
|
|
87 |
|
|
|
88 |
|
|
int OOC_Branch (FVECT key, FVECT org, RREAL *size); |
89 |
|
|
/* Return index of kid containing key, update origin and size accordingly */ |
90 |
|
|
{ |
91 |
|
|
size *= 0.5; |
92 |
|
|
int kidIdx = 0; |
93 |
|
|
|
94 |
|
|
for (int i = 0; i < 3; i++) { |
95 |
|
|
RREAL cent = org [i] + size; |
96 |
|
|
if (key [i] > cent) { |
97 |
|
|
kidIdx |= 1 << i; |
98 |
|
|
org [i] = cent; |
99 |
|
|
} |
100 |
|
|
} |
101 |
|
|
|
102 |
|
|
return kidIdx; |
103 |
|
|
} |
104 |
|
|
|
105 |
|
|
|
106 |
|
|
OOC_Idx OOC_Find (OOC_Octree *oct, FVECT key) |
107 |
|
|
/* Iterative search for given key */ |
108 |
|
|
{ |
109 |
|
|
FVECT org; |
110 |
|
|
RREAL size; |
111 |
|
|
OOC_Idx dataIdx = 0; |
112 |
|
|
OOC_Node *node = NULL; |
113 |
|
|
|
114 |
|
|
if (!oct || !oct -> root) |
115 |
|
|
return -1; |
116 |
|
|
|
117 |
|
|
node = oct -> root; |
118 |
|
|
VCOPY(org, oct -> org); |
119 |
|
|
size = oct -> size; |
120 |
|
|
|
121 |
|
|
/* Quit at terminal/empty node or internal leaf */ |
122 |
|
|
while (OOC_INTNODE(*node)) { |
123 |
|
|
/* Determine kid to branch into and update data index with skipped |
124 |
|
|
* kids' data counts */ |
125 |
|
|
int kidIdx = OOC_Branch(key, org, size); |
126 |
|
|
node = oct -> root + OOC_KID(*node); |
127 |
|
|
while (kidIdx--) |
128 |
|
|
dataIdx += OOC_NUMDATA(node++); |
129 |
|
|
} |
130 |
|
|
|
131 |
|
|
/* Reached empty node -> not found */ |
132 |
|
|
if (OOC_EMPTYNODE(*node)) |
133 |
|
|
return -1; |
134 |
|
|
|
135 |
|
|
/* Reached terminal node -> find leaf to branch into and update data |
136 |
|
|
* index with skipped leaves' data counts */ |
137 |
|
|
if (OOC_TERMNODE(*node)) { |
138 |
|
|
OOC_Leaf *leaf = ooc -> leaves [OOC_KID(*node)]; |
139 |
|
|
int leafIdx = OOC_Branch(key, org, size); |
140 |
|
|
while (leafIdx--) |
141 |
|
|
dataIdx += *leaf++; |
142 |
|
|
} |
143 |
|
|
|
144 |
|
|
/* (Reached internal leaf -> already have data index) */ |
145 |
|
|
return dataIdx; |
146 |
|
|
} |
147 |
|
|
|
148 |
|
|
|
149 |
|
|
#if 0 |
150 |
|
|
OOC_Node *FindNode (OOC_Octree *oct, FVECT key, OOC_Idx *dataIdx, int flags) |
151 |
|
|
/* ITERATIVE node search for given key */ |
152 |
|
|
{ |
153 |
|
|
FVECT org; |
154 |
|
|
double size; |
155 |
|
|
OOC_Idx dataIdx = 0; |
156 |
|
|
OOC_Node *node = NULL; |
157 |
|
|
|
158 |
|
|
if (!oct || !oct -> root) |
159 |
|
|
return NULL; |
160 |
|
|
|
161 |
|
|
node = oct -> root; |
162 |
|
|
VCOPY(org, oct -> org); |
163 |
|
|
size = oct -> size; |
164 |
|
|
|
165 |
|
|
/* Quit at terminal or empty node */ |
166 |
|
|
while (!OOC_TERM(*node) && !OOC_EMPTY(*node)) { |
167 |
|
|
size *= 0.5; |
168 |
|
|
int kidIdx = 0; |
169 |
|
|
|
170 |
|
|
/* Determine kid to branch into and update origin accordingly */ |
171 |
|
|
for (int i = 0; i < 3; i++) { |
172 |
|
|
RREAL cent = org [i] + size; |
173 |
|
|
if (key [i] > cent) { |
174 |
|
|
kidIdx |= 1 << i; |
175 |
|
|
org [i] = cent; |
176 |
|
|
} |
177 |
|
|
} |
178 |
|
|
|
179 |
|
|
/* Increment node's data counter if inserting */ |
180 |
|
|
if (flags & OOC_INSERT) |
181 |
|
|
node -> numData++; |
182 |
|
|
|
183 |
|
|
/* Update data index with skipped kids' data counts */ |
184 |
|
|
node = oct -> root + node -> kid; |
185 |
|
|
while (kidIdx--) |
186 |
|
|
dataIdx += node++ -> numData; |
187 |
|
|
} |
188 |
|
|
|
189 |
|
|
return node; |
190 |
|
|
} |
191 |
|
|
|
192 |
|
|
|
193 |
|
|
OOC_Idx FindLeaf (OOC_Leaf *leaf, FVECT key, OOC_Idx *dataIdx); |
194 |
|
|
/* Return index into data block for leaf containing key */ |
195 |
|
|
{ |
196 |
|
|
int kidIdx = 0; |
197 |
|
|
OOC_Idx = |
198 |
|
|
|
199 |
|
|
for (int i = 0; i < 3; i++) { |
200 |
|
|
if (key [i] |
201 |
|
|
#endif |