三维栅格地图映射到一维vector数组的下标号如何确定问题
公式(3)如何理解如图:
先zy面开始装,然后一列一列装,最后一列是
z
i
n
d
e
x
z_{index}
zindex
实现代码
// set obstalces into grid map for path planning
void AstarPathFinder::setObs(const double coord_x, const double coord_y, const double coord_z)
{
if (coord_x < gl_xl || coord_y < gl_yl || coord_z < gl_zl ||
coord_x >= gl_xu || coord_y >= gl_yu || coord_z >= gl_zu)
return;
int idx_x = static_cast<int>((coord_x - gl_xl) * inv_resolution);//把double型的强制转化为int型
int idx_y = static_cast<int>((coord_y - gl_yl) * inv_resolution);
int idx_z = static_cast<int>((coord_z - gl_zl) * inv_resolution);
data[idx_x * GLYZ_SIZE + idx_y * GLZ_SIZE + idx_z] = 1;//为什么公式是这样的???????????三维映射到一维
}