hogman_minimal: hogman_minimal: primitives.h Source File

00001 // HOG-Man - Hierarchical Optimization for Pose Graphs on Manifolds
00002 // Copyright (C) 2010 G. Grisetti, R. Kümmerle, C. Stachniss
00003 //
00004 // This file is part of HOG-Man.
00005 // 
00006 // HOG-Man is free software: you can redistribute it and/or modify
00007 // it under the terms of the GNU General Public License as published by
00008 // the Free Software Foundation, either version 3 of the License, or
00009 // (at your option) any later version.
00010 // 
00011 // HOG-Man is distributed in the hope that it will be useful,
00012 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00013 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00014 // GNU General Public License for more details.
00015 // 
00016 // You should have received a copy of the GNU General Public License
00017 // along with HOG-Man.  If not, see <http://www.gnu.org/licenses/>.
00018 
00019 #ifndef PRIMITIVES_H
00020 #define PRIMITIVES_H
00021 
00023 // @{
00024 
00029 #include <qgl.h>
00030 #include <cmath>
00031 
00035 extern GLUquadricObj* g_quadratic;
00036 
00040 void initPrimitives();
00041 
00048 inline void drawBox(GLfloat l, GLfloat w, GLfloat h)
00049 {
00050   GLfloat sx = l*0.5f;
00051   GLfloat sy = w*0.5f;
00052   GLfloat sz = h*0.5f;
00053 
00054   glBegin(GL_QUADS);
00055   // bottom
00056   glNormal3f( 0.0f, 0.0f,-1.0f);
00057   glVertex3f(-sx, -sy, -sz);
00058   glVertex3f(-sx, sy, -sz);
00059   glVertex3f(sx, sy, -sz);
00060   glVertex3f(sx, -sy, -sz);
00061   // top
00062   glNormal3f( 0.0f, 0.0f,1.0f);
00063   glVertex3f(-sx, -sy, sz);
00064   glVertex3f(-sx, sy, sz);
00065   glVertex3f(sx, sy, sz);
00066   glVertex3f(sx, -sy, sz);
00067   // back
00068   glNormal3f(-1.0f, 0.0f, 0.0f);
00069   glVertex3f(-sx, -sy, -sz);
00070   glVertex3f(-sx, sy, -sz);
00071   glVertex3f(-sx, sy, sz);
00072   glVertex3f(-sx, -sy, sz);
00073   // front
00074   glNormal3f( 1.0f, 0.0f, 0.0f);
00075   glVertex3f(sx, -sy, -sz);
00076   glVertex3f(sx, sy, -sz);
00077   glVertex3f(sx, sy, sz);
00078   glVertex3f(sx, -sy, sz);
00079   // left
00080   glNormal3f( 0.0f, -1.0f, 0.0f);
00081   glVertex3f(-sx, -sy, -sz);
00082   glVertex3f(sx, -sy, -sz);
00083   glVertex3f(sx, -sy, sz);
00084   glVertex3f(-sx, -sy, sz);
00085   //right
00086   glNormal3f( 0.0f, 1.0f, 0.0f);
00087   glVertex3f(-sx, sy, -sz);
00088   glVertex3f(sx, sy, -sz);
00089   glVertex3f(sx, sy, sz);
00090   glVertex3f(-sx, sy, sz);
00091   glEnd();
00092 }
00093 
00099 inline void drawPlane(GLfloat l, GLfloat w)
00100 {
00101   GLfloat sx = l*0.5f;
00102   GLfloat sy = w*0.5f;
00103 
00104   glBegin(GL_QUADS);
00105   glNormal3f( 0.0f, 0.0f, 1.0f);
00106   glVertex3f(-sx, -sy, 0.f);
00107   glVertex3f(-sx, sy, 0.f);
00108   glVertex3f(sx, sy, 0.f);
00109   glVertex3f(sx, -sy, 0.f);
00110   glEnd();
00111 }
00112 
00117 inline void drawSphere(GLfloat radius)
00118 {
00119   gluSphere(g_quadratic, radius, 32, 32);
00120 }
00121 
00128 inline void drawEllipsoid(GLfloat r1, GLfloat r2, GLfloat r3)
00129 {
00130   GLboolean hasNormalization = glIsEnabled(GL_NORMALIZE);
00131   if (!hasNormalization)
00132     glEnable(GL_NORMALIZE);
00133   glPushMatrix();
00134   glScalef(r1, r2, r3);
00135   gluSphere(g_quadratic, 1.0, 32, 32);
00136   glPopMatrix();
00137   if (!hasNormalization)
00138     glDisable(GL_NORMALIZE);
00139 }
00140 
00144 inline void drawCone(GLfloat radius, GLfloat height)
00145 {
00146   glPushMatrix();
00147   glRotatef(-90.f, 1.f, 0.f, 0.f);
00148   glTranslatef(0, 0, - height/2.0);
00149   gluCylinder(g_quadratic, radius, 0.f, height, 32, 1);
00150   gluDisk(g_quadratic, 0, radius, 32, 1);
00151   glPopMatrix();
00152 }
00153 
00159 inline void drawCylinder(GLfloat radius, GLfloat height)
00160 {
00161   glPushMatrix();
00162   glRotatef(-90, 1.f, 0.f, 0.f);
00163   glTranslatef(0, 0, + height/2.0);
00164   gluDisk(g_quadratic, 0, radius, 32, 1);
00165   glTranslatef(0, 0, - height);
00166   gluCylinder(g_quadratic, radius, radius, height, 32, 1);
00167   glRotatef(180, 1.f, 0.f, 0.f);
00168   gluDisk(g_quadratic, 0, radius, 32, 1);
00169   glPopMatrix();
00170 }
00171 
00175 inline void drawPyramid(GLfloat length, GLfloat height)
00176 {
00177   glPushMatrix();
00178   glTranslatef(0, 0, - height/2.0);
00179   glRotatef(45, 0.f, 0.f, 1.f);
00180   gluCylinder(g_quadratic, length, 0.f, height, 32, 1);
00181   gluDisk(g_quadratic, 0, length, 32, 1);
00182   glPopMatrix();
00183 }
00184 
00191 inline void drawRangeRing(GLfloat range, GLfloat fov, GLfloat range_width = 0.05)
00192 {
00193   glPushMatrix();
00194   glRotatef((fov/2.0f) - 90, 0.f, 0.f, 1.f);
00195   gluPartialDisk(g_quadratic, range, range + range_width, 32, 1, 0.f, fov);
00196   glPopMatrix();
00197 }
00198 
00206 inline void drawSlice(GLfloat radius, GLfloat height, GLfloat fov, int slices_per_circle = 32)
00207 {
00208   double fov_rad = fov/180.*M_PI; // convert to rad
00209   int num_slices = int(slices_per_circle * (fov_rad / (2*M_PI))) + 1;
00210   double angle_step = fov_rad / num_slices;
00211   double angle_step_half = angle_step * 0.5;
00212 
00213   GLfloat height_half = height * 0.5f;
00214   GLfloat lower_z = -height_half;
00215   GLfloat upper_z =  height_half;
00216 
00217   GLfloat last_x = std::cos(-fov_rad * 0.5) * radius;
00218   GLfloat last_y = std::sin(-fov_rad * 0.5) * radius;
00219 
00220   glPushMatrix();
00221   glBegin(GL_TRIANGLES);
00222   glNormal3f(std::sin(-fov_rad * 0.5), -std::cos(-fov_rad * 0.5), -0.f);
00223   glVertex3f(0.f, 0.f, upper_z);
00224   glVertex3f(0.f, 0.f, lower_z);
00225   glVertex3f(last_x, last_y, upper_z);
00226   glVertex3f(last_x, last_y, upper_z);
00227   glVertex3f(last_x, last_y, lower_z);
00228   glVertex3f(0.f, 0.f, lower_z);
00229 
00230   double start_angle = -0.5*fov_rad + angle_step;
00231   double angle       = start_angle;
00232   for (int i = 0; i < num_slices; ++i) {
00233     GLfloat x = std::cos(angle) * radius;
00234     GLfloat y = std::sin(angle) * radius;
00235     GLfloat front_normal_x = std::cos(angle + angle_step_half);
00236     GLfloat front_normal_y = std::sin(angle + angle_step_half);
00237 
00238     // lower triangle
00239     glNormal3f(0.f, 0.f, -1.f);
00240     glVertex3f(0.f, 0.f, lower_z);
00241     glVertex3f(x, y, lower_z);
00242     glVertex3f(last_x, last_y, lower_z);
00243     // upper
00244     glNormal3f(0.f, 0.f, 1.f);
00245     glVertex3f(0.f, 0.f, upper_z);
00246     glVertex3f(x, y, upper_z);
00247     glVertex3f(last_x, last_y, upper_z);
00248     //front rectangle (we use two triangles)
00249     glNormal3f(front_normal_x, front_normal_y, 0.f);
00250     glVertex3f(last_x, last_y, upper_z);
00251     glVertex3f(last_x, last_y, lower_z);
00252     glVertex3f(x, y, upper_z);
00253     glVertex3f(x, y, upper_z);
00254     glVertex3f(x, y, lower_z);
00255     glVertex3f(last_x, last_y, lower_z);
00256 
00257     last_x = x;
00258     last_y = y;
00259     angle += angle_step;
00260   }
00261 
00262   glNormal3f(-std::sin(fov_rad * 0.5), std::cos(fov_rad * 0.5), -0.f);
00263   glVertex3f(0.f, 0.f, upper_z);
00264   glVertex3f(0.f, 0.f, lower_z);
00265   glVertex3f(last_x, last_y, upper_z);
00266   glVertex3f(last_x, last_y, upper_z);
00267   glVertex3f(last_x, last_y, lower_z);
00268   glVertex3f(0.f, 0.f, lower_z);
00269 
00270   glEnd();
00271   glPopMatrix();
00272 }
00273 
00277 void drawPoseBox();
00278 
00282 void drawArrow(float length=1.0f, float radius=-1.0f, int nbSubdivisions=12);
00283 
00287 void drawArrow2D(float len, float head_width, float head_len);
00288 
00295 void drawAxis(float length = 1.f);
00296 
00300 void drawGrid(float size=1.0f, int nbSubdivisions=10);
00301 
00302 // @}
00303 
00304 
00305 
00306 
00307 #endif