| 62 | } |
| 63 | |
| 64 | void OcTreeDrawer::draw() const { |
| 65 | static int gl_list_index = -1; |
| 66 | if(m_alternativeDrawing && gl_list_index < 0){ |
| 67 | gl_list_index = glGenLists(1); |
| 68 | m_update = true; |
| 69 | } |
| 70 | if(!m_alternativeDrawing && gl_list_index != -1){//Free video card memory |
| 71 | //std::cerr << "Freeing VRAM\n"; |
| 72 | glDeleteLists(gl_list_index,1); |
| 73 | gl_list_index = -1; |
| 74 | } |
| 75 | if(m_update || !m_alternativeDrawing) |
| 76 | { |
| 77 | if(m_alternativeDrawing) { |
| 78 | std::cout << "Preparing batch rendering, please wait ...\n"; |
| 79 | glNewList(gl_list_index, GL_COMPILE_AND_EXECUTE); |
| 80 | } |
| 81 | |
| 82 | // push current status |
| 83 | glPushMatrix(); |
| 84 | // octomap::pose6d relative_transform = origin * initial_origin.inv(); |
| 85 | |
| 86 | octomap::pose6d relative_transform = origin;// * initial_origin; |
| 87 | |
| 88 | // apply relative transform |
| 89 | const octomath::Quaternion& q = relative_transform.rot(); |
| 90 | glTranslatef(relative_transform.x(), relative_transform.y(), relative_transform.z()); |
| 91 | |
| 92 | // convert quaternion to angle/axis notation |
| 93 | float scale = sqrt(q.x() * q.x() + q.y() * q.y() + q.z() * q.z()); |
| 94 | if (scale) { |
| 95 | float axis_x = q.x() / scale; |
| 96 | float axis_y = q.y() / scale; |
| 97 | float axis_z = q.z() / scale; |
| 98 | float angle = acos(q.u()) * 2.0f * OTD_RAD2DEG; // opengl expects DEG |
| 99 | glRotatef(angle, axis_x, axis_y, axis_z); |
| 100 | } |
| 101 | |
| 102 | glEnableClientState(GL_VERTEX_ARRAY); |
| 103 | |
| 104 | if (m_drawSelection) // Drawing voxels in descending alpha-channel magnitude avoids (most) artifacts |
| 105 | drawSelection(); |
| 106 | if (m_drawOccupied) |
| 107 | drawOccupiedVoxels(); |
| 108 | if (m_drawFree) |
| 109 | drawFreeVoxels(); |
| 110 | if (m_drawOcTreeGrid) |
| 111 | drawOctreeGrid(); |
| 112 | |
| 113 | if (m_displayAxes) { |
| 114 | drawAxes(); |
| 115 | } |
| 116 | |
| 117 | glDisableClientState(GL_VERTEX_ARRAY); |
| 118 | |
| 119 | // reset previous status |
| 120 | glPopMatrix(); |
| 121 | if(m_alternativeDrawing) { |