/**
* Free node handler
*/
void srs_env_model::CCollisionGridPlugin::handleFreeNode(srs_env_model::tButServerOcTree::iterator & it, const SMapWithParameters & mp )
{
if (it.getDepth() == m_crawlDepth) {
octomap::OcTreeKey nKey = it.getKey(); //TODO: remove intermedate obj (1.4)
int i = (nKey[0] - m_paddedMinKey[0])/m_multires2DScale;;
int j = (nKey[1] - m_paddedMinKey[1])/m_multires2DScale;
if (m_data->data[m_data->info.width * j + i] == -1) {
m_data->data[m_data->info.width * j + i] = 0;
}
} else {
int intSize = 1 << (m_crawlDepth - it.getDepth());
octomap::OcTreeKey minKey = it.getIndexKey();
for (int dx = 0; dx < intSize; dx++) {
int i = (minKey[0] + dx - m_paddedMinKey[0])/m_multires2DScale;
for (int dy = 0; dy < intSize; dy++) {
int j = (minKey[1] + dy - m_paddedMinKey[1])/m_multires2DScale;
if (m_data->data[m_data->info.width * j + i] == -1) {
m_data->data[m_data->info.width * j + i] = 0;
}
}
}
}
}
/**
* Occupied node handler
*/
void srs_env_model::CMap2DPlugin::handleOccupiedNode(srs_env_model::tButServerOcTree::iterator & it, const SMapWithParameters & mp)
{
if (it.getDepth() == mp.treeDepth)
{
octomap::OcTreeKey nKey = it.getKey(); // TODO: remove intermedate obj (1.4)
int i = nKey[0] - m_paddedMinKey[0];
int j = nKey[1] - m_paddedMinKey[1];
m_data->data[m_data->info.width * j + i] = 100;
} else {
int intSize = 1 << (mp.treeDepth - it.getDepth());
octomap::OcTreeKey minKey = it.getIndexKey();
for (int dx = 0; dx < intSize; dx++) {
int i = minKey[0] + dx - m_paddedMinKey[0];
for (int dy = 0; dy < intSize; dy++) {
int j = minKey[1] + dy - m_paddedMinKey[1];
m_data->data[m_data->info.width * j + i] = 100;
}
}
}
}