SpatialIndex.cpp | searchcode

/kstars/htmesh/SpatialIndex.cpp

https://gitlab.com/g10h4ck/kstars
C++ | 577 lines | 340 code | 66 blank | 171 comment | 58 complexity | 074ad94406067a01af85def097bed004 MD5 | raw file
Possible License(s): GPL-2.0, CC-BY-SA-3.0

//#     Filename:       SpatialIndex.cpp
//#
//#     The SpatialIndex class is defined here.
//#
//#     Author:         Peter Z. Kunszt based on A. Szalay's code
//#     
//#     Date:           October 15, 1998
//#
//#		Copyright (C) 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 therough 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 == 0)              // 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 << 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 nomralized or radec to be set, 
//	cerr << " - - - - " << endl;
//	vec.show();
//	cerr << "---------- Point by id Retuning" << 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;
}