| 534 | } |
| 535 | |
| 536 | void OcTreeDrawer::initOctreeGridVis() { |
| 537 | |
| 538 | if (m_octree_grid_vis_initialized) return; |
| 539 | |
| 540 | clearOcTreeStructure(); |
| 541 | // allocate arrays for octree grid visualization |
| 542 | octree_grid_vertex_size = m_grid_voxels.size() * 12 * 2 * 3; |
| 543 | octree_grid_vertex_array = new GLfloat[octree_grid_vertex_size]; |
| 544 | |
| 545 | // generate the cubes, 12 lines each |
| 546 | std::list<octomap::OcTreeVolume>::iterator it_rec; |
| 547 | unsigned int i = 0; |
| 548 | double x,y,z; |
| 549 | for (it_rec=m_grid_voxels.begin(); it_rec != m_grid_voxels.end(); it_rec++) { |
| 550 | |
| 551 | x = it_rec->first.x(); |
| 552 | y = it_rec->first.y(); |
| 553 | z = it_rec->first.z(); |
| 554 | |
| 555 | double half_voxel_size = it_rec->second / 2.0; |
| 556 | |
| 557 | // ---- |
| 558 | octree_grid_vertex_array[i] = x + half_voxel_size; |
| 559 | octree_grid_vertex_array[i+1] = y + half_voxel_size; |
| 560 | octree_grid_vertex_array[i+2] = z - half_voxel_size; |
| 561 | i+= 3; |
| 562 | octree_grid_vertex_array[i] = x - half_voxel_size; |
| 563 | octree_grid_vertex_array[i+1] = y + half_voxel_size; |
| 564 | octree_grid_vertex_array[i+2] = z - half_voxel_size; |
| 565 | i+= 3; |
| 566 | octree_grid_vertex_array[i] = x - half_voxel_size; |
| 567 | octree_grid_vertex_array[i+1] = y + half_voxel_size; |
| 568 | octree_grid_vertex_array[i+2] = z - half_voxel_size; |
| 569 | i += 3; |
| 570 | octree_grid_vertex_array[i] = x - half_voxel_size; |
| 571 | octree_grid_vertex_array[i+1] = y + half_voxel_size; |
| 572 | octree_grid_vertex_array[i+2] = z + half_voxel_size; |
| 573 | i+= 3; |
| 574 | octree_grid_vertex_array[i] = x - half_voxel_size; |
| 575 | octree_grid_vertex_array[i+1] = y + half_voxel_size; |
| 576 | octree_grid_vertex_array[i+2] = z + half_voxel_size; |
| 577 | i+= 3; |
| 578 | octree_grid_vertex_array[i] = x + half_voxel_size; |
| 579 | octree_grid_vertex_array[i+1] = y + half_voxel_size; |
| 580 | octree_grid_vertex_array[i+2] = z + half_voxel_size; |
| 581 | i+= 3; |
| 582 | octree_grid_vertex_array[i] = x + half_voxel_size; |
| 583 | octree_grid_vertex_array[i+1] = y + half_voxel_size; |
| 584 | octree_grid_vertex_array[i+2] = z + half_voxel_size; |
| 585 | i+= 3; |
| 586 | octree_grid_vertex_array[i] = x + half_voxel_size; |
| 587 | octree_grid_vertex_array[i+1] = y + half_voxel_size; |
| 588 | octree_grid_vertex_array[i+2] = z - half_voxel_size; |
| 589 | i+= 3; |
| 590 | // ---- |
| 591 | octree_grid_vertex_array[i] = x + half_voxel_size; |
| 592 | octree_grid_vertex_array[i+1] = y - half_voxel_size; |
| 593 | octree_grid_vertex_array[i+2] = z + half_voxel_size; |