| 306 | } |
| 307 | |
| 308 | unsigned int OcTreeDrawer::generateCube(const octomap::OcTreeVolume& v, |
| 309 | const std::vector<octomath::Vector3>& cube_template, |
| 310 | const unsigned int& current_array_idx, |
| 311 | GLfloat*** glArray) { |
| 312 | |
| 313 | // epsilon to be substracted from cube size so that neighboring planes don't overlap |
| 314 | // seems to introduce strange artifacts nevertheless... |
| 315 | double eps = 1e-5; |
| 316 | |
| 317 | octomath::Vector3 p; |
| 318 | |
| 319 | double half_cube_size = GLfloat(v.second /2.0 -eps); |
| 320 | unsigned int i = current_array_idx; |
| 321 | // Cube surfaces are in gl_array in order: back, front, top, down, left, right. |
| 322 | // Arrays are filled in parallel (increasing i for all at once) |
| 323 | // One color array for all surfaces is filled when requested |
| 324 | |
| 325 | p = v.first + cube_template[0] * half_cube_size; |
| 326 | (*glArray)[0][i] = p.x(); |
| 327 | (*glArray)[0][i+1] = p.y(); |
| 328 | (*glArray)[0][i+2] = p.z(); |
| 329 | |
| 330 | p = v.first + cube_template[1] * half_cube_size; |
| 331 | (*glArray)[1][i] = p.x(); |
| 332 | (*glArray)[1][i+1] = p.y(); |
| 333 | (*glArray)[1][i+2] = p.z(); |
| 334 | |
| 335 | p = v.first + cube_template[2] * half_cube_size; |
| 336 | (*glArray)[2][i] = p.x(); |
| 337 | (*glArray)[2][i+1] = p.y(); |
| 338 | (*glArray)[2][i+2] = p.z(); |
| 339 | |
| 340 | p = v.first + cube_template[3] * half_cube_size; |
| 341 | (*glArray)[3][i] = p.x(); |
| 342 | (*glArray)[3][i+1] = p.y(); |
| 343 | (*glArray)[3][i+2] = p.z(); |
| 344 | |
| 345 | p = v.first + cube_template[4] * half_cube_size; |
| 346 | (*glArray)[4][i] = p.x(); |
| 347 | (*glArray)[4][i+1] = p.y(); |
| 348 | (*glArray)[4][i+2] = p.z(); |
| 349 | |
| 350 | p = v.first + cube_template[5] * half_cube_size; |
| 351 | (*glArray)[5][i] = p.x(); |
| 352 | (*glArray)[5][i+1] = p.y(); |
| 353 | (*glArray)[5][i+2] = p.z(); |
| 354 | i+= 3; //------------------- |
| 355 | |
| 356 | p = v.first + cube_template[6] * half_cube_size; |
| 357 | (*glArray)[0][i] = p.x(); |
| 358 | (*glArray)[0][i+1] = p.y(); |
| 359 | (*glArray)[0][i+2] = p.z(); |
| 360 | |
| 361 | p = v.first + cube_template[7] * half_cube_size; |
| 362 | (*glArray)[1][i] = p.x(); |
| 363 | (*glArray)[1][i+1] = p.y(); |
| 364 | (*glArray)[1][i+2] = p.z(); |
| 365 | |