79 lines
1.6 KiB
C++
79 lines
1.6 KiB
C++
#ifndef POSITION_MAP_H
|
|
#define POSITION_MAP_H
|
|
#include <map>
|
|
|
|
struct PositionMapKey
|
|
{
|
|
int x;
|
|
int y;
|
|
int z;
|
|
bool operator<(const PositionMapKey &other) const {
|
|
if (x < other.x) {
|
|
return true;
|
|
} else if (x > other.x) {
|
|
return false;
|
|
}
|
|
if (y < other.y) {
|
|
return true;
|
|
} else if (y > other.y) {
|
|
return false;
|
|
}
|
|
return z < other.z;
|
|
}
|
|
};
|
|
|
|
template <class T>
|
|
class PositionMap
|
|
{
|
|
public:
|
|
PositionMap(float gridSize=0.001) :
|
|
m_intGridSize(PositionMap<T>::makeInt(gridSize))
|
|
{
|
|
}
|
|
|
|
static int makeInt(float value)
|
|
{
|
|
return value * 1000;
|
|
}
|
|
|
|
PositionMapKey makeKey(float x, float y, float z)
|
|
{
|
|
PositionMapKey key;
|
|
key.x = makeInt(x) / m_intGridSize;
|
|
key.y = makeInt(y) / m_intGridSize;
|
|
key.z = makeInt(z) / m_intGridSize;
|
|
return key;
|
|
}
|
|
|
|
void addPosition(float x, float y, float z, T data)
|
|
{
|
|
m_map[makeKey(x, y, z)] = data;
|
|
}
|
|
|
|
bool findPosition(float x, float y, float z, T *data = nullptr)
|
|
{
|
|
const auto &result = m_map.find(makeKey(x, y, z));
|
|
if (result == m_map.end())
|
|
return false;
|
|
if (data)
|
|
*data = result->second;
|
|
return true;
|
|
}
|
|
|
|
void removePosition(float x, float y, float z)
|
|
{
|
|
m_map.erase(makeKey(x, y, z));
|
|
}
|
|
|
|
std::map<PositionMapKey, T> &map()
|
|
{
|
|
return m_map;
|
|
}
|
|
|
|
private:
|
|
std::map<PositionMapKey, T> m_map;
|
|
int m_intGridSize;
|
|
};
|
|
|
|
#endif
|