toys/nds/cpp/source/nds.cpp

#include "nds.h"

#include <cassert>
#include <cmath>
#include <algorithm>
#include <bitset>
#include <iostream>
#include <limits>
#include <vector>

using namespace std;

inline bool segmentIntersection(const PointD& p1, const PointD& p2, const PointD& q1,
                         const PointD& q2)
{
    if (max(p1.x, p2.x) < min(q1.x, q2.x) || max(q1.x, q2.x) < min(p1.x, p2.x)
        || max(p1.y, p2.y) < min(q1.y, q2.y) || max(q1.y, q2.y) < min(p1.y, p2.y))
        return false;

    double res1 = (p1.x - p2.x) * (q1.y - p2.y) - (p1.y - p2.y) * (q1.x - p2.x);
    double res2 = (p1.x - p2.x) * (q2.y - p2.y) - (p1.y - p2.y) * (q2.x - p2.x);
    double res3 = (q1.x - q2.x) * (p1.y - q2.y) - (q1.y - q2.y) * (p1.x - q2.x);
    double res4 = (q1.x - q2.x) * (p2.y - q2.y) - (q1.y - q2.y) * (p2.x - q2.x);

    return res1 * res2 <= 0 && res3 * res4 <= 0;
}

inline bool segmentIntersection(const PointD& startA, const PointD& endA,
                         const PointD& startB, const PointD& endB,
                         PointD& crossPoint)
{
    /* // xsub * y + ysub * x + inter = 0 // */
    double start1[2] = {startA.x / 100000000.0, startA.y / 100000000.0};
    double end1[2] = {endA.x / 100000000.0, endA.y / 100000000.0};
    double start2[2] = {startB.x / 100000000.0, startB.y / 100000000.0};
    double end2[2] = {endB.x / 100000000.0, endB.y / 100000000.0};
    // calculate A、B、C.
    double xsub1 = start1[0] - end1[0];
    double ysub1 = end1[1] - start1[1];
    double xsub2 = start2[0] - end2[0];
    double ysub2 = end2[1] - start2[1];
    double inter1 = end1[0] * start1[1] - start1[0] * end1[1];
    double inter2 = end2[0] * start2[1] - start2[0] * end2[1];

    double ans[2];
    auto convertCrossPoint = [&ans](PointD& crossPos) {
        crossPos.x = ans[0] * 100000000.0;
        crossPos.y = ans[1] * 100000000.0;
    };

    if (xsub1 * ysub2 == xsub2 * ysub1) {
        if (inter1 != inter2) {
            return false;
        }

        if ((end1[0] >= start2[0] && start1[0] < start2[0])
            || (end1[0] > start2[0] && start1[0] <= start2[0])) {
            ans[0] = (double)start2[0];
            ans[1] = (double)start2[1];
            convertCrossPoint(crossPoint);
            return true;
        }
        if ((end1[0] <= start2[0] && start1[0] > start2[0])
            || (end1[0] < start2[0] && start1[0] >= start2[0])) {
            ans[0] = end1[0];
            ans[1] = end1[1];
            convertCrossPoint(crossPoint);
            return true;
        }
        if ((start1[0] == start2[0] && start1[1] == start2[1])
            || (start1[0] == end2[0] && start1[1] == end2[1])) {
            ans[0] = (double)start1[0];
            ans[1] = (double)start1[1];
            convertCrossPoint(crossPoint);
            return true;
        }
        if ((start2[0] == end1[0] && start2[1] == end1[1])
            || (end1[0] == end2[0] && end1[1] == end2[1])) {
            ans[0] = (double)end1[0];
            ans[1] = (double)end1[1];
            convertCrossPoint(crossPoint);
            return true;
        }
        return false;
    }

    double x = (double)((inter2 * xsub1 - inter1 * xsub2) / (xsub2 * ysub1 - xsub1 * ysub2));
    double y = (double)((inter1 * ysub2 - inter2 * ysub1) / (xsub2 * ysub1 - xsub1 * ysub2));
    ans[0] = x;
    ans[1] = y;

    if (((x <= start1[0] && x >= end1[0]) || (x >= start1[0] && x <= end1[0]))
        && ((x <= start2[0] && x >= end2[0]) || (x >= start2[0] && x <= end2[0]))) {
        convertCrossPoint(crossPoint);
        return true;
    }
    return false;
}

inline bool segmentIntersectionWithRect(const PointD& p1, const PointD& p2,
                                 const RectD& rct)
{
    if (rct.contain(p1) || rct.contain(p2)) return true;

    PointD q1(rct.minX, rct.minY);
    PointD q2(rct.minX, rct.maxY);
    PointD q3(rct.maxX, rct.maxY);
    PointD q4(rct.maxX, rct.minY);

    return segmentIntersection(p1, p2, q1, q2) || segmentIntersection(p1, p2, q2, q3)
           || segmentIntersection(p1, p2, q3, q4) || segmentIntersection(p1, p2, q4, q1);
}

namespace nds {

namespace {
using MortonCode = std::int64_t;

//各个level的grid index mask,获取的时候直接 gridIndexMask[level]
constexpr std::int32_t mashIdMask[] = {0b1,
                                       0b111,
                                       0b11111,
                                       0b1111111,
                                       0b111111111,
                                       0b11111111111,
                                       0b1111111111111,
                                       0b111111111111111,
                                       0b11111111111111111,
                                       0b1111111111111111111,
                                       0b111111111111111111111,
                                       0b11111111111111111111111,
                                       0b1111111111111111111111111,
                                       0b111111111111111111111111111,
                                       0b11111111111111111111111111111,
                                       0b1111111111111111111111111111111};

// nds point各个level的X/Y掩码
constexpr std::int32_t gridXMask[] = {0b1,
                                      0b11,
                                      0b111,
                                      0b1111,
                                      0b11111,
                                      0b111111,
                                      0b1111111,
                                      0b11111111,
                                      0b111111111,
                                      0b1111111111,
                                      0b11111111111,
                                      0b111111111111,
                                      0b1111111111111,
                                      0b11111111111111,
                                      0b111111111111111,
                                      0b1111111111111111};

constexpr std::int32_t gridYMask[] = {0b0,
                                      0b1,
                                      0b11,
                                      0b111,
                                      0b1111,
                                      0b11111,
                                      0b111111,
                                      0b1111111,
                                      0b11111111,
                                      0b111111111,
                                      0b1111111111,
                                      0b11111111111,
                                      0b111111111111,
                                      0b1111111111111,
                                      0b11111111111111,
                                      0b111111111111111};

/// judge (lon, lat) is valid or not
/// \param lon [-180, 180)
/// \param lat [-90, 90)
/// \return
bool isValidLonLat(double lon, double lat)
{
    return lon <= MAX_LON && lon >= MIN_LON && lat <= MAX_LAT && lat >= MIN_LAT;
}

bool isValidLevel(uint level)
{
    return level >= 0 && level <= 15;
}

LevelId getLevelId(uint level)
{
    return LevelId(1 << (16 + level));
}

MortonCode ndsPoint2Mortoncode(const NdsPoint& ndsPoint)
{
    // highest bit of X to 2th highest bit of mortoncode
    MortonCode mortoncode = (((uint64_t)std::get<0>(ndsPoint) >> 31) & 1) << (31 * 2);
    for (int i = 30; i >= 0; i--) {
        int64_t xx = ((uint64_t)(std::get<0>(ndsPoint) >> i) & 1) << (i * 2);
        int64_t yy = ((uint64_t)(std::get<1>(ndsPoint) >> i) & 1) << (i * 2 + 1);
        mortoncode = mortoncode | xx | yy;
    }
    return mortoncode;
}

NdsPoint mortoncode2NdsPoint(MortonCode mortoncode)
{
    int32_t x = 0;
    int32_t y = 0;
    for (uint i = 0; i < 32; i++) {
        x = x | ((mortoncode >> (i * 2)) & 1) << i;
        y = y | ((mortoncode >> (i * 2 + 1)) & 1) << i;
    }
    y = y | ((y >> 30) << 31); // 将y最高位补齐

    return std::make_tuple(x, y);
}

MortonCode lonLat2Mortoncode(double lon, double lat)
{
    return ndsPoint2Mortoncode(lonLat2NdsPoint(lon, lat));
}

std::tuple<double, double> mortoncode2LonLat(MortonCode mortoncode)
{
    return ndsPoint2lonLat(mortoncode2NdsPoint(mortoncode));
}

MeshId mortoncode2MeshId(MortonCode morton, uint level)
{
    return MeshId(morton >> (63 - (level * 2 + 1)));
}

uint getGridLevel(NdsGridId gridId)
{
    if (gridId == NDS_INVALID_ID) {
        return NDS_INVALID_ID;
    }
    for (uint level = NDS_LEVEL_COUNT - 1; level >= 0; level--) {
        LevelId lid = getLevelId(level);
        if (gridId & lid)
            return level;
    }
    return NDS_INVALID_ID;
}

// grid 左下角坐标
MortonCode getGridConnerMortoncode(const NdsGridId gridId)
{
    std::tuple<MeshId, std::int32_t> ml = gridId2MeshIdAndLevel(gridId);

    MeshId meshId = std::get<0>(ml);
    uint level = std::get<1>(ml);
    // mesh id to morton code
    MortonCode morton = (MortonCode)meshId << (63 - (2 * level + 1));
    return morton;
}

std::tuple<double, double> getGridConnerPoint(const NdsGridId gridId)
{
    return mortoncode2LonLat(getGridConnerMortoncode(gridId));
}

} // namespace

/**
 * @brief get the width of the grid at the specified level in latitude and
 * longitude coordinate, unit: degree
 *
 * @param level
 * @return grid width in degree
 */
double getGridSize(uint level)
{
    return ((double)180.0 / (1 << level));
}

NdsPoint lonLat2NdsPoint(double lon, double lat)
{
    double x = lon / NDS_PRECISION;
    double y = lat / NDS_PRECISION;

    return std::make_tuple(int32_t(floor(x)), int32_t(floor(y)));
}

std::tuple<double, double> ndsPoint2lonLat(const NdsPoint& ndsPoint)
{
    double x = std::get<0>(ndsPoint) * NDS_PRECISION;
    double y = std::get<1>(ndsPoint) * NDS_PRECISION;

    return std::make_tuple(x, y);
}

MeshId getMeshId(double lon, double lat, uint level)
{
    if (!isValidLevel(level)) {
        return NDS_INVALID_ID;
    }
    MortonCode morton = lonLat2Mortoncode(lon, lat);
    return mortoncode2MeshId(morton, level);
}

MeshId getMeshId(const NdsPoint& ndsPoint, uint level)
{
    if (!isValidLevel(level)) {
        return NDS_INVALID_ID;
    }
    MortonCode mortoncode = ndsPoint2Mortoncode(ndsPoint);
    return mortoncode2MeshId(mortoncode, level);
}

std::vector<MeshId> getSegmentMeshIds(const PointD& p1, const PointD& p2, uint level)
{
    std::vector<MeshId> ids;

    if (p1 == p2) {
        ids.emplace_back(getMeshId(p1.x, p1.y, level));
    } else {
        MeshId id1 = getMeshId(p1.x, p1.y, level);
        MeshId id2 = getMeshId(p2.x, p2.y, level);
        if (id1 == id2) {
            ids.emplace_back(id1);
        } else {
            RectD rct(std::min(p1.x, p2.x), std::max(p1.x, p2.x), std::min(p1.y, p2.y),
                             std::max(p1.y, p2.y));
            std::vector<MeshId> tmpIds = getMeshIdInRect(rct, level);
            for (auto& id : tmpIds) {
                RectD _rct = getMeshRect(id, level);
                if (segmentIntersectionWithRect(p1, p2, _rct)) {
                    ids.emplace_back(id);
                }
            }
        }
    }

    return ids;
}

NdsGridId getGridId(MeshId meshId, uint level)
{
    return (NdsGridId)(getLevelId(level) | meshId);
}

NdsGridId getGridId(double lon, double lat, uint level)
{
    if (!isValidLevel(level)) {
        return NDS_INVALID_ID;
    }
    MeshId meshId = getMeshId(lon, lat, level);
    return getGridId(meshId, level);
}

NdsGridId getGridId(const NdsPoint& ndsPt, uint level)
{
    if (!isValidLevel(level)) {
        return NDS_INVALID_ID;
    }
    MeshId meshId = getMeshId(ndsPt, level);
    return getGridId(meshId, level);
}

std::tuple<MeshId, std::int32_t> gridId2MeshIdAndLevel(NdsGridId gridId)
{
    uint level = getGridLevel(gridId);
    if (level != NDS_INVALID_ID) {
        MeshId meshId = gridId & mashIdMask[level];
        return std::make_tuple(meshId, level);
    }

    return std::make_tuple(NDS_INVALID_ID, NDS_INVALID_ID);
}

RectD getMeshRect(MeshId meshId, uint level)
{
    NdsGridId gridId = getGridId(meshId, level);
    return getGridRect(gridId);
}

PointD getMeshCenter(MeshId meshId, uint level)
{
    PointD p;
    RectD rct = getMeshRect(meshId, level);
    if (!rct.isInvalid()) {
        p.x = (rct.maxX + rct.minX) / 2;
        p.y = (rct.maxY + rct.minY) / 2;
        p.z = (rct.maxZ + rct.minZ) / 2;
    }
    return p;
}

/// 给定NdsGridId,获取对应网格的l/t/r/b点经纬度
RectD getGridRect(NdsGridId gridId)
{
    std::tuple<MeshId, std::int32_t> ml = gridId2MeshIdAndLevel(gridId);
    MeshId meshId = std::get<0>(ml);
    uint level = std::get<1>(ml);

    // 0级别morton code单独处理
    if (level == 0) {
        if (meshId == 0) { // 东半球
            return RectD(0.0, 180.0, -90.0, 90.0);
            // return std::make_tuple(0, -90, 180, 90);
        } else if (meshId == 1) { // 西半球
            return RectD(-180.0, 0.0, -90.0, 90.0);
            // return std::make_tuple(-180, -90, 0, 90);
        }
    }

    double w = getGridSize(level);
    std::tuple<double, double> lb = getGridConnerPoint(gridId);
    return RectD(std::get<0>(lb), std::get<0>(lb) + w, std::get<1>(lb), std::get<1>(lb) + w);
}

RectD getGridsRect(std::vector<NdsGridId> gridIds)
{
    double l = std::numeric_limits<double>::max();
    double t = -std::numeric_limits<double>::max();
    double r = -std::numeric_limits<double>::max();
    double b = std::numeric_limits<double>::max();

    RectD rct_new; // add by zj
    for (auto id : gridIds) {
        RectD rct = getGridRect(id);
        l = std::min(l, rct.minX);
        t = std::max(t, rct.maxY);
        r = std::max(r, rct.maxX);
        b = std::min(b, rct.minY);

        // l = std::min(l, std::get<0>(rct));
        // t = std::max(t, std::get<1>(rct));
        // r = std::max(r, std::get<2>(rct));
        // b = std::min(b, std::get<3>(rct));
    }
    rct_new.minX = l;
    rct_new.maxY = t;
    rct_new.maxX = r;
    rct_new.minY = b;
    return rct_new;
    // return {l, t, r, b};
}

RectD getMeshsRect(std::vector<MeshId> meshIds, uint level)
{
    std::vector<NdsGridId> gridIds;
    for (auto mId : meshIds) {
        gridIds.push_back(getGridId(mId, level));
    }

    return getGridsRect(gridIds);
}

std::vector<MeshId> getMeshIdInRect(const RectD& rct, uint level)
{
    std::vector<MeshId> ids;
    double l = std::max(MIN_LON, rct.minX);
    double b = std::max(MIN_LAT, rct.minY);
    double t = std::min(MAX_LAT, rct.maxY);
    double r = std::min(MAX_LON, rct.maxX);

    if (b > t || l > r) {
        return ids;
    }

    MeshId minMeshId = getMeshId(l, b, level);
    MeshId maxMeshId = getMeshId(r, t, level);
    // std::cout << "minMeshId=\t" << std::bitset<32>(minMeshId) << std::endl;
    // std::cout << "maxMeshId=\t" << std::bitset<32>(maxMeshId) << std::endl;

    // 当前level级别的NDS point,直接xy加减就是周边瓦片的nds grid id
    NdsPoint minXY = mortoncode2NdsPoint(minMeshId);
    NdsPoint maxXY = mortoncode2NdsPoint(maxMeshId);
    // std::cout << "minXY:x=\t" << std::bitset<32>(std::get<0>(minXY)) << std::endl;
    // std::cout << "minXY:y=\t" << std::bitset<32>(std::get<1>(minXY)) << std::endl;
    // std::cout << "maxXY:x=\t" << std::bitset<32>(std::get<0>(maxXY)) << std::endl;
    // std::cout << "maxXY:y=\t" << std::bitset<32>(std::get<1>(maxXY)) << std::endl;

    int minX = INT_N_2_INT32(std::get<0>(minXY), level + 1); // x的bit个数是level+1
    int maxX = INT_N_2_INT32(std::get<0>(maxXY), level + 1);
    int minY = INT_N_2_INT32(std::get<1>(minXY), level); // y的bit个数是level
    int maxY = INT_N_2_INT32(std::get<1>(maxXY), level);
    // std::cout << "minX=\t\t" << std::bitset<32>(minX) << std::endl;
    // std::cout << "maxX=\t\t" << std::bitset<32>(maxX) << std::endl;
    // std::cout << "minY=\t\t" << std::bitset<32>(minY) << std::endl;
    // std::cout << "maxY=\t\t" << std::bitset<32>(maxY) << std::endl;

    for (int j = maxY; j >= minY; j--) {
        for (int i = minX; i <= maxX; ++i) {
            // std::cout << "i=\t\t" << std::bitset<32>(i) << std::endl;
            // std::cout << "j=\t\t" << std::bitset<32>(j) << std::endl;
            int lon = i & gridXMask[level];
            int lat = j & gridYMask[level];
            // std::cout << "lon=\t\t" << std::bitset<32>(i) << std::endl;
            // std::cout << "lat=\t\t" << std::bitset<32>(j) << std::endl;
            MortonCode m = ndsPoint2Mortoncode(std::make_tuple(lon, lat));
            // std::cout << "mortonCode=\t" << std::bitset<32>(m) << std::endl;
            ids.push_back(static_cast<std::uint32_t>(m));
        }
    }

    return ids;
}

/**
 *  获取和指定经纬度相交的gridid
 */
std::vector<NdsGridId> getGridIdInRect(const RectD& rct, uint level)
{
    std::vector<MeshId> meshIds = getMeshIdInRect(rct, level);

    LevelId levelId = getLevelId(level);
    std::vector<NdsGridId> ids;

    for (auto m : meshIds) {
        ids.push_back(NdsGridId(levelId | m));
    }
    return ids;
}

std::vector<MeshId> getAdjacentMeshIds(MeshId id, uint level, uint32_t n)
{
    std::vector<MeshId> meshIds;

    if (id != NDS_INVALID_ID && level != 0 && level != 1 && n != 0) {
        NdsPoint point = mortoncode2NdsPoint(id);
        int32_t x = std::get<0>(point);
        int32_t y = std::get<1>(point);

        // 只保留对应的有效位
        int32_t minX = (x - n) & gridXMask[level];
        int32_t maxX = (x + n) & gridXMask[level];
        int32_t minY = (y - n) & gridYMask[level];
        int32_t maxY = (y + n) & gridYMask[level];

        for (int j = maxY; j >= minY; j--) {
            for (int i = minX; i <= maxX; ++i) {
                MortonCode m = ndsPoint2Mortoncode(std::make_tuple(i, j));
                meshIds.push_back(static_cast<std::uint32_t>(m));
            }
        }
    }

    return meshIds;
}

std::vector<MeshId> get8AroundMeshId(MeshId meshId, uint level)
{
    std::vector<MeshId> aroundMeshIds;
    aroundMeshIds.assign(8, NDS_INVALID_ID);

    if (meshId != NDS_INVALID_ID && level != 0 && level != 1) {
        // mesh id即当前level的前(2*level+1)bit莫顿码
        // 拆分出来的NDSPoint也是当前瓦片的在全部瓦片中的(x,y)位置
        // x,y 加减即可获取周边瓦片位置
        NdsPoint point = mortoncode2NdsPoint(meshId);
        int32_t x = std::get<0>(point);
        int32_t y = std::get<1>(point);
        // cout << "NDS point x:" << x << "  Y: " << y << endl;

        // 只保留对应的有效位
        int32_t lx = (x - 1) & gridXMask[level];
        int32_t rx = (x + 1) & gridXMask[level];
        int32_t ty = (y + 1) & gridYMask[level];
        int32_t by = (y - 1) & gridYMask[level];
        // cout << "lx: " << lx << " rx: " << rx << " ty: " << ty << " bx: " << by << endl;

        //左右上下组装新的morton code
        aroundMeshIds[0] = static_cast<std::uint32_t>(ndsPoint2Mortoncode({lx, ty}));
        aroundMeshIds[1] = static_cast<std::uint32_t>(ndsPoint2Mortoncode({x, ty}));
        aroundMeshIds[2] = static_cast<std::uint32_t>(ndsPoint2Mortoncode({rx, ty}));
        aroundMeshIds[3] = static_cast<std::uint32_t>(ndsPoint2Mortoncode({rx, y}));
        aroundMeshIds[4] = static_cast<std::uint32_t>(ndsPoint2Mortoncode({rx, by}));
        aroundMeshIds[5] = static_cast<std::uint32_t>(ndsPoint2Mortoncode({x, by}));
        aroundMeshIds[6] = static_cast<std::uint32_t>(ndsPoint2Mortoncode({lx, by}));
        aroundMeshIds[7] = static_cast<std::uint32_t>(ndsPoint2Mortoncode({lx, y}));
        // std::uint32_t std::int64_t
        //边界检查,越界的更新为无效值,只对level>=2的层级管用
        int32_t signMaskX = 1 << level;       // 西半球X最高位为1,东半球为0
        int32_t signMaskY = 1 << (level - 1); // 南半球最高位为1,北半球为0

        // x最高位为1其余为0为左边界
        if (((x & gridXMask[level]) == signMaskX)) {
            //[左边界0, 6, 7网格无效]
            aroundMeshIds[0] = NDS_INVALID_ID;
            aroundMeshIds[6] = NDS_INVALID_ID;
            aroundMeshIds[7] = NDS_INVALID_ID;
        }
        // x最高位为0其余为1为右边界
        else if ((x & gridXMask[level]) == (signMaskX - 1)) {
            //[右边界2,3,4无效]
            aroundMeshIds[2] = NDS_INVALID_ID;
            aroundMeshIds[3] = NDS_INVALID_ID;
            aroundMeshIds[4] = NDS_INVALID_ID;
        }

        // Y最高位1其余为0为下边界
        if ((y & gridYMask[level]) == signMaskY) {
            //[下边界4,5,6无效]
            aroundMeshIds[4] = NDS_INVALID_ID;
            aroundMeshIds[5] = NDS_INVALID_ID;
            aroundMeshIds[6] = NDS_INVALID_ID;
        }
        // Y最高位为0其余为1为上边界
        else if ((y & gridYMask[level]) == (signMaskY - 1)) {
            //[上边界0,1,2无效]
            aroundMeshIds[0] = NDS_INVALID_ID;
            aroundMeshIds[1] = NDS_INVALID_ID;
            aroundMeshIds[2] = NDS_INVALID_ID;
        }
    }

    assert(aroundMeshIds.size() == 8);
    return aroundMeshIds;
}

/**
 * @brief /// 给定gridid,获取其周边的8联通瓦片的id
 * note : level=0/1不关心
 * id顺序如下:
 *      0       1       2
 *      7     gridid    3
 *      6       5       4
 *
 * @param gridId
 * @return std::vector<NdsGridId>
 */
std::vector<NdsGridId> get8AroundGridId(NdsGridId gridId)
{
    std::tuple<MeshId, std::int32_t> lm = gridId2MeshIdAndLevel(gridId);
    MeshId meshId = std::get<0>(lm);
    uint level = std::get<1>(lm);
    LevelId levelId = getLevelId(level);
    cout << "meshid: " << meshId << "  level ID: " << levelId << endl;

    std::vector<NdsGridId> aroundGridIds;
    aroundGridIds.assign(8, NDS_INVALID_ID);

    if (gridId != NDS_INVALID_ID && level != 0 && level != 1) {
        std::vector<MeshId> meshIds = get8AroundMeshId(meshId, level);
        assert(meshIds.size() == 8);

        for (size_t i = 0; i < meshIds.size(); i++) {
            if (meshIds[i] != NDS_INVALID_ID) {
                aroundGridIds[i] = (levelId | meshIds[i]);
            }
        }
    }

    assert(aroundGridIds.size() == 8);
    return aroundGridIds;
}

} // namespace nds