2010-01-30 07:26:05 +03:00
|
|
|
#ifndef GRID_H_
|
|
|
|
#define GRID_H_
|
|
|
|
|
|
|
|
#include <math.h>
|
2010-01-30 08:25:21 +03:00
|
|
|
#include <stdint.h>
|
2010-01-30 07:26:05 +03:00
|
|
|
#include <QHash>
|
|
|
|
|
|
|
|
const double GRID_COARSE = 0.001;
|
|
|
|
const double GRID_FINE = 0.000001;
|
|
|
|
|
|
|
|
template <typename T>
|
|
|
|
class Grid2d
|
|
|
|
{
|
|
|
|
public:
|
|
|
|
double res;
|
|
|
|
QHash<QPair<int64_t,int64_t>, T> db;
|
|
|
|
|
|
|
|
Grid2d(double resolution) {
|
|
|
|
res = resolution;
|
|
|
|
}
|
|
|
|
/*!
|
|
|
|
Aligns x,y to the grid or to existing point if one close enough exists.
|
|
|
|
Returns the value stored if a point already existing or an uninitialized new value
|
|
|
|
if not.
|
|
|
|
*/
|
|
|
|
T &align(double &x, double &y) {
|
|
|
|
int64_t ix = (int64_t)round(x / res);
|
|
|
|
int64_t iy = (int64_t)round(y / res);
|
|
|
|
if (!db.contains(QPair<int64_t,int64_t>(ix, iy))) {
|
|
|
|
int dist = 10;
|
|
|
|
for (int64_t jx = ix - 1; jx <= ix + 1; jx++) {
|
|
|
|
for (int64_t jy = iy - 1; jy <= iy + 1; jy++) {
|
|
|
|
if (!db.contains(QPair<int64_t,int64_t>(jx, jy)))
|
|
|
|
continue;
|
|
|
|
if (abs(ix-jx) + abs(iy-jy) < dist) {
|
|
|
|
dist = abs(ix-jx) + abs(iy-jy);
|
|
|
|
ix = jx;
|
|
|
|
iy = jy;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
x = ix * res, y = iy * res;
|
|
|
|
return db[QPair<int64_t,int64_t>(ix, iy)];
|
|
|
|
}
|
|
|
|
bool has(double x, double y) const {
|
|
|
|
int64_t ix = (int64_t)round(x / res);
|
|
|
|
int64_t iy = (int64_t)round(y / res);
|
|
|
|
if (db.contains(QPair<int64_t,int64_t>(ix, iy)))
|
|
|
|
return true;
|
|
|
|
for (int64_t jx = ix - 1; jx <= ix + 1; jx++)
|
|
|
|
for (int64_t jy = iy - 1; jy <= iy + 1; jy++) {
|
|
|
|
if (db.contains(QPair<int64_t,int64_t>(jx, jy)))
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
bool eq(double x1, double y1, double x2, double y2) {
|
|
|
|
align(x1, y1);
|
|
|
|
align(x2, y2);
|
|
|
|
if (fabs(x1 - x2) < res && fabs(y1 - y2) < res)
|
|
|
|
return true;
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
T &data(double x, double y) {
|
|
|
|
return align(x, y);
|
|
|
|
}
|
|
|
|
T &operator()(double x, double y) {
|
|
|
|
return align(x, y);
|
|
|
|
}
|
|
|
|
};
|
|
|
|
|
|
|
|
template <typename T>
|
|
|
|
class Grid3d
|
|
|
|
{
|
|
|
|
public:
|
|
|
|
double res;
|
|
|
|
QHash<QPair<QPair<int64_t,int64_t>,int64_t>, T> db;
|
|
|
|
|
|
|
|
Grid3d(double resolution) {
|
|
|
|
res = resolution;
|
|
|
|
}
|
|
|
|
T &align(double &x, double &y, double &z) {
|
|
|
|
int64_t ix = (int64_t)round(x / res);
|
|
|
|
int64_t iy = (int64_t)round(y / res);
|
|
|
|
int64_t iz = (int64_t)round(z / res);
|
|
|
|
if (!db.contains(QPair<QPair<int64_t,int64_t>,int64_t>(QPair<int64_t,int64_t>(ix, iy), iz))) {
|
|
|
|
int dist = 10;
|
|
|
|
for (int64_t jx = ix - 1; jx <= ix + 1; jx++) {
|
|
|
|
for (int64_t jy = iy - 1; jy <= iy + 1; jy++) {
|
|
|
|
for (int64_t jz = iz - 1; jz <= iz + 1; jz++) {
|
|
|
|
if (!db.contains(QPair<QPair<int64_t,int64_t>,int64_t>(QPair<int64_t,int64_t>(jx, jy), jz)))
|
|
|
|
continue;
|
|
|
|
if (abs(ix-jx) + abs(iy-jy) + abs(iz-jz) < dist) {
|
|
|
|
dist = abs(ix-jx) + abs(iy-jy) + abs(iz-jz);
|
|
|
|
ix = jx;
|
|
|
|
iy = jy;
|
|
|
|
iz = jz;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
x = ix * res, y = iy * res, z = iz * res;
|
|
|
|
return db[QPair<QPair<int64_t,int64_t>,int64_t>(QPair<int64_t,int64_t>(ix, iy), iz)];
|
|
|
|
}
|
|
|
|
bool has(double x, double y, double z) {
|
|
|
|
int64_t ix = (int64_t)round(x / res);
|
|
|
|
int64_t iy = (int64_t)round(y / res);
|
|
|
|
int64_t iz = (int64_t)round(z / res);
|
|
|
|
if (db.contains(QPair<QPair<int64_t,int64_t>,int64_t>(QPair<int64_t,int64_t>(ix, iy), iz)))
|
|
|
|
return true;
|
|
|
|
for (int64_t jx = ix - 1; jx <= ix + 1; jx++)
|
|
|
|
for (int64_t jy = iy - 1; jy <= iy + 1; jy++)
|
|
|
|
for (int64_t jz = iz - 1; jz <= iz + 1; jz++) {
|
|
|
|
if (db.contains(QPair<QPair<int64_t,int64_t>,int64_t>(QPair<int64_t,int64_t>(jx, jy), jz)))
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
return false;
|
|
|
|
|
|
|
|
}
|
|
|
|
bool eq(double x1, double y1, double z1, double x2, double y2, double z2) {
|
|
|
|
align(x1, y1, z1);
|
|
|
|
align(x2, y2, z2);
|
|
|
|
if (fabs(x1 - x2) < res && fabs(y1 - y2) < res && fabs(z1 - z2) < res)
|
|
|
|
return true;
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
T &data(double x, double y, double z) {
|
|
|
|
return align(x, y, z);
|
|
|
|
}
|
|
|
|
T &operator()(double x, double y, double z) {
|
|
|
|
return align(x, y, z);
|
|
|
|
}
|
|
|
|
};
|
|
|
|
|
|
|
|
#endif
|