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
#define CELL_INVALID 0

// Round a float and return as integer.  Clamp result to the specified range.
static int round_and_clamp(double 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 off
// the map: in that case, returns exactly the valid cells and not the
// invalid ones.
//
static CellRange rect_cell_range(double x1, double y1, double x2, double y2) {
    CellRange result;
    result.xlo = round_and_clamp(x1 / CELL_SCALE, -CELL_LIMIT, CELL_LIMIT + 1);
    result.ylo = round_and_clamp(y1 / CELL_SCALE, -CELL_LIMIT, CELL_LIMIT + 1);
    result.xhi = round_and_clamp(x2 / CELL_SCALE, -CELL_LIMIT - 1, CELL_LIMIT);
    result.yhi = round_and_clamp(y2 / CELL_SCALE, -CELL_LIMIT - 1, 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, or CELL_INVALID if the point is off the map.
static int64_t point_cell_id(double x, double y) {
    double cellx = round(x / CELL_SCALE);
    double celly = round(y / CELL_SCALE);
    if ((cellx < -CELL_LIMIT) || (celly < -CELL_LIMIT) || (cellx > CELL_LIMIT) || (celly > CELL_LIMIT)) {
        return CELL_INVALID;
    }
    return cell_id(int64_t(cellx), int64_t(celly));
}

void PlaneMap::remove(const std::string &plane, int64_t cellid, PlaneItem *client) {
    if (cellid == CELL_INVALID) {
        return;
    }
    if ((plane == "") || (plane == "nowhere")) {
        return;
    }
    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) {
    if (cellid == CELL_INVALID) {
        return;
    }
    if ((plane == "") || (plane == "nowhere")) {
        return;
    }
    Plane &p = planes_[plane];
    EltVec &l = p[cellid];
    l.push_back(client);
}

void PlaneItem::set_pos(const std::string &plane, double x, double y, double 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 PlaneMap::track(PlaneItem *item) {
    if (item->pmap_ != this) {
        item->untrack();
        insert(item->plane(), point_cell_id(item->x(), item->y()), item);
        item->pmap_ = this;
    }
}

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::IdVec PlaneMap::scan_radius(const std::string &plane, double x, double y, double radius, int64_t prepend) const {
    PlaneMap::IdVec 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);
        double 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") {
    double SC = CELL_SCALE;
    double E = CELL_SCALE * 0.4;
    int LO = -CELL_LIMIT;
    int HI = CELL_LIMIT;
    PlaneMap pm;
    PlaneItem pia, pib;
    PlaneMap::EltVec elts;
    PlaneMap::IdVec 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+1, HI+1, 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-1, LO-1));

    // 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) == CELL_INVALID);
    LuaAssert(L, point_cell_id((HI+1)*SC, 0) == CELL_INVALID);
    LuaAssert(L, point_cell_id(0, (LO-1)*SC) == CELL_INVALID);
    LuaAssert(L, point_cell_id(0, (HI+1)*SC) == CELL_INVALID);

    // 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);

    // Test the insert function on the nowhere plane.
    pm.clear();
    pm.insert("nowhere", 12345, &pia);
    pm.insert("nowhere", 12345, &pib);
    LuaAssert(L, pm.total_cells() == 0);
    
    // Test the insert function on an invalid cell.
    pm.clear();
    pm.insert("foo", CELL_INVALID, &pia);
    pm.insert("foo", CELL_INVALID, &pib);
    LuaAssert(L, pm.total_cells() == 0);

    // Try moving a plane item around without it being connected 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();
    pm.track(&pia);
    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.
    pm.track(&pia);
    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.
    pm.track(&pib);
    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;
}