1 //# Filename: SpatialIndex.cpp
2 //#
3 //# The SpatialIndex class is defined here.
4 //#
5 //# Author: Peter Z. Kunszt based on A. Szalay's code
6 //#
7 //# Date: October 15, 1998
8 //#
9 //# SPDX-FileCopyrightText: 2000 Peter Z. Kunszt Alex S. Szalay, Aniruddha R. Thakar
10 //# The Johns Hopkins University
11 //#
12 //# Modification History:
13 //#
14 //# Oct 18, 2001 : Dennis C. Dinge -- Replaced ValVec with std::vector
15 //# Jul 25, 2002 : Gyorgy Fekete -- Added pointById()
16 //#
17
18 #include "SpatialIndex.h"
19 #include "SpatialException.h"
20
21 #ifdef _WIN32
22 #include <malloc.h>
23 #else
24 #ifdef __APPLE__
25 #include <sys/malloc.h>
26 #else
27 #include <cstdlib>
28 #endif
29 #endif
30
31 #include <cstdio>
32 #include <cmath>
33 // ===========================================================================
34 //
35 // Macro definitions for readability
36 //
37 // ===========================================================================
38
39 #define N(x) nodes_[(x)]
40 #define V(x) vertices_[nodes_[index].v_[(x)]]
41 #define IV(x) nodes_[index].v_[(x)]
42 #define W(x) vertices_[nodes_[index].w_[(x)]]
43 #define IW(x) nodes_[index].w_[(x)]
44 #define ICHILD(x) nodes_[index].childID_[(x)]
45
46 #define IV_(x) nodes_[index_].v_[(x)]
47 #define IW_(x) nodes_[index_].w_[(x)]
48 #define ICHILD_(x) nodes_[index_].childID_[(x)]
49 #define IOFFSET 9
50
51 // ===========================================================================
52 //
53 // Member functions for class SpatialIndex
54 //
55 // ===========================================================================
56
57 /////////////CONSTRUCTOR//////////////////////////////////
58 //
SpatialIndex(size_t maxlevel,size_t buildlevel)59 SpatialIndex::SpatialIndex(size_t maxlevel, size_t buildlevel)
60 : maxlevel_(maxlevel), buildlevel_((buildlevel == 0 || buildlevel > maxlevel) ? maxlevel : buildlevel)
61 {
62 size_t nodes, vertices;
63
64 vMax(&nodes, &vertices);
65 layers_.resize(buildlevel_ + 1); // allocate enough space already
66 nodes_.resize(nodes + 1); // allocate space for all nodes
67 vertices_.resize(vertices + 1); // allocate space for all vertices
68
69 N(0).index_ = 0; // initialize invalid node
70
71 // initialize first layer
72 layers_[0].level_ = 0;
73 layers_[0].nVert_ = 6;
74 layers_[0].nNode_ = 8;
75 layers_[0].nEdge_ = 12;
76 layers_[0].firstIndex_ = 1;
77 layers_[0].firstVertex_ = 0;
78
79 // set the first 6 vertices
80 float64 v[6][3] = {
81 { 0.0L, 0.0L, 1.0L }, // 0
82 { 1.0L, 0.0L, 0.0L }, // 1
83 { 0.0L, 1.0L, 0.0L }, // 2
84 { -1.0L, 0.0L, 0.0L }, // 3
85 { 0.0L, -1.0L, 0.0L }, // 4
86 { 0.0L, 0.0L, -1.0L } // 5
87 };
88
89 for (int i = 0; i < 6; i++)
90 vertices_[i].set(v[i][0], v[i][1], v[i][2]);
91
92 // create the first 8 nodes - index 1 through 8
93 index_ = 1;
94 newNode(1, 5, 2, 8, 0); // S0
95 newNode(2, 5, 3, 9, 0); // S1
96 newNode(3, 5, 4, 10, 0); // S2
97 newNode(4, 5, 1, 11, 0); // S3
98 newNode(1, 0, 4, 12, 0); // N0
99 newNode(4, 0, 3, 13, 0); // N1
100 newNode(3, 0, 2, 14, 0); // N2
101 newNode(2, 0, 1, 15, 0); // N3
102
103 // loop through buildlevel steps, and build the nodes for each layer
104
105 size_t pl = 0;
106 size_t level = buildlevel_;
107 while (level-- > 0)
108 {
109 SpatialEdge edge(*this, pl);
110 edge.makeMidPoints();
111 makeNewLayer(pl);
112 ++pl;
113 }
114
115 sortIndex();
116 }
117
118 /////////////NODEVERTEX///////////////////////////////////
119 // nodeVertex: return the vectors of the vertices, based on the ID
120 //
nodeVertex(const uint64 id,SpatialVector & v0,SpatialVector & v1,SpatialVector & v2) const121 void SpatialIndex::nodeVertex(const uint64 id, SpatialVector &v0, SpatialVector &v1, SpatialVector &v2) const
122 {
123 if (buildlevel_ == maxlevel_)
124 {
125 uint32 idx = (uint32)id - leaves_ + IOFFSET; // -jbb: Fix segfault. See "idx =" below.
126 v0 = vertices_[nodes_[idx].v_[0]];
127 v1 = vertices_[nodes_[idx].v_[1]];
128 v2 = vertices_[nodes_[idx].v_[2]];
129 return;
130 }
131
132 // buildlevel < maxlevel
133 // get the id of the stored leaf that we are in
134 // and get the vertices of the node we want
135 uint64 sid = id >> ((maxlevel_ - buildlevel_) * 2);
136 uint32 idx = (uint32)(sid - storedleaves_ + IOFFSET);
137
138 v0 = vertices_[nodes_[idx].v_[0]];
139 v1 = vertices_[nodes_[idx].v_[1]];
140 v2 = vertices_[nodes_[idx].v_[2]];
141
142 // loop through additional levels,
143 // pick the correct triangle accordingly, storing the
144 // vertices in v1,v2,v3
145 for (uint32 i = buildlevel_ + 1; i <= maxlevel_; i++)
146 {
147 uint64 j = (id >> ((maxlevel_ - i) * 2)) & 3;
148 SpatialVector w0 = v1 + v2;
149 w0.normalize();
150 SpatialVector w1 = v0 + v2;
151 w1.normalize();
152 SpatialVector w2 = v1 + v0;
153 w2.normalize();
154
155 switch (j)
156 {
157 case 0:
158 v1 = w2;
159 v2 = w1;
160 break;
161 case 1:
162 v0 = v1;
163 v1 = w0;
164 v2 = w2;
165 break;
166 case 2:
167 v0 = v2;
168 v1 = w1;
169 v2 = w0;
170 break;
171 case 3:
172 v0 = w0;
173 v1 = w1;
174 v2 = w2;
175 break;
176 }
177 }
178 }
179
180 /////////////MAKENEWLAYER/////////////////////////////////
181 // makeNewLayer: generate a new layer and the nodes in it
182 //
makeNewLayer(size_t oldlayer)183 void SpatialIndex::makeNewLayer(size_t oldlayer)
184 {
185 uint64 index, id;
186 size_t newlayer = oldlayer + 1;
187 layers_[newlayer].level_ = layers_[oldlayer].level_ + 1;
188 layers_[newlayer].nVert_ = layers_[oldlayer].nVert_ + layers_[oldlayer].nEdge_;
189 layers_[newlayer].nNode_ = 4 * layers_[oldlayer].nNode_;
190 layers_[newlayer].nEdge_ = layers_[newlayer].nNode_ + layers_[newlayer].nVert_ - 2;
191 layers_[newlayer].firstIndex_ = index_;
192 layers_[newlayer].firstVertex_ = layers_[oldlayer].firstVertex_ + layers_[oldlayer].nVert_;
193
194 uint64 ioffset = layers_[oldlayer].firstIndex_;
195
196 for (index = ioffset; index < ioffset + layers_[oldlayer].nNode_; index++)
197 {
198 id = N(index).id_ << 2;
199 ICHILD(0) = newNode(IV(0), IW(2), IW(1), id++, index);
200 ICHILD(1) = newNode(IV(1), IW(0), IW(2), id++, index);
201 ICHILD(2) = newNode(IV(2), IW(1), IW(0), id++, index);
202 ICHILD(3) = newNode(IW(0), IW(1), IW(2), id, index);
203 }
204 }
205
206 /////////////NEWNODE//////////////////////////////////////
207 // newNode: make a new node
208 //
newNode(size_t v1,size_t v2,size_t v3,uint64 id,uint64 parent)209 uint64 SpatialIndex::newNode(size_t v1, size_t v2, size_t v3, uint64 id, uint64 parent)
210 {
211 IV_(0) = v1; // vertex indices
212 IV_(1) = v2;
213 IV_(2) = v3;
214
215 IW_(0) = 0; // middle point indices
216 IW_(1) = 0;
217 IW_(2) = 0;
218
219 ICHILD_(0) = 0; // child indices
220 ICHILD_(1) = 0; // index 0 is invalid node.
221 ICHILD_(2) = 0;
222 ICHILD_(3) = 0;
223
224 N(index_).id_ = id; // set the id
225 N(index_).index_ = index_; // set the index
226 N(index_).parent_ = parent; // set the parent
227
228 return index_++;
229 }
230
231 /////////////VMAX/////////////////////////////////////////
232 // vMax: compute the maximum number of vertices for the
233 // polyhedron after buildlevel of subdivisions and
234 // the total number of nodes that we store
235 // also, calculate the number of leaf nodes that we eventually have.
236 //
vMax(size_t * nodes,size_t * vertices)237 void SpatialIndex::vMax(size_t *nodes, size_t *vertices)
238 {
239 uint64 nv = 6; // initial values
240 uint64 ne = 12;
241 uint64 nf = 8;
242 int32 i = buildlevel_;
243 *nodes = (size_t)nf;
244
245 while (i-- > 0)
246 {
247 nv += ne;
248 nf *= 4;
249 ne = nf + nv - 2;
250 *nodes += (size_t)nf;
251 }
252 *vertices = (size_t)nv;
253 storedleaves_ = nf;
254
255 // calculate number of leaves
256 i = maxlevel_ - buildlevel_;
257 while (i-- > 0)
258 nf *= 4;
259 leaves_ = nf;
260 }
261
262 /////////////SORTINDEX////////////////////////////////////
263 // sortIndex: sort the index so that the first node is the invalid node
264 // (index 0), the next 8 nodes are the root nodes
265 // and then we put all the leaf nodes in the following block
266 // in ascending id-order.
267 // All the rest of the nodes is at the end.
sortIndex()268 void SpatialIndex::sortIndex()
269 {
270 std::vector<QuadNode> oldnodes(nodes_); // create a copy of the node list
271 size_t index;
272 size_t nonleaf;
273 size_t leaf;
274
275 #define ON(x) oldnodes[(x)]
276
277 // now refill the nodes_ list according to our sorting.
278 for (index = IOFFSET, leaf = IOFFSET, nonleaf = nodes_.size() - 1; index < nodes_.size(); index++)
279 {
280 if (ON(index).childID_[0] == 0) // childnode
281 {
282 // set leaf into list
283 N(leaf) = ON(index);
284 // set parent's pointer to this leaf
285 for (size_t i = 0; i < 4; i++)
286 {
287 if (N(N(leaf).parent_).childID_[i] == index)
288 {
289 N(N(leaf).parent_).childID_[i] = leaf;
290 break;
291 }
292 }
293 leaf++;
294 }
295 else
296 {
297 // set nonleaf into list from the end
298 // set parent of the children already to this
299 // index, they come later in the list.
300 N(nonleaf) = ON(index);
301 ON(N(nonleaf).childID_[0]).parent_ = nonleaf;
302 ON(N(nonleaf).childID_[1]).parent_ = nonleaf;
303 ON(N(nonleaf).childID_[2]).parent_ = nonleaf;
304 ON(N(nonleaf).childID_[3]).parent_ = nonleaf;
305 // set parent's pointer to this leaf
306 for (size_t i = 0; i < 4; i++)
307 {
308 if (N(N(nonleaf).parent_).childID_[i] == index)
309 {
310 N(N(nonleaf).parent_).childID_[i] = nonleaf;
311 break;
312 }
313 }
314 nonleaf--;
315 }
316 }
317 }
318 //////////////////IDBYNAME/////////////////////////////////////////////////
319 // Translate ascii leaf name to a uint32
320 //
321 // The following encoding is used:
322 //
323 // The string leaf name has the always the same structure, it begins with
324 // an N or S, indicating north or south cap and then numbers 0-3 follow
325 // indicating which child to descend into. So for a depth-5-index we have
326 // strings like
327 // N012023 S000222 N102302 etc
328 //
329 // Each of the numbers correspond to 2 bits of code (00 01 10 11) in the
330 // uint32. The first two bits are 10 for S and 11 for N. For example
331 //
332 // N 0 1 2 0 2 3
333 // 11000110001011 = 12683 (dec)
334 //
335 // The leading bits are always 0.
336 //
337 // --- WARNING: This works only up to 15 levels.
338 // (we probably never need more than 7)
339 //
340
idByName(const char * name)341 uint64 SpatialIndex::idByName(const char *name)
342 {
343 uint64 out = 0, i;
344 uint32 size = 0;
345
346 if (name == nullptr) // null pointer-name
347 throw SpatialFailure("SpatialIndex:idByName:no name given");
348 if (name[0] != 'N' && name[0] != 'S') // invalid name
349 throw SpatialFailure("SpatialIndex:idByName:invalid name", name);
350
351 size = strlen(name); // determine string length
352 // at least size-2 required, don't exceed max
353 if (size < 2)
354 throw SpatialFailure("SpatialIndex:idByName:invalid name - too short ", name);
355 if (size > HTMNAMEMAX)
356 throw SpatialFailure("SpatialIndex:idByName:invalid name - too long ", name);
357
358 for (i = size - 1; i > 0; i--) // set bits starting from the end
359 {
360 if (name[i] > '3' || name[i] < '0') // invalid name
361 throw SpatialFailure("SpatialIndex:idByName:invalid name digit ", name);
362 out += (uint64(name[i] - '0') << 2 * (size - i - 1));
363 }
364
365 i = 2; // set first pair of bits, first bit always set
366 if (name[0] == 'N')
367 i++; // for north set second bit too
368 out += (i << (2 * size - 2));
369
370 /************************
371 // This code may be used later for hashing !
372 if(size==2)out -= 8;
373 else {
374 size -= 2;
375 uint32 offset = 0, level4 = 8;
376 for(i = size; i > 0; i--) { // calculate 4 ^ (level-1), level = size-2
377 offset += level4;
378 level4 *= 4;
379 }
380 out -= level4 - offset;
381 }
382 **************************/
383 return out;
384 }
385
386 //////////////////NAMEBYID/////////////////////////////////////////////////
387 // Translate uint32 to an ascii leaf name
388 //
389 // The encoding described above may be decoded again using the following
390 // procedure:
391 //
392 // * Traverse the uint32 from left to right.
393 // * Find the first 'true' bit.
394 // * The first pair gives N (11) or S (10).
395 // * The subsequent bit-pairs give the numbers 0-3.
396 //
397
nameById(uint64 id,char * name)398 char *SpatialIndex::nameById(uint64 id, char *name)
399 {
400 uint32 size = 0, i;
401 #ifdef _WIN32
402 uint64 IDHIGHBIT = 1;
403 uint64 IDHIGHBIT2 = 1;
404 IDHIGHBIT = IDHIGHBIT << 63;
405 IDHIGHBIT2 = IDHIGHBIT2 << 62;
406 #endif
407
408 /*************
409 // This code might be useful for hashing later !!
410
411 // calculate the level (i.e. 8*4^level) and add it to the id:
412 uint32 level=0, level4=8, offset=8;
413 while(id >= offset) {
414 if(++level > 13) { ok = false; offset = 0; break; }// level too deep
415 level4 *= 4;
416 offset += level4;
417 }
418 id += 2 * level4 - offset;
419 **************/
420
421 // determine index of first set bit
422 for (i = 0; i < IDSIZE; i += 2)
423 {
424 if ((id << i) & IDHIGHBIT)
425 break;
426 if ((id << i) & IDHIGHBIT2) // invalid id
427 throw SpatialFailure("SpatialIndex:nameById: invalid ID");
428 }
429 if (id == 0)
430 throw SpatialFailure("SpatialIndex:nameById: invalid ID");
431
432 size = (IDSIZE - i) >> 1;
433 // allocate characters
434 if (!name)
435 name = new char[size + 1];
436
437 // fill characters starting with the last one
438 for (i = 0; i < size - 1; i++)
439 name[size - i - 1] = '0' + char((id >> i * 2) & 3);
440
441 // put in first character
442 if ((id >> (size * 2 - 2)) & 1)
443 {
444 name[0] = 'N';
445 }
446 else
447 {
448 name[0] = 'S';
449 }
450 name[size] = 0; // end string
451
452 return name;
453 }
454 //////////////////POINTBYID////////////////////////////////////////////////
455 // Find a vector for the leaf node given by its ID
456 //
pointById(SpatialVector & vec,uint64 ID) const457 void SpatialIndex::pointById(SpatialVector &vec, uint64 ID) const
458 {
459 // uint64 index;
460 float64 center_x, center_y, center_z, sum;
461 char name[HTMNAMEMAX];
462
463 SpatialVector v0, v1, v2; //
464
465 this->nodeVertex(ID, v0, v1, v2);
466
467 nameById(ID, name);
468 /*
469 I started to go this way until I discovered nameByID...
470 Some docs would be nice for this
471
472 switch(name[1]){
473 case '0':
474 index = name[0] == 'S' ? 1 : 5;
475 break;
476 case '1':
477 index = name[0] == 'S' ? 2 : 6;
478 break;
479 case '2':
480 index = name[0] == 'S' ? 3 : 7;
481 break;
482 case '3':
483 index = name[0] == 'S' ? 4 : 8;
484 break;
485 }
486 */
487 // cerr << "---------- Point by id: " << name << endl;
488 // v0.show(); v1.show(); v2.show();
489 center_x = v0.x_ + v1.x_ + v2.x_;
490 center_y = v0.y_ + v1.y_ + v2.y_;
491 center_z = v0.z_ + v1.z_ + v2.z_;
492 sum = center_x * center_x + center_y * center_y + center_z * center_z;
493 sum = sqrt(sum);
494 center_x /= sum;
495 center_y /= sum;
496 center_z /= sum;
497 vec.x_ = center_x;
498 vec.y_ = center_y;
499 vec.z_ = center_z; // I don't want it normalized or radec to be set,
500 // cerr << " - - - - " << endl;
501 // vec.show();
502 // cerr << "---------- Point by id Retuning" << endl;
503 }
504 //////////////////IDBYPOINT////////////////////////////////////////////////
505 // Find a leaf node where a vector points to
506 //
507
idByPoint(const SpatialVector & v) const508 uint64 SpatialIndex::idByPoint(const SpatialVector &v) const
509 {
510 uint64 index;
511
512 // start with the 8 root triangles, find the one which v points to
513 for (index = 1; index <= 8; index++)
514 {
515 if ((V(0) ^ V(1)) * v < -gEpsilon)
516 continue;
517 if ((V(1) ^ V(2)) * v < -gEpsilon)
518 continue;
519 if ((V(2) ^ V(0)) * v < -gEpsilon)
520 continue;
521 break;
522 }
523 // loop through matching child until leaves are reached
524 while (ICHILD(0) != 0)
525 {
526 uint64 oldindex = index;
527 for (size_t i = 0; i < 4; i++)
528 {
529 index = nodes_[oldindex].childID_[i];
530 if ((V(0) ^ V(1)) * v < -gEpsilon)
531 continue;
532 if ((V(1) ^ V(2)) * v < -gEpsilon)
533 continue;
534 if ((V(2) ^ V(0)) * v < -gEpsilon)
535 continue;
536 break;
537 }
538 }
539 // return if we have reached maxlevel
540 if (maxlevel_ == buildlevel_)
541 return N(index).id_;
542
543 // from now on, continue to build name dynamically.
544 // until maxlevel_ levels depth, continue to append the
545 // correct index, build the index on the fly.
546 char name[HTMNAMEMAX];
547 nameById(N(index).id_, name);
548 size_t len = strlen(name);
549 SpatialVector v0 = V(0);
550 SpatialVector v1 = V(1);
551 SpatialVector v2 = V(2);
552
553 size_t level = maxlevel_ - buildlevel_;
554 while (level--)
555 {
556 SpatialVector w0 = v1 + v2;
557 w0.normalize();
558 SpatialVector w1 = v0 + v2;
559 w1.normalize();
560 SpatialVector w2 = v1 + v0;
561 w2.normalize();
562
563 if (isInside(v, v0, w2, w1))
564 {
565 name[len++] = '0';
566 v1 = w2;
567 v2 = w1;
568 continue;
569 }
570 else if (isInside(v, v1, w0, w2))
571 {
572 name[len++] = '1';
573 v0 = v1;
574 v1 = w0;
575 v2 = w2;
576 continue;
577 }
578 else if (isInside(v, v2, w1, w0))
579 {
580 name[len++] = '2';
581 v0 = v2;
582 v1 = w1;
583 v2 = w0;
584 continue;
585 }
586 else if (isInside(v, w0, w1, w2))
587 {
588 name[len++] = '3';
589 v0 = w0;
590 v1 = w1;
591 v2 = w2;
592 continue;
593 }
594 }
595 name[len] = '\0';
596 return idByName(name);
597 }
598
599 //////////////////ISINSIDE/////////////////////////////////////////////////
600 // Test whether a vector is inside a triangle. Input triangle has
601 // to be sorted in a counter-clockwise direction.
602 //
isInside(const SpatialVector & v,const SpatialVector & v0,const SpatialVector & v1,const SpatialVector & v2) const603 bool SpatialIndex::isInside(const SpatialVector &v, const SpatialVector &v0, const SpatialVector &v1,
604 const SpatialVector &v2) const
605 {
606 if ((v0 ^ v1) * v < -gEpsilon)
607 return false;
608 if ((v1 ^ v2) * v < -gEpsilon)
609 return false;
610 if ((v2 ^ v0) * v < -gEpsilon)
611 return false;
612 return true;
613 }
614