integration/luprex/core/cpp/planemap.cpp

#include <cmath>
#include <algorithm>
#include "luastack.hpp"
#include "util.hpp"
#include "planemap.hpp"

// Cell X, Y coordinates are packed such that they have 24 bits for X and Y.
// A cell is 10 Meters square.
// Cell ID zero is used to represent an invalid position.
//
#define CELL_LIMIT 0x7FFFFF
#define CELL_SCALE 10.0

// Round a float and return as integer.  Clamp result to the specified range.
static int round_and_clamp(float x, int lo, int hi) {
    x = round(x);
    if (x < lo) return lo;
    if (x > hi) return hi;
    return int(x);
}

// A cell range is inclusive.
struct CellRange {
    int xlo;
    int ylo;
    int xhi;
    int yhi;

    bool equal(int xl, int yl, int xh, int yh) {
        return ((xlo==xl)&&(ylo==yl)&&(xh==xhi)&&(yh==yhi));
    }
};

// Get the range of cells that includes everything in the rectangle.
//
// Gracefully handles the case that some or all of the rectangle is
// beyond the maximum cell range.  In that case, it clamps to the edge
// of the cell range.
//
static CellRange rect_cell_range(float x1, float y1, float x2, float y2) {
    CellRange result;
    result.xlo = round_and_clamp(x1 / CELL_SCALE, -CELL_LIMIT, CELL_LIMIT);
    result.ylo = round_and_clamp(y1 / CELL_SCALE, -CELL_LIMIT, CELL_LIMIT);
    result.xhi = round_and_clamp(x2 / CELL_SCALE, -CELL_LIMIT, CELL_LIMIT);
    result.yhi = round_and_clamp(y2 / CELL_SCALE, -CELL_LIMIT, CELL_LIMIT);
    return result;
}

static int64_t cell_id(int64_t cellx, int64_t celly) {
    int64_t icellx = cellx & 0xFFFFFF;
    int64_t icelly = celly & 0xFFFFFF;
    return 0x0001000000000000 | (icellx << 24) | (icelly << 0);
}

// Get the cell ID of the specified point
static int64_t point_cell_id(float x, float y) {
    float cellx = round_and_clamp(x / CELL_SCALE, -CELL_LIMIT, CELL_LIMIT);
    float celly = round_and_clamp(y / CELL_SCALE, -CELL_LIMIT, CELL_LIMIT);
    return cell_id(int64_t(cellx), int64_t(celly));
}

void PlaneMap::remove(const std::string &plane, int64_t cellid, PlaneItem *client) {
    auto piter = planes_.find(plane);
    if (piter == planes_.end()) {
        return;
    }
    Plane &p = piter->second;
    auto liter = p.find(cellid);
    if (liter == p.end()) {
        return;
    }
    EltVec &l = liter->second;
    l.erase(std::remove(l.begin(), l.end(), client), l.end());
    if (l.empty()) {
        p.erase(liter);
    }
    if (p.empty()) {
        planes_.erase(piter);
    }
}

void PlaneMap::insert(const std::string &plane, int64_t cellid, PlaneItem *client) {
    Plane &p = planes_[plane];
    EltVec &l = p[cellid];
    l.push_back(client);
}

void PlaneItem::set_pos(const std::string &plane, float x, float y, float z) {
    int64_t old_cell = point_cell_id(x_, y_);
    int64_t new_cell = point_cell_id(x, y);

    // Update the grid.
    if (pmap_ != 0) {
        if ((plane_ != plane) || (old_cell != new_cell)) {
            pmap_->remove(plane_, old_cell, this);
            pmap_->insert(plane, new_cell, this);
        }
    }

    // Update the client position.
    plane_ = plane;
    x_ = x;
    y_ = y;
    z_ = z;    
}

void PlaneItem::untrack() {
    if (pmap_ != 0) {
        pmap_->remove(plane_, point_cell_id(x_, y_), this);
        pmap_ = 0;
    }
}

void PlaneItem::track(PlaneMap *pmap) {
    if (pmap_ != pmap) {
        untrack();
        pmap->insert(plane_, point_cell_id(x_, y_), this);
        pmap_ = pmap;
    }
}

PlaneItem::PlaneItem() : pmap_(NULL), x_(0.0), y_(0.0), z_(0.0) {
}

PlaneMap::PlaneMap() {
}

PlaneItem::~PlaneItem() {
    untrack();
}

PlaneMap::~PlaneMap() {
    for (const auto &p : planes_) {
        for (const auto &l : p.second) {
            for (PlaneItem *i : l.second) {
                i->pmap_ = NULL;
            }
        }
    }
}

PlaneMap::EltVec PlaneMap::get_cell(const std::string &plane, int64_t cellid) const {
    PlaneMap::EltVec result;
    auto piter = planes_.find(plane);
    if (piter != planes_.end()) {
        const Plane &p = piter->second;
        auto liter = p.find(cellid);
        if (liter != p.end()) {
            result = liter->second;
        }
    }
    return result;
}

int PlaneMap::total_cells() const {
    int total = 0;
    for (const auto &p : planes_) {
        total += p.second.size();
    }
    return total;
}

PlaneMap::IdVector PlaneMap::scan_radius(const std::string &plane, float x, float y, float radius, int64_t prepend) const {
    PlaneMap::IdVector result;
    if (prepend != 0) {
        result.push_back(prepend);
    }
    auto piter = planes_.find(plane);
    if (piter != planes_.end()) {
        const Plane &p = piter->second;
        CellRange range = rect_cell_range(x - radius, y - radius, x + radius, y + radius);
        float radsq = radius*radius;
        for (int cy = range.ylo; cy <= range.yhi; cy++) {
            for (int cx = range.xlo; cx <= range.xhi; cx++) {
                auto liter = p.find(cell_id(cx, cy));
                if (liter != p.end()) {
                    for (PlaneItem *client : liter->second) {
                        if (util::distance_squared(client->x(), client->y(), x, y) <= radsq) {
                            if (client->id() != prepend) {
                                result.push_back(client->id());
                            }
                        }
                    }
                }
            }
        }
    }
    return result;
}

LuaDefine(unittests_planemap, "c") {
    float SC = CELL_SCALE;
    float E = CELL_SCALE * 0.4;
    int LO = -CELL_LIMIT;
    int HI = CELL_LIMIT;
    PlaneMap pm;
    PlaneItem pia, pib;
    PlaneMap::EltVec elts;
    PlaneMap::IdVector ids;

    // Simple test.
    LuaAssert(L, rect_cell_range(-7*SC, -15*SC, 87*SC, 21*SC).equal(-7, -15, 87, 21));

    // Adding an epsilon doesn't change result, if epsilon is less than half of cell scale.
    LuaAssert(L, rect_cell_range(-7*SC+E, -15*SC+E, 87*SC-E, 21*SC-E).equal(-7, -15, 87, 21));
    
    // Rectangle that crosses the high end of the range.
    LuaAssert(L, rect_cell_range((HI-7)*SC, (HI-5)*SC, (HI+3)*SC, (HI+6)*SC).equal(HI-7, HI-5, HI, HI));

    // Rectangle that exceeds the high end of the range.
    LuaAssert(L, rect_cell_range((HI+7)*SC, (HI+5)*SC, (HI+15)*SC, (HI+12)*SC).equal(HI, HI, HI, HI));
    
    // Rectangle that crosses the low end of the range.
    LuaAssert(L, rect_cell_range((LO-7)*SC, (LO-5)*SC, (LO+3)*SC, (LO+4)*SC).equal(LO, LO, LO+3, LO+4));
    
    // Rectangle that exceeds the low end of the range.
    LuaAssert(L, rect_cell_range((LO-15)*SC, (LO-17)*SC, (LO-7)*SC, (LO-5)*SC).equal(LO, LO, LO, LO));

    // Simple test.
    LuaAssert(L, point_cell_id(-7*SC, 15*SC) == cell_id(-7, 15));

    // Adding epsilon doesn't change the result if less than half cell scale.
    LuaAssert(L, point_cell_id(-7*SC+E, 15*SC+E) == cell_id(-7, 15));

    // Right at the top edge of the range.
    LuaAssert(L, point_cell_id(HI*SC, HI*SC) == cell_id(HI, HI));

    // Right at the bottom edge of the range.
    LuaAssert(L, point_cell_id(LO*SC, LO*SC) == cell_id(LO, LO));

    // Beyond various edges.
    LuaAssert(L, point_cell_id((LO-1)*SC, 0) == point_cell_id(LO*SC, 0));
    LuaAssert(L, point_cell_id((HI+1)*SC, 0) == point_cell_id(HI*SC, 0));
    LuaAssert(L, point_cell_id(0, (LO-1)*SC) == point_cell_id(0, LO*SC));
    LuaAssert(L, point_cell_id(0, (HI+1)*SC) == point_cell_id(0, HI*SC));

    // Test using the insert function.
    pm.clear();
    LuaAssert(L, pm.total_cells() == 0);
    pm.insert("foo", 12345, &pia);
    LuaAssert(L, pm.total_cells() == 1);
    pm.insert("foo", 12345, &pib);
    LuaAssert(L, pm.total_cells() == 1);
    elts = pm.get_cell("foo", 12345);
    LuaAssert(L, elts.size() == 2);
    LuaAssert(L, elts[0] == &pia);
    LuaAssert(L, elts[1] == &pib);

    // Test the remove function.
    pm.remove("foo", 12345, &pia);
    LuaAssert(L, pm.total_cells() == 1);
    elts = pm.get_cell("foo", 12345);
    LuaAssert(L, elts.size() == 1);
    LuaAssert(L, elts[0] == &pib);
    pm.remove("foo", 12345, &pib);
    LuaAssert(L, pm.total_cells() == 0);
    
    // Try moving a plane item around without it being tracked to a grid.
    pia.set_pos("foo", 3, 4, 5);
    LuaAssert(L, pia.plane() == "foo");
    LuaAssert(L, pia.x() == 3.0);
    LuaAssert(L, pia.y() == 4.0);
    LuaAssert(L, pia.z() == 5.0);

    // Attach pia to the grid.  This should record it.
    pm.clear();
    pia.track(&pm);
    elts = pm.get_cell("foo", point_cell_id(3.0, 4.0));
    LuaAssert(L, elts.size() == 1);
    LuaAssert(L, elts[0] == &pia);

    // Unattach pia from the grid.  This should unrecord it.
    pia.untrack();
    LuaAssert(L, pm.total_cells() == 0);
    
    // Reattach pia to the grid, then move it.
    pia.track(&pm);
    LuaAssert(L, pm.total_cells() == 1);
    pia.set_pos("bar", 1000.0, 1000.0, 0.0);
    LuaAssert(L, pm.total_cells() == 1);
    elts = pm.get_cell("bar", point_cell_id(1000.0, 1000.0));
    LuaAssert(L, elts.size() == 1);
    LuaAssert(L, elts[0] == &pia);

    // Insert the four elements, then test the scan function.
    pib.track(&pm);
    pia.set_id(123);
    pib.set_id(456);
    pib.set_pos("bar", 1100.0, 1000.0, 0.0);
    ids = pm.scan_radius("bar", 1000.0, 1000.0, 1.0, 0);
    LuaAssert(L, ids.size() == 1);
    LuaAssert(L, ids[0] == 123);
    ids = pm.scan_radius("bar", 1000.0, 1000.0, 99.9, 0);
    LuaAssert(L, ids.size() == 1);
    LuaAssert(L, ids[0] == 123);
    ids = pm.scan_radius("bar", 1000.0, 1000.0, 100.0, 0);
    LuaAssert(L, ids.size() == 2);
    LuaAssert(L, ids[0] == 123);
    LuaAssert(L, ids[1] == 456);

    return 0;
}