MRPT  1.9.9
COctoMapVoxels.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "opengl-precomp.h" // Precompiled header
11 
15 #include "opengl_internals.h"
16 
17 using namespace mrpt;
18 using namespace mrpt::opengl;
19 using namespace mrpt::math;
20 using namespace std;
21 
23 
24 /** Ctor */
25 COctoMapVoxels::COctoMapVoxels() : m_grid_color(0xE0, 0xE0, 0xE0, 0x90) {}
26 /** Clears everything */
28 {
29  m_voxel_sets.clear();
30  m_grid_cubes.clear();
31 
33 }
34 
37 {
38  m_bb_min = bb_min;
39  m_bb_max = bb_max;
40 }
41 
42 #if MRPT_HAS_OPENGL_GLUT
43 
44 // See: http://www.songho.ca/opengl/gl_vertexarray.html
45 
46 // cube ///////////////////////////////////////////////////////////////////////
47 // v6----- v5
48 // +Z /| /|
49 // A v1------v0|
50 // | | | | |
51 // | | |v7---|-|v4 / -X
52 // | |/ |/ /
53 // | v2------v3 L +X
54 // ----------------------> +Y
55 //
56 
57 const GLubyte grid_line_indices[] = {0, 1, 1, 2, 2, 3, 3, 0, 4, 5, 5, 6,
58  6, 7, 7, 4, 0, 5, 1, 6, 2, 7, 3, 4};
59 
60 const GLubyte cube_indices[36] = {0, 1, 2, 2, 3, 0, 0, 3, 4, 4, 5, 0,
61  0, 5, 6, 6, 1, 0, 1, 6, 7, 7, 2, 1,
62  7, 4, 3, 3, 2, 7, 4, 7, 6, 6, 5, 4};
63 
64 // normal array
65 const GLfloat normals_cube[3 * 6 * 4] = {
66  1, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, // v0,v1,v2,v3 (front)
67  0, 1, 0, 0, 1, 0, 0, 1, 0, 0, 1, 0, // v0,v3,v4,v5 (right)
68  0, 0, 1, 0, 0, 1, 0, 0, 1, 0, 0, 1, // v0,v5,v6,v1 (top)
69  0, -1, 0, 0, -1, 0, 0, -1, 0, 0, -1, 0, // v1,v6,v7,v2 (left)
70  0, 0, -1, 0, 0, -1, 0, 0, -1, 0, 0, -1, // v7,v4,v3,v2 (bottom)
71  -1, 0, 0, -1, 0, 0, -1, 0, 0, -1, 0, 0}; // v4,v7,v6,v5 (back)
72 
73 #endif
74 
75 /*---------------------------------------------------------------
76  render
77  ---------------------------------------------------------------*/
79 {
80 #if MRPT_HAS_OPENGL_GLUT
81 
83 
84  // Draw grids ====================================
85  if (m_show_grids)
86  {
87  glLineWidth(m_grid_width);
89 
90  glDisable(GL_LIGHTING); // Disable lights when drawing lines
91 
92  // Antialiasing:
95  if (m_grid_color.A != 255)
96  {
99  }
100 
101  glColor4ub(
102  m_grid_color.R, m_grid_color.G, m_grid_color.B, m_grid_color.A);
103 
104  const size_t nGrids = m_grid_cubes.size();
105  for (size_t i = 0; i < nGrids; i++)
106  {
107  const TGridCube& c = m_grid_cubes[i];
108 
109  const GLdouble vertices[8 * 3] = {
110  c.max.x, c.max.y, c.max.z, c.max.x, c.min.y, c.max.z,
111  c.max.x, c.min.y, c.min.z, c.max.x, c.max.y, c.min.z,
112  c.min.x, c.max.y, c.min.z, c.min.x, c.max.y, c.max.z,
113  c.min.x, c.min.y, c.max.z, c.min.x, c.min.y, c.min.z};
114  glVertexPointer(3, GL_DOUBLE, 0, vertices);
116  GL_LINES,
117  sizeof(grid_line_indices) / sizeof(grid_line_indices[0]),
118  GL_UNSIGNED_BYTE, grid_line_indices);
119  }
120 
121  glEnable(GL_LIGHTING); // Disable lights when drawing lines
122  // End of antialiasing:
123  glPopAttrib();
124  }
125 
126  // Draw cubes ====================================
127  if (!m_enable_lighting) glDisable(GL_LIGHTING);
128 
130 
131  glNormalPointer(GL_FLOAT, 0, normals_cube);
132 
133  if (m_enable_cube_transparency)
134  {
137  }
138 
139  if (m_showVoxelsAsPoints)
140  {
141  glPointSize(m_showVoxelsAsPointsSize);
143  }
144 
145  for (const auto& m_voxel_set : m_voxel_sets)
146  {
147  if (!m_voxel_set.visible) continue;
148 
149  const std::vector<TVoxel>& voxels = m_voxel_set.voxels;
150  const size_t N = voxels.size();
151  for (size_t j = 0; j < N; j++)
152  {
153  const mrpt::img::TColor& vx_j_col = voxels[j].color;
154  glColor4ub(vx_j_col.R, vx_j_col.G, vx_j_col.B, vx_j_col.A);
155 
156  const mrpt::math::TPoint3D& c = voxels[j].coords;
157  const double L = voxels[j].side_length * 0.5;
158 
159  if (!m_showVoxelsAsPoints)
160  {
161  // Render as cubes:
162  const GLdouble vertices[8 * 3] = {
163  c.x + L, c.y + L, c.z + L, c.x + L, c.y - L, c.z + L,
164  c.x + L, c.y - L, c.z - L, c.x + L, c.y + L, c.z - L,
165  c.x - L, c.y + L, c.z - L, c.x - L, c.y + L, c.z + L,
166  c.x - L, c.y - L, c.z + L, c.x - L, c.y - L, c.z - L};
167  glVertexPointer(3, GL_DOUBLE, 0, vertices);
169  GL_TRIANGLES,
170  sizeof(cube_indices) / sizeof(cube_indices[0]),
171  GL_UNSIGNED_BYTE, cube_indices);
172  }
173  else
174  {
175  // Render as simple points:
176  glVertex3f(c.x, c.y, c.z);
177  }
178  }
179  }
180 
181  if (m_showVoxelsAsPoints)
182  {
183  glEnd(); // of GL_POINTS
184  }
185 
186  if (m_enable_cube_transparency) glDisable(GL_BLEND);
187 
189 
190  if (!m_enable_lighting) glEnable(GL_LIGHTING);
191 
194 
195 #endif
196 }
197 
201 
202 namespace mrpt::opengl
203 {
206 {
207  out << a.visible << a.voxels;
208  return out;
209 }
211 {
212  in >> a.visible >> a.voxels;
213  return in;
214 }
215 
217 {
218  out << a.min << a.max;
219  return out;
220 }
222 {
223  in >> a.min >> a.max;
224  return in;
225 }
226 
228 {
229  out << a.coords << a.side_length << a.color;
230  return out;
231 }
233 {
234  in >> a.coords >> a.side_length >> a.color;
235  return in;
236 }
237 } // end of namespace mrpt::opengl
238 
239 uint8_t COctoMapVoxels::serializeGetVersion() const { return 2; }
241 {
242  writeToStreamRender(out);
243 
244  out << m_voxel_sets << m_grid_cubes << m_bb_min << m_bb_max
245  << m_enable_lighting << m_showVoxelsAsPoints << m_showVoxelsAsPointsSize
246  << m_show_grids << m_grid_width << m_grid_color
247  << m_enable_cube_transparency // added in v1
248  << uint32_t(m_visual_mode); // added in v2
249 }
250 
252 {
253  switch (version)
254  {
255  case 0:
256  case 1:
257  case 2:
258  {
259  readFromStreamRender(in);
260 
261  in >> m_voxel_sets >> m_grid_cubes >> m_bb_min >> m_bb_max >>
262  m_enable_lighting >> m_showVoxelsAsPoints >>
263  m_showVoxelsAsPointsSize >> m_show_grids >> m_grid_width >>
264  m_grid_color;
265 
266  if (version >= 1)
267  in >> m_enable_cube_transparency;
268  else
269  m_enable_cube_transparency = false;
270 
271  if (version >= 2)
272  {
273  uint32_t i;
274  in >> i;
275  m_visual_mode =
276  static_cast<COctoMapVoxels::visualization_mode_t>(i);
277  }
278  else
279  m_visual_mode = COctoMapVoxels::COLOR_FROM_OCCUPANCY;
280  }
281  break;
282  default:
284  };
285 
287 }
288 
291 {
292  bb_min = m_bb_min;
293  bb_max = m_bb_max;
294 
295  // Convert to coordinates of my parent:
296  m_pose.composePoint(bb_min, bb_min);
297  m_pose.composePoint(bb_max, bb_max);
298 }
299 
302 {
303  return a.coords.z < b.coords.z;
304 }
305 
307 {
308  for (auto& m_voxel_set : m_voxel_sets)
309  {
310  std::sort(
311  m_voxel_set.voxels.begin(), m_voxel_set.voxels.end(),
312  &sort_voxels_z);
313  }
314 }
GLAPI void GLAPIENTRY glDisableClientState(GLenum array)
GLAPI void GLAPIENTRY glEnableClientState(GLenum array)
void notifyChange() const
Must be called to notify that the object has changed (so, the display list must be updated) ...
double x
X,Y,Z coordinates.
Definition: TPoint3D.h:83
visualization_mode_t
The different coloring schemes, which modulate the generic mrpt::opengl::CRenderizable object color...
double GLdouble
Definition: glew.h:220
GLAPI void GLAPIENTRY glEnable(GLenum cap)
GLAPI void GLAPIENTRY glPointSize(GLfloat size)
#define GL_VERTEX_ARRAY
Definition: glew.h:725
mrpt::serialization::CArchive & operator<<(mrpt::serialization::CArchive &out, const mrpt::opengl::CLight &o)
Definition: CLight.cpp:122
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
void setBoundingBox(const mrpt::math::TPoint3D &bb_min, const mrpt::math::TPoint3D &bb_max)
Manually changes the bounding box (normally the user doesn&#39;t need to call this)
#define GL_TRIANGLES
Definition: glew.h:277
STL namespace.
uint8_t B
Definition: TColor.h:46
#define GL_UNSIGNED_BYTE
Definition: glew.h:303
#define GL_ONE_MINUS_SRC_ALPHA
Definition: glew.h:288
uint8_t G
Definition: TColor.h:46
void clear()
Clears everything.
GLAPI void GLAPIENTRY glPopAttrib(void)
#define GL_LIGHTING
Definition: glew.h:386
bool sort_voxels_z(const COctoMapVoxels::TVoxel &a, const COctoMapVoxels::TVoxel &b)
GLAPI void GLAPIENTRY glLineWidth(GLfloat width)
float GLfloat
Definition: glew.h:218
GLAPI void GLAPIENTRY glBlendFunc(GLenum sfactor, GLenum dfactor)
A renderizable object suitable for rendering with OpenGL&#39;s display lists.
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
A flexible renderer of voxels, typically from a 3D octo map (see mrpt::maps::COctoMap).
#define GL_COLOR_BUFFER_BIT
Definition: glew.h:266
This base provides a set of functions for maths stuff.
#define GL_LINE_SMOOTH
Definition: glew.h:368
#define GL_NORMAL_ARRAY
Definition: glew.h:726
const GLubyte * c
Definition: glext.h:6406
void render_dl() const override
Render.
GLubyte GLubyte b
Definition: glext.h:6372
GLAPI void GLAPIENTRY glVertexPointer(GLint size, GLenum type, GLsizei stride, const GLvoid *pointer)
GLAPI void GLAPIENTRY glBegin(GLenum mode)
#define GL_BLEND
Definition: glew.h:433
#define GL_POINTS
Definition: glew.h:273
GLAPI void GLAPIENTRY glVertex3f(GLfloat x, GLfloat y, GLfloat z)
uint8_t R
Definition: TColor.h:46
GLAPI void GLAPIENTRY glColor4ub(GLubyte red, GLubyte green, GLubyte blue, GLubyte alpha)
The info of each grid block.
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
GLAPI void GLAPIENTRY glDrawElements(GLenum mode, GLsizei count, GLenum type, const GLvoid *indices)
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
#define GL_SRC_ALPHA
Definition: glew.h:287
The info of each of the voxels.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Color goes from black (occupied voxel) to the chosen color (free voxel)
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
void getBoundingBox(mrpt::math::TPoint3D &bb_min, mrpt::math::TPoint3D &bb_max) const override
Evaluates the bounding box of this object (including possible children) in the coordinate frame of th...
GLAPI void GLAPIENTRY glNormalPointer(GLenum type, GLsizei stride, const GLvoid *pointer)
GLAPI void GLAPIENTRY glPushAttrib(GLbitfield mask)
#define DECLARE_CUSTOM_TTYPENAME(_TYPE)
Identical to MRPT_DECLARE_TTYPENAME but intended for user code.
Definition: TTypeName.h:90
void checkOpenGLError()
Checks glGetError and throws an exception if an error situation is found.
Definition: gl_utils.cpp:155
GLuint in
Definition: glext.h:7391
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:15
const auto bb_max
GLAPI void GLAPIENTRY glEnd(void)
mrpt::serialization::CArchive & operator>>(mrpt::serialization::CArchive &in, mrpt::opengl::CLight &o)
Definition: CLight.cpp:116
#define GL_DOUBLE
Definition: glew.h:312
#define GL_FLOAT
Definition: glew.h:308
const auto bb_min
#define GL_LINES
Definition: glew.h:274
A RGB color - 8bit.
Definition: TColor.h:20
Lightweight 3D point.
Definition: TPoint3D.h:90
GLAPI void GLAPIENTRY glDisable(GLenum cap)
GLubyte GLubyte GLubyte a
Definition: glext.h:6372
unsigned char GLubyte
Definition: glew.h:215
#define GL_LINE_BIT
Definition: glew.h:254
uint8_t A
Definition: TColor.h:46



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: abb8b1a1e Fri Oct 18 14:19:12 2019 +0200 at vie oct 18 14:20:13 CEST 2019