316 lines
10 KiB
C++
316 lines
10 KiB
C++
#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_unsorted(const std::string &plane, float x, float y, float radius, bool exclude_nowhere, int64_t special, bool omit) const {
|
|
PlaneMap::IdVector result;
|
|
if ((special != 0)&&(!omit)) {
|
|
result.push_back(special);
|
|
}
|
|
if (exclude_nowhere && (plane == "nowhere")) {
|
|
return result;
|
|
}
|
|
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() != special) {
|
|
result.push_back(client->id());
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
return result;
|
|
}
|
|
|
|
PlaneMap::IdVector PlaneMap::scan_radius(const std::string &plane, float x, float y, float radius, bool exclude_nowhere, int64_t special, bool omit) const {
|
|
PlaneMap::IdVector result = scan_radius_unsorted(plane, x, y, radius, exclude_nowhere, special, omit);
|
|
if ((special != 0)&&(!omit)) {
|
|
std::sort(result.begin() + 1, result.end());
|
|
} else {
|
|
std::sort(result.begin(), result.end());
|
|
}
|
|
return result;
|
|
}
|
|
|
|
LuaDefine(unittests_planemap, "", "some unit tests") {
|
|
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, false, 0, false);
|
|
LuaAssert(L, ids.size() == 1);
|
|
LuaAssert(L, ids[0] == 123);
|
|
ids = pm.scan_radius("bar", 1000.0, 1000.0, 99.9, false, 0, false);
|
|
LuaAssert(L, ids.size() == 1);
|
|
LuaAssert(L, ids[0] == 123);
|
|
ids = pm.scan_radius("bar", 1000.0, 1000.0, 100.0, false, 0, false);
|
|
LuaAssert(L, ids.size() == 2);
|
|
LuaAssert(L, ids[0] == 123);
|
|
LuaAssert(L, ids[1] == 456);
|
|
|
|
return 0;
|
|
}
|