g2o
Loading...
Searching...
No Matches
Classes | Functions
g2o::opengl Namespace Reference

Classes

class  GLUWrapper
 handle the GLU quadratic More...
 

Functions

void drawArrow2D (float len, float head_width, float head_len)
 
void drawPoseBox ()
 
void drawBox (GLfloat l, GLfloat w, GLfloat h)
 
void drawPlane (GLfloat l, GLfloat w)
 
void drawSphere (GLfloat radius)
 
void drawEllipsoid (GLfloat r1, GLfloat r2, GLfloat r3)
 
void drawCone (GLfloat radius, GLfloat height)
 
void drawCylinder (GLfloat radius, GLfloat height)
 
void drawDisk (GLfloat radius)
 
void drawCircle (GLfloat radius, int segments)
 
void drawPyramid (GLfloat length, GLfloat height)
 
void drawRangeRing (GLfloat range, GLfloat fov, GLfloat range_width)
 
void drawSlice (GLfloat radius, GLfloat height, GLfloat fov, int slices_per_circle)
 
void drawPoint (float pointSize)
 

Function Documentation

◆ drawArrow2D()

void G2O_OPENGL_API g2o::opengl::drawArrow2D ( float  len,
float  head_width,
float  head_len 
)

draw a 2D arrow along the x axis with the given len

Definition at line 53 of file opengl_primitives.cpp.

53 {
54 glBegin(GL_LINES);
55 glVertex2f(0.f, 0.f);
56 glVertex2f(len, 0.f);
57 glEnd();
58
59 glNormal3f(0.f, 0.f, 1.f);
60 glBegin(GL_TRIANGLES);
61 glVertex2f(len, 0.f);
62 glVertex2f(len - head_len, 0.5f * head_width);
63 glVertex2f(len - head_len, -0.5f * head_width);
64 glEnd();
65}

◆ drawBox()

void G2O_OPENGL_API g2o::opengl::drawBox ( GLfloat  l,
GLfloat  w,
GLfloat  h 
)

draw a box that is centered in the current coordinate frame

Parameters
llength of the box (x dimension)
wwidth of the box (y dimension)
hheight of the box (z dimension)

Definition at line 100 of file opengl_primitives.cpp.

100 {
101 GLfloat sx = l * 0.5f;
102 GLfloat sy = w * 0.5f;
103 GLfloat sz = h * 0.5f;
104
105 glBegin(GL_QUADS);
106 // bottom
107 glNormal3f(0.0f, 0.0f, -1.0f);
108 glVertex3f(-sx, -sy, -sz);
109 glVertex3f(-sx, sy, -sz);
110 glVertex3f(sx, sy, -sz);
111 glVertex3f(sx, -sy, -sz);
112 // top
113 glNormal3f(0.0f, 0.0f, 1.0f);
114 glVertex3f(-sx, -sy, sz);
115 glVertex3f(-sx, sy, sz);
116 glVertex3f(sx, sy, sz);
117 glVertex3f(sx, -sy, sz);
118 // back
119 glNormal3f(-1.0f, 0.0f, 0.0f);
120 glVertex3f(-sx, -sy, -sz);
121 glVertex3f(-sx, sy, -sz);
122 glVertex3f(-sx, sy, sz);
123 glVertex3f(-sx, -sy, sz);
124 // front
125 glNormal3f(1.0f, 0.0f, 0.0f);
126 glVertex3f(sx, -sy, -sz);
127 glVertex3f(sx, sy, -sz);
128 glVertex3f(sx, sy, sz);
129 glVertex3f(sx, -sy, sz);
130 // left
131 glNormal3f(0.0f, -1.0f, 0.0f);
132 glVertex3f(-sx, -sy, -sz);
133 glVertex3f(sx, -sy, -sz);
134 glVertex3f(sx, -sy, sz);
135 glVertex3f(-sx, -sy, sz);
136 // right
137 glNormal3f(0.0f, 1.0f, 0.0f);
138 glVertex3f(-sx, sy, -sz);
139 glVertex3f(sx, sy, -sz);
140 glVertex3f(sx, sy, sz);
141 glVertex3f(-sx, sy, sz);
142 glEnd();
143}

Referenced by drawPoseBox().

◆ drawCircle()

void G2O_OPENGL_API g2o::opengl::drawCircle ( GLfloat  radius,
int  segments = 32 
)

draw a circle using GL_LINES

Definition at line 198 of file opengl_primitives.cpp.

198 {
199 double angleStep = (2 * M_PI / (segments));
200 glBegin(GL_LINE_STRIP);
201 for (int i = 0; i <= segments; i++) {
202 double angle = i * angleStep;
203 float x = radius * cos(angle);
204 float y = radius * sin(angle);
205 glVertex3f(x, y, 0.f);
206 }
207 glEnd();
208}

◆ drawCone()

void G2O_OPENGL_API g2o::opengl::drawCone ( GLfloat  radius,
GLfloat  height 
)

draw a cone

Definition at line 172 of file opengl_primitives.cpp.

172 {
173 glPushMatrix();
174 glRotatef(-90.f, 1.f, 0.f, 0.f);
175 glTranslatef(0.f, 0.f, -height / 2.0f);
176 gluCylinder(GLUWrapper::getQuadradic(), radius, 0.f, height, 32, 1);
177 gluDisk(GLUWrapper::getQuadradic(), 0, radius, 32, 1);
178 glPopMatrix();
179}

References g2o::opengl::GLUWrapper::getQuadradic().

◆ drawCylinder()

void G2O_OPENGL_API g2o::opengl::drawCylinder ( GLfloat  radius,
GLfloat  height 
)

draw a (closed) cylinder

Parameters
radiusthe radius of the cylinder
heightthe height of the cylinder

Definition at line 181 of file opengl_primitives.cpp.

181 {
182 glPushMatrix();
183 glRotatef(-90, 1.f, 0.f, 0.f);
184 glTranslatef(0.f, 0.f, +height / 2.0f);
185 gluDisk(GLUWrapper::getQuadradic(), 0.f, radius, 32, 1);
186 glTranslatef(0, 0, -height);
187 gluCylinder(GLUWrapper::getQuadradic(), radius, radius, height, 32, 1);
188 glRotatef(180, 1.f, 0.f, 0.f);
189 gluDisk(GLUWrapper::getQuadradic(), 0, radius, 32, 1);
190 glPopMatrix();
191}

References g2o::opengl::GLUWrapper::getQuadradic().

◆ drawDisk()

void G2O_OPENGL_API g2o::opengl::drawDisk ( GLfloat  radius)

draw a disk

Definition at line 193 of file opengl_primitives.cpp.

193 {
194 glRotatef(90, 0.f, 1.f, 0.f);
195 gluDisk(GLUWrapper::getQuadradic(), 0, radius, 32, 1);
196}

References g2o::opengl::GLUWrapper::getQuadradic().

◆ drawEllipsoid()

void G2O_OPENGL_API g2o::opengl::drawEllipsoid ( GLfloat  r1,
GLfloat  r2,
GLfloat  r3 
)

draw a ellipsoid whose center is in the origin of the current coordinate frame

Parameters
r1radius along x axis
r2radius along y axis
r3radius along z axis

Definition at line 162 of file opengl_primitives.cpp.

162 {
163 GLboolean hasNormalization = glIsEnabled(GL_NORMALIZE);
164 if (!hasNormalization) glEnable(GL_NORMALIZE);
165 glPushMatrix();
166 glScalef(r1, r2, r3);
167 gluSphere(GLUWrapper::getQuadradic(), 1.0f, 32, 32);
168 glPopMatrix();
169 if (!hasNormalization) glDisable(GL_NORMALIZE);
170}

References g2o::opengl::GLUWrapper::getQuadradic().

◆ drawPlane()

void G2O_OPENGL_API g2o::opengl::drawPlane ( GLfloat  l,
GLfloat  w 
)

draw a plane in x-y dimension with a height of zero

Parameters
llength in x
wwidth in y

Definition at line 145 of file opengl_primitives.cpp.

145 {
146 GLfloat sx = l * 0.5f;
147 GLfloat sy = w * 0.5f;
148
149 glBegin(GL_QUADS);
150 glNormal3f(0.0f, 0.0f, 1.0f);
151 glVertex3f(-sx, -sy, 0.f);
152 glVertex3f(-sx, sy, 0.f);
153 glVertex3f(sx, sy, 0.f);
154 glVertex3f(sx, -sy, 0.f);
155 glEnd();
156}

◆ drawPoint()

void G2O_OPENGL_API g2o::opengl::drawPoint ( float  pointSize)

draw a point in the origin, having a size of pointSize

Definition at line 297 of file opengl_primitives.cpp.

297 {
298 glPointSize(pointSize);
299 glBegin(GL_POINTS);
300 glVertex3f(0, 0, 0);
301 glEnd();
302}

◆ drawPoseBox()

void G2O_OPENGL_API g2o::opengl::drawPoseBox ( )

draws a box used to represent a 6d pose

Definition at line 67 of file opengl_primitives.cpp.

67 {
68 glPushMatrix();
69 glScalef(0.5f, 1.f, 1.f);
70 glPushMatrix();
71 glScalef(1.f, 0.25f, 0.5f);
72 glTranslatef(-0.5f, 0.5f, 0.f);
73 glColor3f(1.0f, 0.3f, 0.3f);
74 drawBox(1.f, 1.f, 1.f);
75 glPopMatrix();
76
77 glPushMatrix();
78 glScalef(1.f, 0.25f, 0.5f);
79 glTranslatef(-0.5f, -0.5f, 0.f);
80 glColor3f(1.0f, 0.1f, 0.1f);
81 drawBox(1.f, 1.f, 1.f);
82 glPopMatrix();
83
84 glPushMatrix();
85 glScalef(1.f, 0.25f, 0.5f);
86 glTranslatef(+0.5f, 0.5f, 0.f);
87 glColor3f(0.3f, 0.3f, 1.0f);
88 drawBox(1.f, 1.f, 1.f);
89 glPopMatrix();
90
91 glPushMatrix();
92 glScalef(1.f, 0.25f, 0.5f);
93 glTranslatef(+0.5f, -0.5f, 0.f);
94 glColor3f(0.1f, 0.1f, 1.f);
95 drawBox(1.f, 1.f, 1.f);
96 glPopMatrix();
97 glPopMatrix();
98}
void drawBox(GLfloat l, GLfloat w, GLfloat h)

References drawBox().

◆ drawPyramid()

void G2O_OPENGL_API g2o::opengl::drawPyramid ( GLfloat  length,
GLfloat  height 
)

draw a pyramid

Definition at line 210 of file opengl_primitives.cpp.

210 {
211 glPushMatrix();
212 glTranslatef(0.f, 0.f, -height / 2.0f);
213 glRotatef(45, 0.f, 0.f, 1.f);
214 gluCylinder(GLUWrapper::getQuadradic(), length, 0.f, height, 4, 1);
215 gluDisk(GLUWrapper::getQuadradic(), 0, length, 4, 1);
216 glPopMatrix();
217}

References g2o::opengl::GLUWrapper::getQuadradic().

◆ drawRangeRing()

void G2O_OPENGL_API g2o::opengl::drawRangeRing ( GLfloat  range,
GLfloat  fov,
GLfloat  range_width = 0.05 
)

draw a range ring

Parameters
rangethe range (radius) of the partial ring
fovField Of View of the range sensor
range_widthspecify how thick the ring should be drawn

Definition at line 219 of file opengl_primitives.cpp.

219 {
220 glPushMatrix();
221 glRotatef((fov / 2.0f) - 90, 0.f, 0.f, 1.f);
222 gluPartialDisk(GLUWrapper::getQuadradic(), range, range + range_width, 32, 1,
223 0.f, fov);
224 glPopMatrix();
225}

References g2o::opengl::GLUWrapper::getQuadradic().

◆ drawSlice()

void G2O_OPENGL_API g2o::opengl::drawSlice ( GLfloat  radius,
GLfloat  height,
GLfloat  fov,
int  slices_per_circle = 32 
)

draw a slice of a cylinder (approximated with slices_per_circle triangles for the complete circle)

Parameters
radiusthe radius of the cylinder
heightthe height of the cylinder
fovthe "fov" of the slice (om degree)
slices_per_circlethe number of triangle used to approximate the fulle circle

Definition at line 227 of file opengl_primitives.cpp.

228 {
229 double fov_rad = fov / 180. * M_PI; // convert to rad
230 int num_slices = int(slices_per_circle * (fov_rad / (2 * M_PI))) + 1;
231 double angle_step = fov_rad / num_slices;
232 double angle_step_half = angle_step * 0.5;
233
234 GLfloat height_half = height * 0.5f;
235 GLfloat lower_z = -height_half;
236 GLfloat upper_z = height_half;
237
238 GLfloat last_x = float(std::cos(-fov_rad * 0.5f) * radius);
239 GLfloat last_y = float(std::sin(-fov_rad * 0.5f) * radius);
240
241 glPushMatrix();
242 glBegin(GL_TRIANGLES);
243 glNormal3f((float)std::sin(-fov_rad * 0.5), (float)-std::cos(-fov_rad * 0.5),
244 0.f);
245 glVertex3f(0.f, 0.f, upper_z);
246 glVertex3f(0.f, 0.f, lower_z);
247 glVertex3f(last_x, last_y, upper_z);
248 glVertex3f(last_x, last_y, upper_z);
249 glVertex3f(last_x, last_y, lower_z);
250 glVertex3f(0.f, 0.f, lower_z);
251
252 double start_angle = -0.5 * fov_rad + angle_step;
253 double angle = start_angle;
254 for (int i = 0; i < num_slices; ++i) {
255 GLfloat x = float(std::cos(angle) * radius);
256 GLfloat y = float(std::sin(angle) * radius);
257 GLfloat front_normal_x = (float)std::cos(angle + angle_step_half);
258 GLfloat front_normal_y = (float)std::sin(angle + angle_step_half);
259
260 // lower triangle
261 glNormal3f(0.f, 0.f, -1.f);
262 glVertex3f(0.f, 0.f, lower_z);
263 glVertex3f(x, y, lower_z);
264 glVertex3f(last_x, last_y, lower_z);
265 // upper
266 glNormal3f(0.f, 0.f, 1.f);
267 glVertex3f(0.f, 0.f, upper_z);
268 glVertex3f(x, y, upper_z);
269 glVertex3f(last_x, last_y, upper_z);
270 // front rectangle (we use two triangles)
271 glNormal3f(front_normal_x, front_normal_y, 0.f);
272 glVertex3f(last_x, last_y, upper_z);
273 glVertex3f(last_x, last_y, lower_z);
274 glVertex3f(x, y, upper_z);
275 glVertex3f(x, y, upper_z);
276 glVertex3f(x, y, lower_z);
277 glVertex3f(last_x, last_y, lower_z);
278
279 last_x = x;
280 last_y = y;
281 angle += angle_step;
282 }
283
284 glNormal3f(float(-std::sin(fov_rad * 0.5)), float(std::cos(fov_rad * 0.5)),
285 -0.f);
286 glVertex3f(0.f, 0.f, upper_z);
287 glVertex3f(0.f, 0.f, lower_z);
288 glVertex3f(last_x, last_y, upper_z);
289 glVertex3f(last_x, last_y, upper_z);
290 glVertex3f(last_x, last_y, lower_z);
291 glVertex3f(0.f, 0.f, lower_z);
292
293 glEnd();
294 glPopMatrix();
295}

◆ drawSphere()

void G2O_OPENGL_API g2o::opengl::drawSphere ( GLfloat  radius)

draw a sphere whose center is in the origin of the current coordinate frame

Parameters
radiusthe radius of the sphere

Definition at line 158 of file opengl_primitives.cpp.

158 {
159 gluSphere(GLUWrapper::getQuadradic(), radius, 32, 32);
160}

References g2o::opengl::GLUWrapper::getQuadradic().