File indexing completed on 2024-04-21 03:44:01

 //#     Filename:       SpatialIndex.cpp
 //#
 //#     The SpatialIndex class is defined here.
 //#
 //#     Author:         Peter Z. Kunszt based on A. Szalay's code
 //#
 //#     Date:           October 15, 1998
 //#
 //#     SPDX-FileCopyrightText: 2000 Peter Z. Kunszt Alex S. Szalay, Aniruddha R. Thakar
 //#                     The Johns Hopkins University
 //#
 //#     Modification History:
 //#
 //#     Oct 18, 2001 : Dennis C. Dinge -- Replaced ValVec with std::vector
 //#     Jul 25, 2002 : Gyorgy Fekete -- Added pointById()
 //#
 
 #include "SpatialIndex.h"
 #include "SpatialException.h"
 
 #ifdef _WIN32
 #include <malloc.h>
 #else
 #ifdef __APPLE__
 #include <sys/malloc.h>
 #else
 #include <cstdlib>
 #endif
 #endif
 
 #include <cstdio>
 #include <cmath>
 // ===========================================================================
 //
 // Macro definitions for readability
 //
 // ===========================================================================
 
 #define N(x)      nodes_[(x)]
 #define V(x)      vertices_[nodes_[index].v_[(x)]]
 #define IV(x)     nodes_[index].v_[(x)]
 #define W(x)      vertices_[nodes_[index].w_[(x)]]
 #define IW(x)     nodes_[index].w_[(x)]
 #define ICHILD(x) nodes_[index].childID_[(x)]
 
 #define IV_(x)     nodes_[index_].v_[(x)]
 #define IW_(x)     nodes_[index_].w_[(x)]
 #define ICHILD_(x) nodes_[index_].childID_[(x)]
 #define IOFFSET    9
 
 // ===========================================================================
 //
 // Member functions for class SpatialIndex
 //
 // ===========================================================================
 
 /////////////CONSTRUCTOR//////////////////////////////////
 //
 SpatialIndex::SpatialIndex(size_t maxlevel, size_t buildlevel)
     : maxlevel_(maxlevel), buildlevel_((buildlevel == 0 || buildlevel > maxlevel) ? maxlevel : buildlevel)
 {
     size_t nodes, vertices;
 
     vMax(&nodes, &vertices);
     layers_.resize(buildlevel_ + 1); // allocate enough space already
     nodes_.resize(nodes + 1);        // allocate space for all nodes
     vertices_.resize(vertices + 1);  // allocate space for all vertices
 
     N(0).index_ = 0; // initialize invalid node
 
     // initialize first layer
     layers_[0].level_       = 0;
     layers_[0].nVert_       = 6;
     layers_[0].nNode_       = 8;
     layers_[0].nEdge_       = 12;
     layers_[0].firstIndex_  = 1;
     layers_[0].firstVertex_ = 0;
 
     // set the first 6 vertices
     float64 v[6][3] = {
         { 0.0L, 0.0L, 1.0L },  // 0
         { 1.0L, 0.0L, 0.0L },  // 1
         { 0.0L, 1.0L, 0.0L },  // 2
         { -1.0L, 0.0L, 0.0L }, // 3
         { 0.0L, -1.0L, 0.0L }, // 4
         { 0.0L, 0.0L, -1.0L }  // 5
     };
 
     for (int i = 0; i < 6; i++)
         vertices_[i].set(v[i][0], v[i][1], v[i][2]);
 
     // create the first 8 nodes - index 1 through 8
     index_ = 1;
     newNode(1, 5, 2, 8, 0);  // S0
     newNode(2, 5, 3, 9, 0);  // S1
     newNode(3, 5, 4, 10, 0); // S2
     newNode(4, 5, 1, 11, 0); // S3
     newNode(1, 0, 4, 12, 0); // N0
     newNode(4, 0, 3, 13, 0); // N1
     newNode(3, 0, 2, 14, 0); // N2
     newNode(2, 0, 1, 15, 0); // N3
 
     //  loop through buildlevel steps, and build the nodes for each layer
 
     size_t pl    = 0;
     size_t level = buildlevel_;
     while (level-- > 0)
     {
         SpatialEdge edge(*this, pl);
         edge.makeMidPoints();
         makeNewLayer(pl);
         ++pl;
     }
 
     sortIndex();
 }
 
 /////////////NODEVERTEX///////////////////////////////////
 // nodeVertex: return the vectors of the vertices, based on the ID
 //
 void SpatialIndex::nodeVertex(const uint64 id, SpatialVector &v0, SpatialVector &v1, SpatialVector &v2) const
 {
     if (buildlevel_ == maxlevel_)
     {
         uint32 idx = (uint32)id - leaves_ + IOFFSET; // -jbb: Fix segfault.  See "idx =" below.
         v0         = vertices_[nodes_[idx].v_[0]];
         v1         = vertices_[nodes_[idx].v_[1]];
         v2         = vertices_[nodes_[idx].v_[2]];
         return;
     }
 
     // buildlevel < maxlevel
     // get the id of the stored leaf that we are in
     // and get the vertices of the node we want
     uint64 sid = id >> ((maxlevel_ - buildlevel_) * 2);
     uint32 idx = (uint32)(sid - storedleaves_ + IOFFSET);
 
     v0 = vertices_[nodes_[idx].v_[0]];
     v1 = vertices_[nodes_[idx].v_[1]];
     v2 = vertices_[nodes_[idx].v_[2]];
 
     // loop through additional levels,
     // pick the correct triangle accordingly, storing the
     // vertices in v1,v2,v3
     for (uint32 i = buildlevel_ + 1; i <= maxlevel_; i++)
     {
         uint64 j         = (id >> ((maxlevel_ - i) * 2)) & 3;
         SpatialVector w0 = v1 + v2;
         w0.normalize();
         SpatialVector w1 = v0 + v2;
         w1.normalize();
         SpatialVector w2 = v1 + v0;
         w2.normalize();
 
         switch (j)
         {
             case 0:
                 v1 = w2;
                 v2 = w1;
                 break;
             case 1:
                 v0 = v1;
                 v1 = w0;
                 v2 = w2;
                 break;
             case 2:
                 v0 = v2;
                 v1 = w1;
                 v2 = w0;
                 break;
             case 3:
                 v0 = w0;
                 v1 = w1;
                 v2 = w2;
                 break;
         }
     }
 }
 
 /////////////MAKENEWLAYER/////////////////////////////////
 // makeNewLayer: generate a new layer and the nodes in it
 //
 void SpatialIndex::makeNewLayer(size_t oldlayer)
 {
     uint64 index, id;
     size_t newlayer                = oldlayer + 1;
     layers_[newlayer].level_       = layers_[oldlayer].level_ + 1;
     layers_[newlayer].nVert_       = layers_[oldlayer].nVert_ + layers_[oldlayer].nEdge_;
     layers_[newlayer].nNode_       = 4 * layers_[oldlayer].nNode_;
     layers_[newlayer].nEdge_       = layers_[newlayer].nNode_ + layers_[newlayer].nVert_ - 2;
     layers_[newlayer].firstIndex_  = index_;
     layers_[newlayer].firstVertex_ = layers_[oldlayer].firstVertex_ + layers_[oldlayer].nVert_;
 
     uint64 ioffset = layers_[oldlayer].firstIndex_;
 
     for (index = ioffset; index < ioffset + layers_[oldlayer].nNode_; index++)
     {
         id        = N(index).id_ << 2;
         ICHILD(0) = newNode(IV(0), IW(2), IW(1), id++, index);
         ICHILD(1) = newNode(IV(1), IW(0), IW(2), id++, index);
         ICHILD(2) = newNode(IV(2), IW(1), IW(0), id++, index);
         ICHILD(3) = newNode(IW(0), IW(1), IW(2), id, index);
     }
 }
 
 /////////////NEWNODE//////////////////////////////////////
 // newNode: make a new node
 //
 uint64 SpatialIndex::newNode(size_t v1, size_t v2, size_t v3, uint64 id, uint64 parent)
 {
     IV_(0) = v1; // vertex indices
     IV_(1) = v2;
     IV_(2) = v3;
 
     IW_(0) = 0; // middle point indices
     IW_(1) = 0;
     IW_(2) = 0;
 
     ICHILD_(0) = 0; // child indices
     ICHILD_(1) = 0; // index 0 is invalid node.
     ICHILD_(2) = 0;
     ICHILD_(3) = 0;
 
     N(index_).id_     = id;     // set the id
     N(index_).index_  = index_; // set the index
     N(index_).parent_ = parent; // set the parent
 
     return index_++;
 }
 
 /////////////VMAX/////////////////////////////////////////
 // vMax: compute the maximum number of vertices for the
 //       polyhedron after buildlevel of subdivisions and
 //       the total number of nodes that we store
 //       also, calculate the number of leaf nodes that we eventually have.
 //
 void SpatialIndex::vMax(size_t *nodes, size_t *vertices)
 {
     uint64 nv = 6; // initial values
     uint64 ne = 12;
     uint64 nf = 8;
     int32 i   = buildlevel_;
     *nodes    = (size_t)nf;
 
     while (i-- > 0)
     {
         nv += ne;
         nf *= 4;
         ne = nf + nv - 2;
         *nodes += (size_t)nf;
     }
     *vertices     = (size_t)nv;
     storedleaves_ = nf;
 
     // calculate number of leaves
     i = maxlevel_ - buildlevel_;
     while (i-- > 0)
         nf *= 4;
     leaves_ = nf;
 }
 
 /////////////SORTINDEX////////////////////////////////////
 // sortIndex: sort the index so that the first node is the invalid node
 //            (index 0), the next 8 nodes are the root nodes
 //            and then we put all the leaf nodes in the following block
 //            in ascending id-order.
 //            All the rest of the nodes is at the end.
 void SpatialIndex::sortIndex()
 {
     std::vector<QuadNode> oldnodes(nodes_); // create a copy of the node list
     size_t index;
     size_t nonleaf;
     size_t leaf;
 
 #define ON(x) oldnodes[(x)]
 
     // now refill the nodes_ list according to our sorting.
     for (index = IOFFSET, leaf = IOFFSET, nonleaf = nodes_.size() - 1; index < nodes_.size(); index++)
     {
         if (ON(index).childID_[0] == 0) // childnode
         {
             // set leaf into list
             N(leaf) = ON(index);
             // set parent's pointer to this leaf
             for (size_t i = 0; i < 4; i++)
             {
                 if (N(N(leaf).parent_).childID_[i] == index)
                 {
                     N(N(leaf).parent_).childID_[i] = leaf;
                     break;
                 }
             }
             leaf++;
         }
         else
         {
             // set nonleaf into list from the end
             // set parent of the children already to this
             // index, they come later in the list.
             N(nonleaf)                         = ON(index);
             ON(N(nonleaf).childID_[0]).parent_ = nonleaf;
             ON(N(nonleaf).childID_[1]).parent_ = nonleaf;
             ON(N(nonleaf).childID_[2]).parent_ = nonleaf;
             ON(N(nonleaf).childID_[3]).parent_ = nonleaf;
             // set parent's pointer to this leaf
             for (size_t i = 0; i < 4; i++)
             {
                 if (N(N(nonleaf).parent_).childID_[i] == index)
                 {
                     N(N(nonleaf).parent_).childID_[i] = nonleaf;
                     break;
                 }
             }
             nonleaf--;
         }
     }
 }
 //////////////////IDBYNAME/////////////////////////////////////////////////
 // Translate ascii leaf name to a uint32
 //
 // The following encoding is used:
 //
 // The string leaf name has the always the same structure, it begins with
 // an N or S, indicating north or south cap and then numbers 0-3 follow
 // indicating which child to descend into. So for a depth-5-index we have
 // strings like
 //                 N012023  S000222  N102302  etc
 //
 // Each of the numbers correspond to 2 bits of code (00 01 10 11) in the
 // uint32. The first two bits are 10 for S and 11 for N. For example
 //
 //                 N 0 1 2 0 2 3
 //                 11000110001011  =  12683 (dec)
 //
 // The leading bits are always 0.
 //
 // --- WARNING: This works only up to 15 levels.
 //              (we probably never need more than 7)
 //
 
 uint64 SpatialIndex::idByName(const char *name)
 {
     uint64 out  = 0, i;
     uint32 size = 0;
 
     if (name == nullptr) // null pointer-name
         throw SpatialFailure("SpatialIndex:idByName:no name given");
     if (name[0] != 'N' && name[0] != 'S') // invalid name
         throw SpatialFailure("SpatialIndex:idByName:invalid name", name);
 
     size = strlen(name); // determine string length
     // at least size-2 required, don't exceed max
     if (size < 2)
         throw SpatialFailure("SpatialIndex:idByName:invalid name - too short ", name);
     if (size > HTMNAMEMAX)
         throw SpatialFailure("SpatialIndex:idByName:invalid name - too long ", name);
 
     for (i = size - 1; i > 0; i--) // set bits starting from the end
     {
         if (name[i] > '3' || name[i] < '0') // invalid name
             throw SpatialFailure("SpatialIndex:idByName:invalid name digit ", name);
         out += (uint64(name[i] - '0') << 2 * (size - i - 1));
     }
 
     i = 2; // set first pair of bits, first bit always set
     if (name[0] == 'N')
         i++; // for north set second bit too
     out += (i << (2 * size - 2));
 
     /************************
     // This code may be used later for hashing !
     if(size==2)out -= 8;
     else {
       size -= 2;
       uint32 offset = 0, level4 = 8;
       for(i = size; i > 0; i--) { // calculate 4 ^ (level-1), level = size-2
         offset += level4;
         level4 *= 4;
       }
       out -= level4 - offset;
     }
     **************************/
     return out;
 }
 
 //////////////////NAMEBYID/////////////////////////////////////////////////
 // Translate uint32 to an ascii leaf name
 //
 // The encoding described above may be decoded again using the following
 // procedure:
 //
 //  * Traverse the uint32 from left to right.
 //  * Find the first 'true' bit.
 //  * The first pair gives N (11) or S (10).
 //  * The subsequent bit-pairs give the numbers 0-3.
 //
 
 char *SpatialIndex::nameById(uint64 id, char *name)
 {
     uint32 size = 0, i;
 #ifdef _WIN32
     uint64 IDHIGHBIT  = 1;
     uint64 IDHIGHBIT2 = 1;
     IDHIGHBIT         = IDHIGHBIT << 63;
     IDHIGHBIT2        = IDHIGHBIT2 << 62;
 #endif
 
     /*************
     // This code might be useful for hashing later !!
 
     // calculate the level (i.e. 8*4^level) and add it to the id:
     uint32 level=0, level4=8, offset=8;
     while(id >= offset) {
       if(++level > 13) { ok = false; offset = 0; break; }// level too deep
       level4 *= 4;
       offset += level4;
     }
     id += 2 * level4 - offset;
     **************/
 
     // determine index of first set bit
     for (i = 0; i < IDSIZE; i += 2)
     {
         if ((id << i) & IDHIGHBIT)
             break;
         if ((id << i) & IDHIGHBIT2) // invalid id
             throw SpatialFailure("SpatialIndex:nameById: invalid ID");
     }
     if (id == 0)
         throw SpatialFailure("SpatialIndex:nameById: invalid ID");
 
     size = (IDSIZE - i) >> 1;
     // allocate characters
     if (!name)
         name = new char[size + 1];
 
     // fill characters starting with the last one
     for (i = 0; i < size - 1; i++)
         name[size - i - 1] = '0' + char((id >> i * 2) & 3);
 
     // put in first character
     if ((id >> (size * 2 - 2)) & 1)
     {
         name[0] = 'N';
     }
     else
     {
         name[0] = 'S';
     }
     name[size] = 0; // end string
 
     return name;
 }
 //////////////////POINTBYID////////////////////////////////////////////////
 // Find a vector for the leaf node given by its ID
 //
 void SpatialIndex::pointById(SpatialVector &vec, uint64 ID) const
 {
     //  uint64 index;
     float64 center_x, center_y, center_z, sum;
     char name[HTMNAMEMAX];
 
     SpatialVector v0, v1, v2; //
 
     this->nodeVertex(ID, v0, v1, v2);
 
     nameById(ID, name);
     /*
         I started to go this way until I discovered nameByID...
         Some docs would be nice for this
 
         switch(name[1]){
         case '0':
             index = name[0] == 'S' ? 1 : 5;
             break;
         case '1':
             index = name[0] == 'S' ? 2 : 6;
             break;
         case '2':
             index = name[0] == 'S' ? 3 : 7;
             break;
         case '3':
             index = name[0] == 'S' ? 4 : 8;
             break;
         }
     */
     //    cerr << "---------- Point by id: " << name << Qt::endl;
     //  v0.show(); v1.show(); v2.show();
     center_x = v0.x_ + v1.x_ + v2.x_;
     center_y = v0.y_ + v1.y_ + v2.y_;
     center_z = v0.z_ + v1.z_ + v2.z_;
     sum      = center_x * center_x + center_y * center_y + center_z * center_z;
     sum      = sqrt(sum);
     center_x /= sum;
     center_y /= sum;
     center_z /= sum;
     vec.x_ = center_x;
     vec.y_ = center_y;
     vec.z_ = center_z; // I don't want it normalized or radec to be set,
     //  cerr << " - - - - " << Qt::endl;
     //  vec.show();
     //  cerr << "---------- Point by id Retuning" << Qt::endl;
 }
 //////////////////IDBYPOINT////////////////////////////////////////////////
 // Find a leaf node where a vector points to
 //
 
 uint64 SpatialIndex::idByPoint(const SpatialVector &v) const
 {
     uint64 index;
 
     // start with the 8 root triangles, find the one which v points to
     for (index = 1; index <= 8; index++)
     {
         if ((V(0) ^ V(1)) * v < -gEpsilon)
             continue;
         if ((V(1) ^ V(2)) * v < -gEpsilon)
             continue;
         if ((V(2) ^ V(0)) * v < -gEpsilon)
             continue;
         break;
     }
     // loop through matching child until leaves are reached
     while (ICHILD(0) != 0)
     {
         uint64 oldindex = index;
         for (size_t i = 0; i < 4; i++)
         {
             index = nodes_[oldindex].childID_[i];
             if ((V(0) ^ V(1)) * v < -gEpsilon)
                 continue;
             if ((V(1) ^ V(2)) * v < -gEpsilon)
                 continue;
             if ((V(2) ^ V(0)) * v < -gEpsilon)
                 continue;
             break;
         }
     }
     // return if we have reached maxlevel
     if (maxlevel_ == buildlevel_)
         return N(index).id_;
 
     // from now on, continue to build name dynamically.
     // until maxlevel_ levels depth, continue to append the
     // correct index, build the index on the fly.
     char name[HTMNAMEMAX];
     nameById(N(index).id_, name);
     size_t len       = strlen(name);
     SpatialVector v0 = V(0);
     SpatialVector v1 = V(1);
     SpatialVector v2 = V(2);
 
     size_t level = maxlevel_ - buildlevel_;
     while (level--)
     {
         SpatialVector w0 = v1 + v2;
         w0.normalize();
         SpatialVector w1 = v0 + v2;
         w1.normalize();
         SpatialVector w2 = v1 + v0;
         w2.normalize();
 
         if (isInside(v, v0, w2, w1))
         {
             name[len++] = '0';
             v1          = w2;
             v2          = w1;
             continue;
         }
         else if (isInside(v, v1, w0, w2))
         {
             name[len++] = '1';
             v0          = v1;
             v1          = w0;
             v2          = w2;
             continue;
         }
         else if (isInside(v, v2, w1, w0))
         {
             name[len++] = '2';
             v0          = v2;
             v1          = w1;
             v2          = w0;
             continue;
         }
         else if (isInside(v, w0, w1, w2))
         {
             name[len++] = '3';
             v0          = w0;
             v1          = w1;
             v2          = w2;
             continue;
         }
     }
     name[len] = '\0';
     return idByName(name);
 }
 
 //////////////////ISINSIDE/////////////////////////////////////////////////
 // Test whether a vector is inside a triangle. Input triangle has
 // to be sorted in a counter-clockwise direction.
 //
 bool SpatialIndex::isInside(const SpatialVector &v, const SpatialVector &v0, const SpatialVector &v1,
                             const SpatialVector &v2) const
 {
     if ((v0 ^ v1) * v < -gEpsilon)
         return false;
     if ((v1 ^ v2) * v < -gEpsilon)
         return false;
     if ((v2 ^ v0) * v < -gEpsilon)
         return false;
     return true;
 }