spatial.h

Go to the documentation of this file.

/******************************************************************************
** Copyright (c) 2020 MAK Technologies, Inc.
** All rights reserved.
******************************************************************************/

#pragma once


#include <unordered_map>
#include <queue>

#include "makUtil/dataTypes.h"
#include "makUtil/array.h"
#include "makUtil/vector.h"

#include <cmath>
#include <immintrin.h>
#include <tbb/spin_mutex.h>

namespace MAK
{
   template<typename DATA_ID>
   class SpatialIndex
   {
      static const int BITS_PER_COMPONENT = 30;
      static const int BITS_PER_COMPONENT_AND_REGION = 31;

   public:
      SpatialIndex();
      ~SpatialIndex();

      typedef unsigned int  NodeIndex;
      typedef unsigned int  PageIndex;

      Bool insert(const DATA_ID globalId, const Vector3d& pos);
      Bool insert(const DATA_ID globalId, const Vector3d& pos, const UInt64 key);
      Bool remove(const DATA_ID globalId);
      Bool update(const DATA_ID globalId, const Vector3d& pos);

      std::vector<DATA_ID> nearestNeighbors(const Vector3d& loc, double distanceInMeter) const;
      std::vector<DATA_ID> objectsWithin(const Vector3d& min, const Vector3d& max) const;
      std::vector<DATA_ID> objectsWithin(const Vector2d& minLatLonDeg, const Vector2d& maxLatLonDeg) const;
      std::vector<DATA_ID> objectsNearChord(const Vector3d& pointA, const Vector3d& pointB, const double delta) const;
      std::vector<DATA_ID> spatialTreeWalk(UInt32 &totalDataCnt) const;
      bool isDistanceToLineWithin(const Vector3d &pointC, double distance, const Vector3d &lineA, const Vector3d &lineB) const;

      bool isWholeGlobeRegion(const Vector3d &pos1, const Vector3d &pos2) const;

      void reset();


      void endFrame();

   public:

      static  Vector2d geocToSpherical(const Vector3d& pos)
      {
         double radius = pos.length();
         Vector2d ret;
         ret.x = atan2(pos.y, pos.x);  //azimuth.  atan2 returns -pi to pi
         ret.y = radius ? acos(pos.z / radius) : 0; //elevation.  returns between 0..pi

         ret.x = (ret.x + 3.1415926535897932);
         ret.y = ret.y * 2;

         return ret;
      }

      /*
      bit scattering done so that we can interleave the two values so that we can take 2 bits at time and use them to determine which child to go into in the quadtree

      The earth is split into 4 regions. A region goes from the south to the north pole and contains a 90 degree section around the equator.
      Positions are converted to polar coordinates and are quantized into 30 bit numbers.
      Then the positions are converted into bit keys. The bit key is 64 bit longs.  The top 2 significant bits are not used (maybe flags later)

      The next two bits (62,61) are the region that contains the quadtree.
      The remaining 60 bits are a Morton code that determine the children to go into in the quadtree.

      As the azimuth values increase, the x bit increases, as the elevation increases the y bit increases.
      More bits are required the deeper you move into the tree.

      With 30 bits, on the equator, we get about 3cm precision.  This is close enough for determining if entities are close.

      Key layout:
      |xx|rr|ea|ea|ea|ea|ea|ea|ea|ea|ea|ea|ea|ea|ea|ea|ea|ea|ea|ea|ea|ea|ea|ea|ea|ea|ea|ea|ea|ea|ea|ea|

      x = not used
      r = region (interpreted as 0..3)
      e = elevation for that level
      a = azimuth for that level
      deepest level (30) in LSBs



      Bit encoding used the quadtree:
      |-------------------------|
      | 1010 | 1011|            |
      |-----10-----|     11     |
      | 1000 | 1001|            |
      |-------------------------|
      |            |            |
      |    00      |     01     |
      |            |            |
      |-------------------------|
      */
      static UInt64 my_pdep_u64(UInt64 val, UInt64 mask) {
        UInt64 res = 0;
        for (UInt64 bb = 1; mask; bb += bb) {
          if (val & bb)
            res |= mask & -mask;
          mask &= mask - 1;
        }
        return res;
      }

      static UInt64 geocentricToSpatialKey(const Vector3d& pos)
      {
         //convert geocentric position to a spherical coordinate, normalize values to [0 - 2pi]
         Vector2d spos = geocToSpherical(pos);

         //we're going to use 30 bits in our Morton code.  Gives us about 3cm regions

         static const UInt64 quantization = 1ULL << BITS_PER_COMPONENT;
         static const UInt64 bitmask = (1ULL << (BITS_PER_COMPONENT * 2)) - 1; //mask for bottom 60 bits

         //we're going to use 30 bits (about 3cm resolution at earth radius) in our Morton code, so quantize the spherical coordinates to that
         static double qauntizationFactor = quantization / 6.283185307179586;  

         //get quantized values
         UInt64 azimuth = spos.x * qauntizationFactor;
         UInt64 elevation = spos.y * qauntizationFactor;

         UInt64 region = azimuth / (quantization / 4); //which of the 4 world wide diamonds is it in, integer division should work

         //scatter the bits 
         azimuth = my_pdep_u64(azimuth, 0x5555555555555555); //scatter azimuth (x value) to the odd bits so the x value changes the least significant bit in a tuple
         elevation = my_pdep_u64(elevation, 0xaaaaaaaaaaaaaaaa); //scatter elevation (y value) the even bits so the y value changes the most significant bit in a tuple

         //merge the values together
         UInt64 ret = region << (BITS_PER_COMPONENT * 2);  
         ret |= azimuth;
         ret |= elevation;

         return ret;
      }

    protected:
      struct Page;
      struct Node;

      void growNodes(int count);
      void growPages(int count);

      int  findNodeToAdd(const UInt64 key);
      Bool needSplit(const NodeIndex nodeIdx) const;
      Bool canSplit(const NodeIndex nodeIdx) const;
      Bool splitNode(NodeIndex n);
      int  nextNode();
      int  nextPage();
      NodeIndex findNodeContaining(NodeIndex, const UInt64 key) const;
      NodeIndex findCommonRootNode(NodeIndex idx, const UInt64 key1, const UInt64 key2) const;

      bool generateMinMaxKeys(const Vector3d& pos1, const Vector3d& pos2, UInt64 &minKey, UInt64 &maxKey, Vector3d &minPos, Vector3d &maxPos) const;
      bool generateMinMaxKeys(const Vector3d& pos1, const Vector3d& pos2, UInt64 &minKey, UInt64 &maxKey) const;

      UInt32 objectsWithinNode(const NodeIndex idx, const UInt64 minKey, const UInt64 maxKey,
                                    const Vector3d& absMin, const Vector3d& absMax, std::vector<DATA_ID> &result) const;
      UInt32 objectsWithinNode(const NodeIndex idx, const UInt64 minKey, const UInt64 maxKey,
                                    const Vector3d& pos1, const Vector3d& pos2, const double distance,
                                    std::vector<DATA_ID> &result) const;
      UInt32 objectsWithinNode(const NodeIndex idx, const Vector2d& latLong1, const Vector2d& latLong2, std::vector<DATA_ID> &result) const;

      void eraseFromPage(Page& p, DATA_ID globalId);
      void addToPage(Page& p, DATA_ID globalId, UInt64 key, const Vector3d& loc);

      bool contains(UInt64 key, NodeIndex n);

      bool isPositionWithin(const Vector3d &min, const Vector3d &max, const Vector3d &pos) const;
      bool isPositionWithin(const Vector2d &latLong1, const Vector2d &latLong2, const Vector3d &pos) const;

      static const UInt64 MASK_AZIMUTH_ODD_BITS = (0x5555555555555555ULL);
      static const UInt64 MASK_ELEVATION_EVEN_BITS = (0xAAAAAAAAAAAAAAAAULL);
      static const UInt64 MASK_AZIMUTH_OBITS_REVERSE = ~(0x5555555555555555ULL);
      static const UInt64 MASK_ELEVATION_EBITS_REVERSE = ~(0xAAAAAAAAAAAAAAAAULL);

      static const int SpatialMaxNodeDepth = 31;
      struct Node
      {
         UInt64 key = 0;
         UInt32 children[4] = {0, 0, 0, 0};
         UInt32 depth = 0;
         UInt32 pageIdx = 0;
      };

      struct Entry
      {
         UInt64   key = 0;
         DATA_ID  globalId = DATA_ID();
         Vector3d location;
      };

      static const int thePageSize = 100;

      struct Page
      {
         int count = 0;
         std::vector<Entry> data;
         std::shared_ptr<tbb::spin_mutex> dataMutex;
         typedef tbb::spin_mutex::scoped_lock DataMutexLock;

         Page()
         {
            dataMutex = std::shared_ptr<tbb::spin_mutex>(new tbb::spin_mutex);
            data.resize(thePageSize);
         }

         Page(const Page& orig) : data(orig.data), count(orig.count), dataMutex(orig.dataMutex) {};
      };

      UInt32 myNodeCount;
      UInt32 myPageCount;
      std::queue<int> myFreePageQueue;
      std::vector<Node> myNodes;
      std::vector<Page> myPages;
      std::mutex myPageMutex;
      std::mutex myNodeMutex;
      //we expect this to be a fairly large data structure of every entity (possibly millions)
      //with an index to a node
      std::unordered_map<DATA_ID, int> myReverseLookup;
   };
}

#include "spatial.inl"