| 874 | } |
| 875 | |
| 876 | void OcTreeDrawer::drawAxes() const { |
| 877 | |
| 878 | octomap::pose6d relative_transform = origin * initial_origin.inv(); |
| 879 | |
| 880 | glPushMatrix(); |
| 881 | |
| 882 | float length = 0.15f; |
| 883 | |
| 884 | GLboolean lighting, colorMaterial; |
| 885 | glGetBooleanv(GL_LIGHTING, &lighting); |
| 886 | glGetBooleanv(GL_COLOR_MATERIAL, &colorMaterial); |
| 887 | |
| 888 | glDisable(GL_COLOR_MATERIAL); |
| 889 | |
| 890 | double angle= 2 * acos(initial_origin.rot().u()); |
| 891 | double scale = sqrt (initial_origin.rot().x()*initial_origin.rot().x() |
| 892 | + initial_origin.rot().y()*initial_origin.rot().y() |
| 893 | + initial_origin.rot().z()*initial_origin.rot().z()); |
| 894 | double ax= initial_origin.rot().x() / scale; |
| 895 | double ay= initial_origin.rot().y() / scale; |
| 896 | double az= initial_origin.rot().z() / scale; |
| 897 | |
| 898 | if (angle > 0) glRotatef(OTD_RAD2DEG*angle, ax, ay, az); |
| 899 | |
| 900 | float color[4]; |
| 901 | color[0] = 0.7f; color[1] = 0.7f; color[2] = 1.0f; color[3] = 1.0f; |
| 902 | glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, color); |
| 903 | QGLViewer::drawArrow(length, 0.01*length); |
| 904 | |
| 905 | color[0] = 1.0f; color[1] = 0.7f; color[2] = 0.7f; color[3] = 1.0f; |
| 906 | glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, color); |
| 907 | glPushMatrix(); |
| 908 | glRotatef(90.0, 0.0, 1.0, 0.0); |
| 909 | QGLViewer::drawArrow(length, 0.01*length); |
| 910 | glPopMatrix(); |
| 911 | |
| 912 | color[0] = 0.7f; color[1] = 1.0f; color[2] = 0.7f; color[3] = 1.0f; |
| 913 | glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, color); |
| 914 | glPushMatrix(); |
| 915 | glRotatef(-90.0, 1.0, 0.0, 0.0); |
| 916 | QGLViewer::drawArrow(length, 0.01*length); |
| 917 | glPopMatrix(); |
| 918 | |
| 919 | glTranslatef(relative_transform.trans().x(), relative_transform.trans().y(), relative_transform.trans().z()); |
| 920 | |
| 921 | if (colorMaterial) |
| 922 | glEnable(GL_COLOR_MATERIAL); |
| 923 | if (!lighting) |
| 924 | glDisable(GL_LIGHTING); |
| 925 | |
| 926 | glPopMatrix(); |
| 927 | } |
| 928 | |
| 929 | } // namespace |