37 #ifndef VOXEL_GRID_VOXEL_GRID_H 38 #define VOXEL_GRID_VOXEL_GRID_H 74 VoxelGrid(
unsigned int size_x,
unsigned int size_y,
unsigned int size_z);
84 void resize(
unsigned int size_x,
unsigned int size_y,
unsigned int size_z);
89 inline void markVoxel(
unsigned int x,
unsigned int y,
unsigned int z)
93 ROS_DEBUG(
"Error, voxel out of bounds.\n");
96 uint32_t full_mask = ((uint32_t)1<<z<<16) | (1<<z);
100 inline bool markVoxelInMap(
unsigned int x,
unsigned int y,
unsigned int z,
unsigned int marked_threshold)
104 ROS_DEBUG(
"Error, voxel out of bounds.\n");
109 uint32_t* col = &
data_[index];
110 uint32_t full_mask = ((uint32_t)1<<z<<16) | (1<<z);
113 unsigned int marked_bits = *col>>16;
119 inline void clearVoxel(
unsigned int x,
unsigned int y,
unsigned int z)
123 ROS_DEBUG(
"Error, voxel out of bounds.\n");
126 uint32_t full_mask = ((uint32_t)1<<z<<16) | (1<<z);
140 ROS_DEBUG(
"Error, voxel out of bounds.\n");
144 uint32_t* col = &
data_[index];
145 uint32_t full_mask = ((uint32_t)1<<z<<16) | (1<<z);
146 *col &= ~(full_mask);
148 unsigned int unknown_bits = uint16_t(*col>>16) ^ uint16_t(*col);
149 unsigned int marked_bits = *col>>16;
160 unsigned int bit_count;
161 for (bit_count = 0; n;)
164 if (bit_count > bit_threshold)
173 static inline unsigned int numBits(
unsigned int n)
175 unsigned int bit_count;
176 for (bit_count = 0; n; ++bit_count)
184 unsigned int x,
unsigned int y,
unsigned int z,
185 unsigned int size_x,
unsigned int size_y,
unsigned int size_z,
const uint32_t* data)
187 if (x >= size_x || y >= size_y || z >= size_z)
189 ROS_DEBUG(
"Error, voxel out of bounds. (%d, %d, %d)\n", x, y, z);
192 uint32_t full_mask = ((uint32_t)1<<z<<16) | (1<<z);
193 uint32_t result = data[y * size_x + x] & full_mask;
194 unsigned int bits =
numBits(result);
208 void markVoxelLine(
double x0,
double y0,
double z0,
double x1,
double y1,
double z1,
unsigned int max_length = UINT_MAX);
209 void clearVoxelLine(
double x0,
double y0,
double z0,
double x1,
double y1,
double z1,
unsigned int max_length = UINT_MAX);
210 void clearVoxelLineInMap(
double x0,
double y0,
double z0,
double x1,
double y1,
double z1,
unsigned char *map_2d,
211 unsigned int unknown_threshold,
unsigned int mark_threshold,
212 unsigned char free_cost = 0,
unsigned char unknown_cost = 255,
unsigned int max_length = UINT_MAX);
218 unsigned int unknown_threshold = 0,
unsigned int marked_threshold = 0);
222 unsigned int sizeX();
223 unsigned int sizeY();
224 unsigned int sizeZ();
226 template <
class ActionType>
228 ActionType at,
double x0,
double y0,
double z0,
229 double x1,
double y1,
double z1,
unsigned int max_length = UINT_MAX)
231 int dx = int(x1) - int(x0);
232 int dy = int(y1) - int(y0);
233 int dz = int(z1) - int(z0);
235 unsigned int abs_dx = abs(dx);
236 unsigned int abs_dy = abs(dy);
237 unsigned int abs_dz = abs(dz);
239 int offset_dx =
sign(dx);
241 int offset_dz =
sign(dz);
243 unsigned int z_mask = ((1 << 16) | 1) << (
unsigned int)z0;
244 unsigned int offset = (
unsigned int)y0 *
size_x_ + (
unsigned int)x0;
250 double dist = sqrt((x0 - x1) * (x0 - x1) + (y0 - y1) * (y0 - y1) + (z0 - z1) * (z0 - z1));
251 double scale = std::min(1.0, max_length / dist);
254 if (abs_dx >=
max(abs_dy, abs_dz))
256 int error_y = abs_dx / 2;
257 int error_z = abs_dx / 2;
259 bresenham3D(at, grid_off, grid_off, z_off, abs_dx, abs_dy, abs_dz, error_y, error_z, offset_dx, offset_dy, offset_dz, offset, z_mask, (
unsigned int)(scale * abs_dx));
264 if (abs_dy >= abs_dz)
266 int error_x = abs_dy / 2;
267 int error_z = abs_dy / 2;
269 bresenham3D(at, grid_off, grid_off, z_off, abs_dy, abs_dx, abs_dz, error_x, error_z, offset_dy, offset_dx, offset_dz, offset, z_mask, (
unsigned int)(scale * abs_dy));
274 int error_x = abs_dz / 2;
275 int error_y = abs_dz / 2;
277 bresenham3D(at, z_off, grid_off, grid_off, abs_dz, abs_dx, abs_dy, error_x, error_y, offset_dz, offset_dx, offset_dy, offset, z_mask, (
unsigned int)(scale * abs_dz));
282 template <
class ActionType,
class OffA,
class OffB,
class OffC>
284 ActionType at, OffA off_a, OffB off_b, OffC off_c,
285 unsigned int abs_da,
unsigned int abs_db,
unsigned int abs_dc,
286 int error_b,
int error_c,
int offset_a,
int offset_b,
int offset_c,
unsigned int &offset,
287 unsigned int &z_mask,
unsigned int max_length = UINT_MAX)
289 unsigned int end = std::min(max_length, abs_da);
290 for (
unsigned int i = 0; i < end; ++i)
296 if ((
unsigned int)error_b >= abs_da)
301 if ((
unsigned int)error_c >= abs_da)
312 return i > 0 ? 1 : -1;
315 inline unsigned int max(
unsigned int x,
unsigned int y)
317 return x > y ? x : y;
329 inline void operator()(
unsigned int offset,
unsigned int z_mask)
331 data_[offset] |= z_mask;
341 inline void operator()(
unsigned int offset,
unsigned int z_mask)
343 data_[offset] &= ~(z_mask);
353 uint32_t* data,
unsigned char *costmap,
354 unsigned int unknown_clear_threshold,
unsigned int marked_clear_threshold,
355 unsigned char free_cost = 0,
unsigned char unknown_cost = 255): data_(data), costmap_(costmap),
356 unknown_clear_threshold_(unknown_clear_threshold), marked_clear_threshold_(marked_clear_threshold),
357 free_cost_(free_cost), unknown_cost_(unknown_cost)
361 inline void operator()(
unsigned int offset,
unsigned int z_mask)
363 uint32_t* col = &data_[offset];
366 unsigned int unknown_bits = uint16_t(*col>>16) ^ uint16_t(*col);
367 unsigned int marked_bits = *col>>16;
374 costmap_[offset] = free_cost_;
378 costmap_[offset] = unknown_cost_;
385 unsigned int bit_count;
386 for (bit_count = 0; n;)
389 if (bit_count > bit_threshold)
410 offset_ += offset_val;
419 ZOffset(
unsigned int &z_mask) : z_mask_(z_mask) {}
422 offset_val > 0 ? z_mask_ <<= 1 : z_mask_ >>= 1;
431 #endif // VOXEL_GRID_VOXEL_GRID_H
void clearVoxel(unsigned int x, unsigned int y, unsigned int z)
void operator()(unsigned int offset, unsigned int z_mask)
MarkVoxel(uint32_t *data)
VoxelStatus getVoxelColumn(unsigned int x, unsigned int y, unsigned int unknown_threshold=0, unsigned int marked_threshold=0)
static VoxelStatus getVoxel(unsigned int x, unsigned int y, unsigned int z, unsigned int size_x, unsigned int size_y, unsigned int size_z, const uint32_t *data)
ClearVoxelInMap(uint32_t *data, unsigned char *costmap, unsigned int unknown_clear_threshold, unsigned int marked_clear_threshold, unsigned char free_cost=0, unsigned char unknown_cost=255)
void bresenham3D(ActionType at, OffA off_a, OffB off_b, OffC off_c, unsigned int abs_da, unsigned int abs_db, unsigned int abs_dc, int error_b, int error_c, int offset_a, int offset_b, int offset_c, unsigned int &offset, unsigned int &z_mask, unsigned int max_length=UINT_MAX)
ZOffset(unsigned int &z_mask)
unsigned char unknown_cost_
void operator()(int offset_val)
VoxelGrid(unsigned int size_x, unsigned int size_y, unsigned int size_z)
Constructor for a voxel grid.
void operator()(int offset_val)
void clearVoxelInMap(unsigned int x, unsigned int y, unsigned int z)
bool markVoxelInMap(unsigned int x, unsigned int y, unsigned int z, unsigned int marked_threshold)
GridOffset(unsigned int &offset)
void clearVoxelLineInMap(double x0, double y0, double z0, double x1, double y1, double z1, unsigned char *map_2d, unsigned int unknown_threshold, unsigned int mark_threshold, unsigned char free_cost=0, unsigned char unknown_cost=255, unsigned int max_length=UINT_MAX)
bool bitsBelowThreshold(unsigned int n, unsigned int bit_threshold)
void resize(unsigned int size_x, unsigned int size_y, unsigned int size_z)
Resizes a voxel grid to the desired size.
void operator()(unsigned int offset, unsigned int z_mask)
void operator()(unsigned int offset, unsigned int z_mask)
unsigned int max(unsigned int x, unsigned int y)
bool bitsBelowThreshold(unsigned int n, unsigned int bit_threshold)
ClearVoxel(uint32_t *data)
void markVoxel(unsigned int x, unsigned int y, unsigned int z)
void markVoxelLine(double x0, double y0, double z0, double x1, double y1, double z1, unsigned int max_length=UINT_MAX)
static unsigned int numBits(unsigned int n)
unsigned int unknown_clear_threshold_
void clearVoxelColumn(unsigned int index)
void raytraceLine(ActionType at, double x0, double y0, double z0, double x1, double y1, double z1, unsigned int max_length=UINT_MAX)
void clearVoxelLine(double x0, double y0, double z0, double x1, double y1, double z1, unsigned int max_length=UINT_MAX)