File indexing completed on 2024-04-21 14:46:00
0001 //# Filename: SpatialIndex.cpp 0002 //# 0003 //# The SpatialIndex class is defined here. 0004 //# 0005 //# Author: Peter Z. Kunszt based on A. Szalay's code 0006 //# 0007 //# Date: October 15, 1998 0008 //# 0009 //# SPDX-FileCopyrightText: 2000 Peter Z. Kunszt Alex S. Szalay, Aniruddha R. Thakar 0010 //# The Johns Hopkins University 0011 //# 0012 //# Modification History: 0013 //# 0014 //# Oct 18, 2001 : Dennis C. Dinge -- Replaced ValVec with std::vector 0015 //# Jul 25, 2002 : Gyorgy Fekete -- Added pointById() 0016 //# 0017 0018 #include "SpatialIndex.h" 0019 #include "SpatialException.h" 0020 0021 #ifdef _WIN32 0022 #include <malloc.h> 0023 #else 0024 #ifdef __APPLE__ 0025 #include <sys/malloc.h> 0026 #else 0027 #include <cstdlib> 0028 #endif 0029 #endif 0030 0031 #include <cstdio> 0032 #include <cmath> 0033 // =========================================================================== 0034 // 0035 // Macro definitions for readability 0036 // 0037 // =========================================================================== 0038 0039 #define N(x) nodes_[(x)] 0040 #define V(x) vertices_[nodes_[index].v_[(x)]] 0041 #define IV(x) nodes_[index].v_[(x)] 0042 #define W(x) vertices_[nodes_[index].w_[(x)]] 0043 #define IW(x) nodes_[index].w_[(x)] 0044 #define ICHILD(x) nodes_[index].childID_[(x)] 0045 0046 #define IV_(x) nodes_[index_].v_[(x)] 0047 #define IW_(x) nodes_[index_].w_[(x)] 0048 #define ICHILD_(x) nodes_[index_].childID_[(x)] 0049 #define IOFFSET 9 0050 0051 // =========================================================================== 0052 // 0053 // Member functions for class SpatialIndex 0054 // 0055 // =========================================================================== 0056 0057 /////////////CONSTRUCTOR////////////////////////////////// 0058 // 0059 SpatialIndex::SpatialIndex(size_t maxlevel, size_t buildlevel) 0060 : maxlevel_(maxlevel), buildlevel_((buildlevel == 0 || buildlevel > maxlevel) ? maxlevel : buildlevel) 0061 { 0062 size_t nodes, vertices; 0063 0064 vMax(&nodes, &vertices); 0065 layers_.resize(buildlevel_ + 1); // allocate enough space already 0066 nodes_.resize(nodes + 1); // allocate space for all nodes 0067 vertices_.resize(vertices + 1); // allocate space for all vertices 0068 0069 N(0).index_ = 0; // initialize invalid node 0070 0071 // initialize first layer 0072 layers_[0].level_ = 0; 0073 layers_[0].nVert_ = 6; 0074 layers_[0].nNode_ = 8; 0075 layers_[0].nEdge_ = 12; 0076 layers_[0].firstIndex_ = 1; 0077 layers_[0].firstVertex_ = 0; 0078 0079 // set the first 6 vertices 0080 float64 v[6][3] = { 0081 { 0.0L, 0.0L, 1.0L }, // 0 0082 { 1.0L, 0.0L, 0.0L }, // 1 0083 { 0.0L, 1.0L, 0.0L }, // 2 0084 { -1.0L, 0.0L, 0.0L }, // 3 0085 { 0.0L, -1.0L, 0.0L }, // 4 0086 { 0.0L, 0.0L, -1.0L } // 5 0087 }; 0088 0089 for (int i = 0; i < 6; i++) 0090 vertices_[i].set(v[i][0], v[i][1], v[i][2]); 0091 0092 // create the first 8 nodes - index 1 through 8 0093 index_ = 1; 0094 newNode(1, 5, 2, 8, 0); // S0 0095 newNode(2, 5, 3, 9, 0); // S1 0096 newNode(3, 5, 4, 10, 0); // S2 0097 newNode(4, 5, 1, 11, 0); // S3 0098 newNode(1, 0, 4, 12, 0); // N0 0099 newNode(4, 0, 3, 13, 0); // N1 0100 newNode(3, 0, 2, 14, 0); // N2 0101 newNode(2, 0, 1, 15, 0); // N3 0102 0103 // loop through buildlevel steps, and build the nodes for each layer 0104 0105 size_t pl = 0; 0106 size_t level = buildlevel_; 0107 while (level-- > 0) 0108 { 0109 SpatialEdge edge(*this, pl); 0110 edge.makeMidPoints(); 0111 makeNewLayer(pl); 0112 ++pl; 0113 } 0114 0115 sortIndex(); 0116 } 0117 0118 /////////////NODEVERTEX/////////////////////////////////// 0119 // nodeVertex: return the vectors of the vertices, based on the ID 0120 // 0121 void SpatialIndex::nodeVertex(const uint64 id, SpatialVector &v0, SpatialVector &v1, SpatialVector &v2) const 0122 { 0123 if (buildlevel_ == maxlevel_) 0124 { 0125 uint32 idx = (uint32)id - leaves_ + IOFFSET; // -jbb: Fix segfault. See "idx =" below. 0126 v0 = vertices_[nodes_[idx].v_[0]]; 0127 v1 = vertices_[nodes_[idx].v_[1]]; 0128 v2 = vertices_[nodes_[idx].v_[2]]; 0129 return; 0130 } 0131 0132 // buildlevel < maxlevel 0133 // get the id of the stored leaf that we are in 0134 // and get the vertices of the node we want 0135 uint64 sid = id >> ((maxlevel_ - buildlevel_) * 2); 0136 uint32 idx = (uint32)(sid - storedleaves_ + IOFFSET); 0137 0138 v0 = vertices_[nodes_[idx].v_[0]]; 0139 v1 = vertices_[nodes_[idx].v_[1]]; 0140 v2 = vertices_[nodes_[idx].v_[2]]; 0141 0142 // loop through additional levels, 0143 // pick the correct triangle accordingly, storing the 0144 // vertices in v1,v2,v3 0145 for (uint32 i = buildlevel_ + 1; i <= maxlevel_; i++) 0146 { 0147 uint64 j = (id >> ((maxlevel_ - i) * 2)) & 3; 0148 SpatialVector w0 = v1 + v2; 0149 w0.normalize(); 0150 SpatialVector w1 = v0 + v2; 0151 w1.normalize(); 0152 SpatialVector w2 = v1 + v0; 0153 w2.normalize(); 0154 0155 switch (j) 0156 { 0157 case 0: 0158 v1 = w2; 0159 v2 = w1; 0160 break; 0161 case 1: 0162 v0 = v1; 0163 v1 = w0; 0164 v2 = w2; 0165 break; 0166 case 2: 0167 v0 = v2; 0168 v1 = w1; 0169 v2 = w0; 0170 break; 0171 case 3: 0172 v0 = w0; 0173 v1 = w1; 0174 v2 = w2; 0175 break; 0176 } 0177 } 0178 } 0179 0180 /////////////MAKENEWLAYER///////////////////////////////// 0181 // makeNewLayer: generate a new layer and the nodes in it 0182 // 0183 void SpatialIndex::makeNewLayer(size_t oldlayer) 0184 { 0185 uint64 index, id; 0186 size_t newlayer = oldlayer + 1; 0187 layers_[newlayer].level_ = layers_[oldlayer].level_ + 1; 0188 layers_[newlayer].nVert_ = layers_[oldlayer].nVert_ + layers_[oldlayer].nEdge_; 0189 layers_[newlayer].nNode_ = 4 * layers_[oldlayer].nNode_; 0190 layers_[newlayer].nEdge_ = layers_[newlayer].nNode_ + layers_[newlayer].nVert_ - 2; 0191 layers_[newlayer].firstIndex_ = index_; 0192 layers_[newlayer].firstVertex_ = layers_[oldlayer].firstVertex_ + layers_[oldlayer].nVert_; 0193 0194 uint64 ioffset = layers_[oldlayer].firstIndex_; 0195 0196 for (index = ioffset; index < ioffset + layers_[oldlayer].nNode_; index++) 0197 { 0198 id = N(index).id_ << 2; 0199 ICHILD(0) = newNode(IV(0), IW(2), IW(1), id++, index); 0200 ICHILD(1) = newNode(IV(1), IW(0), IW(2), id++, index); 0201 ICHILD(2) = newNode(IV(2), IW(1), IW(0), id++, index); 0202 ICHILD(3) = newNode(IW(0), IW(1), IW(2), id, index); 0203 } 0204 } 0205 0206 /////////////NEWNODE////////////////////////////////////// 0207 // newNode: make a new node 0208 // 0209 uint64 SpatialIndex::newNode(size_t v1, size_t v2, size_t v3, uint64 id, uint64 parent) 0210 { 0211 IV_(0) = v1; // vertex indices 0212 IV_(1) = v2; 0213 IV_(2) = v3; 0214 0215 IW_(0) = 0; // middle point indices 0216 IW_(1) = 0; 0217 IW_(2) = 0; 0218 0219 ICHILD_(0) = 0; // child indices 0220 ICHILD_(1) = 0; // index 0 is invalid node. 0221 ICHILD_(2) = 0; 0222 ICHILD_(3) = 0; 0223 0224 N(index_).id_ = id; // set the id 0225 N(index_).index_ = index_; // set the index 0226 N(index_).parent_ = parent; // set the parent 0227 0228 return index_++; 0229 } 0230 0231 /////////////VMAX///////////////////////////////////////// 0232 // vMax: compute the maximum number of vertices for the 0233 // polyhedron after buildlevel of subdivisions and 0234 // the total number of nodes that we store 0235 // also, calculate the number of leaf nodes that we eventually have. 0236 // 0237 void SpatialIndex::vMax(size_t *nodes, size_t *vertices) 0238 { 0239 uint64 nv = 6; // initial values 0240 uint64 ne = 12; 0241 uint64 nf = 8; 0242 int32 i = buildlevel_; 0243 *nodes = (size_t)nf; 0244 0245 while (i-- > 0) 0246 { 0247 nv += ne; 0248 nf *= 4; 0249 ne = nf + nv - 2; 0250 *nodes += (size_t)nf; 0251 } 0252 *vertices = (size_t)nv; 0253 storedleaves_ = nf; 0254 0255 // calculate number of leaves 0256 i = maxlevel_ - buildlevel_; 0257 while (i-- > 0) 0258 nf *= 4; 0259 leaves_ = nf; 0260 } 0261 0262 /////////////SORTINDEX//////////////////////////////////// 0263 // sortIndex: sort the index so that the first node is the invalid node 0264 // (index 0), the next 8 nodes are the root nodes 0265 // and then we put all the leaf nodes in the following block 0266 // in ascending id-order. 0267 // All the rest of the nodes is at the end. 0268 void SpatialIndex::sortIndex() 0269 { 0270 std::vector<QuadNode> oldnodes(nodes_); // create a copy of the node list 0271 size_t index; 0272 size_t nonleaf; 0273 size_t leaf; 0274 0275 #define ON(x) oldnodes[(x)] 0276 0277 // now refill the nodes_ list according to our sorting. 0278 for (index = IOFFSET, leaf = IOFFSET, nonleaf = nodes_.size() - 1; index < nodes_.size(); index++) 0279 { 0280 if (ON(index).childID_[0] == 0) // childnode 0281 { 0282 // set leaf into list 0283 N(leaf) = ON(index); 0284 // set parent's pointer to this leaf 0285 for (size_t i = 0; i < 4; i++) 0286 { 0287 if (N(N(leaf).parent_).childID_[i] == index) 0288 { 0289 N(N(leaf).parent_).childID_[i] = leaf; 0290 break; 0291 } 0292 } 0293 leaf++; 0294 } 0295 else 0296 { 0297 // set nonleaf into list from the end 0298 // set parent of the children already to this 0299 // index, they come later in the list. 0300 N(nonleaf) = ON(index); 0301 ON(N(nonleaf).childID_[0]).parent_ = nonleaf; 0302 ON(N(nonleaf).childID_[1]).parent_ = nonleaf; 0303 ON(N(nonleaf).childID_[2]).parent_ = nonleaf; 0304 ON(N(nonleaf).childID_[3]).parent_ = nonleaf; 0305 // set parent's pointer to this leaf 0306 for (size_t i = 0; i < 4; i++) 0307 { 0308 if (N(N(nonleaf).parent_).childID_[i] == index) 0309 { 0310 N(N(nonleaf).parent_).childID_[i] = nonleaf; 0311 break; 0312 } 0313 } 0314 nonleaf--; 0315 } 0316 } 0317 } 0318 //////////////////IDBYNAME///////////////////////////////////////////////// 0319 // Translate ascii leaf name to a uint32 0320 // 0321 // The following encoding is used: 0322 // 0323 // The string leaf name has the always the same structure, it begins with 0324 // an N or S, indicating north or south cap and then numbers 0-3 follow 0325 // indicating which child to descend into. So for a depth-5-index we have 0326 // strings like 0327 // N012023 S000222 N102302 etc 0328 // 0329 // Each of the numbers correspond to 2 bits of code (00 01 10 11) in the 0330 // uint32. The first two bits are 10 for S and 11 for N. For example 0331 // 0332 // N 0 1 2 0 2 3 0333 // 11000110001011 = 12683 (dec) 0334 // 0335 // The leading bits are always 0. 0336 // 0337 // --- WARNING: This works only up to 15 levels. 0338 // (we probably never need more than 7) 0339 // 0340 0341 uint64 SpatialIndex::idByName(const char *name) 0342 { 0343 uint64 out = 0, i; 0344 uint32 size = 0; 0345 0346 if (name == nullptr) // null pointer-name 0347 throw SpatialFailure("SpatialIndex:idByName:no name given"); 0348 if (name[0] != 'N' && name[0] != 'S') // invalid name 0349 throw SpatialFailure("SpatialIndex:idByName:invalid name", name); 0350 0351 size = strlen(name); // determine string length 0352 // at least size-2 required, don't exceed max 0353 if (size < 2) 0354 throw SpatialFailure("SpatialIndex:idByName:invalid name - too short ", name); 0355 if (size > HTMNAMEMAX) 0356 throw SpatialFailure("SpatialIndex:idByName:invalid name - too long ", name); 0357 0358 for (i = size - 1; i > 0; i--) // set bits starting from the end 0359 { 0360 if (name[i] > '3' || name[i] < '0') // invalid name 0361 throw SpatialFailure("SpatialIndex:idByName:invalid name digit ", name); 0362 out += (uint64(name[i] - '0') << 2 * (size - i - 1)); 0363 } 0364 0365 i = 2; // set first pair of bits, first bit always set 0366 if (name[0] == 'N') 0367 i++; // for north set second bit too 0368 out += (i << (2 * size - 2)); 0369 0370 /************************ 0371 // This code may be used later for hashing ! 0372 if(size==2)out -= 8; 0373 else { 0374 size -= 2; 0375 uint32 offset = 0, level4 = 8; 0376 for(i = size; i > 0; i--) { // calculate 4 ^ (level-1), level = size-2 0377 offset += level4; 0378 level4 *= 4; 0379 } 0380 out -= level4 - offset; 0381 } 0382 **************************/ 0383 return out; 0384 } 0385 0386 //////////////////NAMEBYID///////////////////////////////////////////////// 0387 // Translate uint32 to an ascii leaf name 0388 // 0389 // The encoding described above may be decoded again using the following 0390 // procedure: 0391 // 0392 // * Traverse the uint32 from left to right. 0393 // * Find the first 'true' bit. 0394 // * The first pair gives N (11) or S (10). 0395 // * The subsequent bit-pairs give the numbers 0-3. 0396 // 0397 0398 char *SpatialIndex::nameById(uint64 id, char *name) 0399 { 0400 uint32 size = 0, i; 0401 #ifdef _WIN32 0402 uint64 IDHIGHBIT = 1; 0403 uint64 IDHIGHBIT2 = 1; 0404 IDHIGHBIT = IDHIGHBIT << 63; 0405 IDHIGHBIT2 = IDHIGHBIT2 << 62; 0406 #endif 0407 0408 /************* 0409 // This code might be useful for hashing later !! 0410 0411 // calculate the level (i.e. 8*4^level) and add it to the id: 0412 uint32 level=0, level4=8, offset=8; 0413 while(id >= offset) { 0414 if(++level > 13) { ok = false; offset = 0; break; }// level too deep 0415 level4 *= 4; 0416 offset += level4; 0417 } 0418 id += 2 * level4 - offset; 0419 **************/ 0420 0421 // determine index of first set bit 0422 for (i = 0; i < IDSIZE; i += 2) 0423 { 0424 if ((id << i) & IDHIGHBIT) 0425 break; 0426 if ((id << i) & IDHIGHBIT2) // invalid id 0427 throw SpatialFailure("SpatialIndex:nameById: invalid ID"); 0428 } 0429 if (id == 0) 0430 throw SpatialFailure("SpatialIndex:nameById: invalid ID"); 0431 0432 size = (IDSIZE - i) >> 1; 0433 // allocate characters 0434 if (!name) 0435 name = new char[size + 1]; 0436 0437 // fill characters starting with the last one 0438 for (i = 0; i < size - 1; i++) 0439 name[size - i - 1] = '0' + char((id >> i * 2) & 3); 0440 0441 // put in first character 0442 if ((id >> (size * 2 - 2)) & 1) 0443 { 0444 name[0] = 'N'; 0445 } 0446 else 0447 { 0448 name[0] = 'S'; 0449 } 0450 name[size] = 0; // end string 0451 0452 return name; 0453 } 0454 //////////////////POINTBYID//////////////////////////////////////////////// 0455 // Find a vector for the leaf node given by its ID 0456 // 0457 void SpatialIndex::pointById(SpatialVector &vec, uint64 ID) const 0458 { 0459 // uint64 index; 0460 float64 center_x, center_y, center_z, sum; 0461 char name[HTMNAMEMAX]; 0462 0463 SpatialVector v0, v1, v2; // 0464 0465 this->nodeVertex(ID, v0, v1, v2); 0466 0467 nameById(ID, name); 0468 /* 0469 I started to go this way until I discovered nameByID... 0470 Some docs would be nice for this 0471 0472 switch(name[1]){ 0473 case '0': 0474 index = name[0] == 'S' ? 1 : 5; 0475 break; 0476 case '1': 0477 index = name[0] == 'S' ? 2 : 6; 0478 break; 0479 case '2': 0480 index = name[0] == 'S' ? 3 : 7; 0481 break; 0482 case '3': 0483 index = name[0] == 'S' ? 4 : 8; 0484 break; 0485 } 0486 */ 0487 // cerr << "---------- Point by id: " << name << Qt::endl; 0488 // v0.show(); v1.show(); v2.show(); 0489 center_x = v0.x_ + v1.x_ + v2.x_; 0490 center_y = v0.y_ + v1.y_ + v2.y_; 0491 center_z = v0.z_ + v1.z_ + v2.z_; 0492 sum = center_x * center_x + center_y * center_y + center_z * center_z; 0493 sum = sqrt(sum); 0494 center_x /= sum; 0495 center_y /= sum; 0496 center_z /= sum; 0497 vec.x_ = center_x; 0498 vec.y_ = center_y; 0499 vec.z_ = center_z; // I don't want it normalized or radec to be set, 0500 // cerr << " - - - - " << Qt::endl; 0501 // vec.show(); 0502 // cerr << "---------- Point by id Retuning" << Qt::endl; 0503 } 0504 //////////////////IDBYPOINT//////////////////////////////////////////////// 0505 // Find a leaf node where a vector points to 0506 // 0507 0508 uint64 SpatialIndex::idByPoint(const SpatialVector &v) const 0509 { 0510 uint64 index; 0511 0512 // start with the 8 root triangles, find the one which v points to 0513 for (index = 1; index <= 8; index++) 0514 { 0515 if ((V(0) ^ V(1)) * v < -gEpsilon) 0516 continue; 0517 if ((V(1) ^ V(2)) * v < -gEpsilon) 0518 continue; 0519 if ((V(2) ^ V(0)) * v < -gEpsilon) 0520 continue; 0521 break; 0522 } 0523 // loop through matching child until leaves are reached 0524 while (ICHILD(0) != 0) 0525 { 0526 uint64 oldindex = index; 0527 for (size_t i = 0; i < 4; i++) 0528 { 0529 index = nodes_[oldindex].childID_[i]; 0530 if ((V(0) ^ V(1)) * v < -gEpsilon) 0531 continue; 0532 if ((V(1) ^ V(2)) * v < -gEpsilon) 0533 continue; 0534 if ((V(2) ^ V(0)) * v < -gEpsilon) 0535 continue; 0536 break; 0537 } 0538 } 0539 // return if we have reached maxlevel 0540 if (maxlevel_ == buildlevel_) 0541 return N(index).id_; 0542 0543 // from now on, continue to build name dynamically. 0544 // until maxlevel_ levels depth, continue to append the 0545 // correct index, build the index on the fly. 0546 char name[HTMNAMEMAX]; 0547 nameById(N(index).id_, name); 0548 size_t len = strlen(name); 0549 SpatialVector v0 = V(0); 0550 SpatialVector v1 = V(1); 0551 SpatialVector v2 = V(2); 0552 0553 size_t level = maxlevel_ - buildlevel_; 0554 while (level--) 0555 { 0556 SpatialVector w0 = v1 + v2; 0557 w0.normalize(); 0558 SpatialVector w1 = v0 + v2; 0559 w1.normalize(); 0560 SpatialVector w2 = v1 + v0; 0561 w2.normalize(); 0562 0563 if (isInside(v, v0, w2, w1)) 0564 { 0565 name[len++] = '0'; 0566 v1 = w2; 0567 v2 = w1; 0568 continue; 0569 } 0570 else if (isInside(v, v1, w0, w2)) 0571 { 0572 name[len++] = '1'; 0573 v0 = v1; 0574 v1 = w0; 0575 v2 = w2; 0576 continue; 0577 } 0578 else if (isInside(v, v2, w1, w0)) 0579 { 0580 name[len++] = '2'; 0581 v0 = v2; 0582 v1 = w1; 0583 v2 = w0; 0584 continue; 0585 } 0586 else if (isInside(v, w0, w1, w2)) 0587 { 0588 name[len++] = '3'; 0589 v0 = w0; 0590 v1 = w1; 0591 v2 = w2; 0592 continue; 0593 } 0594 } 0595 name[len] = '\0'; 0596 return idByName(name); 0597 } 0598 0599 //////////////////ISINSIDE///////////////////////////////////////////////// 0600 // Test whether a vector is inside a triangle. Input triangle has 0601 // to be sorted in a counter-clockwise direction. 0602 // 0603 bool SpatialIndex::isInside(const SpatialVector &v, const SpatialVector &v0, const SpatialVector &v1, 0604 const SpatialVector &v2) const 0605 { 0606 if ((v0 ^ v1) * v < -gEpsilon) 0607 return false; 0608 if ((v1 ^ v2) * v < -gEpsilon) 0609 return false; 0610 if ((v2 ^ v0) * v < -gEpsilon) 0611 return false; 0612 return true; 0613 }