MRPT  1.9.9
CVectorField3D.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "opengl-precomp.h" // Precompiled header
11 
14 #include "opengl_internals.h"
15 
16 using namespace mrpt;
17 using namespace mrpt::opengl;
18 using namespace mrpt::math;
19 using namespace std;
20 
22 
23 /** Constructor */
25  : x_vf(0, 0),
26  y_vf(0, 0),
27  z_vf(0, 0),
28  x_p(0, 0),
29  y_p(0, 0),
30  z_p(0, 0),
31  m_LineWidth(1.0),
32  m_pointSize(1.0),
33  m_antiAliasing(true),
34  m_colorFromModule(false),
35  m_showPoints(true)
36 {
37  m_point_color = m_color;
38  m_field_color = m_color;
39  m_still_color = m_color;
40  m_maxspeed_color = m_color;
41  m_maxspeed = 1.f;
42 }
43 
44 /** Constructor with a initial set of lines. */
46  CMatrixFloat x_vf_ini, CMatrixFloat y_vf_ini, CMatrixFloat z_vf_ini,
47  CMatrixFloat x_p_ini, CMatrixFloat y_p_ini, CMatrixFloat z_p_ini)
48  : m_LineWidth(1.0),
49  m_pointSize(1.0),
50  m_antiAliasing(true),
51  m_colorFromModule(false),
52  m_showPoints(true)
53 {
54  x_vf = x_vf_ini;
55  y_vf = y_vf_ini;
56  z_vf = z_vf_ini;
57  x_p = x_p_ini;
58  y_p = y_p_ini;
59  z_p = z_p_ini;
64  m_maxspeed = 1.f;
65 }
66 
67 /*---------------------------------------------------------------
68  render
69  ---------------------------------------------------------------*/
71 {
72 #if MRPT_HAS_OPENGL_GLUT
73 
74  // Enable antialiasing:
76  if (m_antiAliasing || m_color.A != 255)
77  {
80  }
81  if (m_antiAliasing)
82  {
85  }
86 
89 
91 
92  glDisable(GL_LIGHTING); // Disable lights when drawing lines
93 
94  if (m_showPoints)
95  {
97  glColor4ub(
99 
100  for (unsigned int i = 0; i < x_p.cols(); i++)
101  for (unsigned int j = 0; j < x_p.rows(); j++)
102  {
103  glVertex3f(x_p(j, i), y_p(j, i), z_p(j, i));
104  }
105 
106  glEnd();
107  }
108 
109  glBegin(GL_LINES);
110  if (m_colorFromModule == false)
111  {
112  glColor4ub(
114  for (unsigned int i = 0; i < x_vf.cols(); i++)
115  for (unsigned int j = 0; j < x_vf.rows(); j++)
116  {
117  glVertex3f(x_p(j, i), y_p(j, i), z_p(j, i));
118  glVertex3f(
119  x_p(j, i) + x_vf(j, i), y_p(j, i) + y_vf(j, i),
120  z_p(j, i) + z_vf(j, i));
121  }
122  }
123  else
124  {
125  for (unsigned int i = 0; i < x_vf.cols(); i++)
126  for (unsigned int j = 0; j < x_vf.rows(); j++)
127  {
128  // Compute color
129  const float module = sqrt(
130  square(x_vf(j, i)) + square(y_vf(j, i)) +
131  square(z_vf(j, i)));
132  if (module > m_maxspeed)
133  glColor4ub(
136  else
137  {
138  const float R =
139  (m_maxspeed - module) * m_still_color.R / m_maxspeed +
140  module * m_maxspeed_color.R / m_maxspeed;
141  const float G =
142  (m_maxspeed - module) * m_still_color.G / m_maxspeed +
143  module * m_maxspeed_color.G / m_maxspeed;
144  const float B =
145  (m_maxspeed - module) * m_still_color.B / m_maxspeed +
146  module * m_maxspeed_color.B / m_maxspeed;
147  const float A =
148  (m_maxspeed - module) * m_still_color.A / m_maxspeed +
149  module * m_maxspeed_color.A / m_maxspeed;
150  glColor4ub(R, G, B, A);
151  }
152 
153  glVertex3f(x_p(j, i), y_p(j, i), z_p(j, i));
154  glVertex3f(
155  x_p(j, i) + x_vf(j, i), y_p(j, i) + y_vf(j, i),
156  z_p(j, i) + z_vf(j, i));
157  }
158  }
159  glEnd();
160 
161  //******** Future ************
162  // glBegin(GL_TRIANGLES);
163  // glColor4ub( m_field_color.R, m_field_color.G, m_field_color.B,
164  // m_field_color.A);
165  // for (unsigned int i=0; i<xcomp.cols(); i++)
166  // for (unsigned int j=0; j<xcomp.rows(); j++)
167  // {
168  // const float tri_side = 0.25*sqrt(xcomp(j,i)*xcomp(j,i) +
169  // ycomp(j,i)*ycomp(j,i));
170  // const float ang = ::atan2(ycomp(j,i), xcomp(j,i)) - 1.5708;
171  // glVertex3f( -sin(ang)*0.866*tri_side + xMin+i*x_cell_size +
172  // xcomp(j,i), cos(ang)*0.866*tri_side + yMin+j*y_cell_size + ycomp(j,i),
173  // 0);
174  // glVertex3f( cos(ang)*0.5*tri_side + xMin+i*x_cell_size +
175  // xcomp(j,i),
176  // sin(ang)*0.5*tri_side + yMin+j*y_cell_size + ycomp(j,i), 0);
177  // glVertex3f( -cos(ang)*0.5*tri_side + xMin+i*x_cell_size +
178  // xcomp(j,i), -sin(ang)*0.5*tri_side + yMin+j*y_cell_size + ycomp(j,i), 0);
179  // }
180  // glEnd();
181 
183  glEnable(GL_LIGHTING); // Disable lights when drawing lines
184 
185  // End of antialiasing:
186  glPopAttrib();
187 
188 #endif
189 }
190 
193 {
194  writeToStreamRender(out);
195 
196  out << x_vf << y_vf << z_vf;
197  out << x_p << y_p << z_p;
198  out << m_LineWidth;
199  out << m_pointSize;
200  out << m_antiAliasing;
201  out << m_point_color;
202  out << m_field_color;
203 }
206 {
207  switch (version)
208  {
209  case 0:
211 
212  in >> x_vf >> y_vf >> z_vf;
213  in >> x_p >> y_p >> z_p;
214  in >> m_LineWidth;
215  in >> m_pointSize;
216  in >> m_antiAliasing;
217  in >> m_point_color;
218  in >> m_field_color;
219  break;
220 
221  default:
223  break;
224  };
226 }
227 
229  mrpt::math::TPoint3D& bb_min, mrpt::math::TPoint3D& bb_max) const
230 {
231  bb_min.x = 10e10;
232  bb_min.y = 10e10;
233  bb_min.z = 10e10;
234  bb_max.x = -10e10;
235  bb_max.y = -10e10;
236  bb_max.z = -10e10;
237 
238  for (unsigned int i = 0; i < x_p.cols(); i++)
239  for (unsigned int j = 0; j < x_p.rows(); j++)
240  {
241  // Minimum values
242  if (x_p(j, i) < bb_min.x) bb_min.x = x_p(j, i);
243 
244  if (x_p(j, i) + x_vf(j, i) < bb_min.x)
245  bb_min.x = x_p(j, i) + x_vf(j, i);
246 
247  if (y_p(j, i) < bb_min.y) bb_min.y = y_p(j, i);
248 
249  if (y_p(j, i) + y_vf(j, i) < bb_min.y)
250  bb_min.y = y_p(j, i) + y_vf(j, i);
251 
252  if (z_p(j, i) < bb_min.z) bb_min.z = z_p(j, i);
253 
254  if (z_p(j, i) + z_vf(j, i) < bb_min.z)
255  bb_min.z = z_p(j, i) + z_vf(j, i);
256 
257  // Maximum values
258  if (x_p(j, i) > bb_max.x) bb_max.x = x_p(j, i);
259 
260  if (x_p(j, i) + x_vf(j, i) > bb_max.x)
261  bb_max.x = x_p(j, i) + x_vf(j, i);
262 
263  if (y_p(j, i) > bb_max.y) bb_max.y = y_p(j, i);
264 
265  if (y_p(j, i) + y_vf(j, i) > bb_max.y)
266  bb_max.y = y_p(j, i) + y_vf(j, i);
267 
268  if (z_p(j, i) > bb_max.z) bb_max.z = z_p(j, i);
269 
270  if (z_p(j, i) + z_vf(j, i) > bb_max.z)
271  bb_max.z = z_p(j, i) + z_vf(j, i);
272  }
273 
274  // Convert to coordinates of my parent:
275  m_pose.composePoint(bb_min, bb_min);
276  m_pose.composePoint(bb_max, bb_max);
277 }
mrpt::math::CMatrix x_vf
X component of the vector field.
float m_LineWidth
By default it is 1.0.
GLAPI void GLAPIENTRY glEnable(GLenum cap)
GLAPI void GLAPIENTRY glPointSize(GLfloat size)
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
const double G
mrpt::img::TColor m_still_color
Color associated to fields with null module.
EIGEN_STRONG_INLINE void notifyChange() const
Must be called to notify that the object has changed (so, the display list must be updated) ...
STL namespace.
uint8_t B
Definition: TColor.h:46
#define GL_ONE_MINUS_SRC_ALPHA
Definition: glew.h:287
uint8_t G
Definition: TColor.h:46
float m_maxspeed
Value of the module of the motion field which will correspond to &#39;m_maxspeed_color&#39;.
GLAPI void GLAPIENTRY glPopAttrib(void)
float m_pointSize
By default it is 1.0.
#define GL_LIGHTING
Definition: glew.h:385
GLAPI void GLAPIENTRY glLineWidth(GLfloat width)
mrpt::poses::CPose3D m_pose
6D pose wrt the parent coordinate reference.
Definition: CRenderizable.h:55
GLAPI void GLAPIENTRY glBlendFunc(GLenum sfactor, GLenum dfactor)
unsigned char uint8_t
Definition: rptypes.h:41
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:90
bool m_showPoints
By default it is true.
T square(const T x)
Inline function for the square of a number.
#define GL_COLOR_BUFFER_BIT
Definition: glew.h:265
This base provides a set of functions for maths stuff.
mrpt::img::TColor m_point_color
mrpt::math::CMatrix y_p
Y coordinate of the points at which the vector field is plotted.
void writeToStreamRender(mrpt::serialization::CArchive &out) const
#define GL_LINE_SMOOTH
Definition: glew.h:367
mrpt::math::CMatrix z_vf
Z component of the vector field.
mrpt::math::CMatrix y_vf
Y component of the vector field.
double x
X,Y,Z coordinates.
#define GL_POINT_SMOOTH
Definition: glew.h:363
mrpt::img::TColor m_field_color
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
GLAPI void GLAPIENTRY glBegin(GLenum mode)
#define GL_BLEND
Definition: glew.h:432
#define GL_POINTS
Definition: glew.h:272
mrpt::math::CMatrix z_p
Z coordinate of the points at which the vector field is plotted.
A 3D vector field representation, consisting of points and arrows drawn at any spatial position...
GLAPI void GLAPIENTRY glVertex3f(GLfloat x, GLfloat y, GLfloat z)
uint8_t R
Definition: TColor.h:46
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
GLAPI void GLAPIENTRY glColor4ub(GLubyte red, GLubyte green, GLubyte blue, GLubyte alpha)
mrpt::img::TColor m_color
Color components in the range [0,255].
Definition: CRenderizable.h:52
#define GL_SRC_ALPHA
Definition: glew.h:286
static void checkOpenGLError()
Checks glGetError and throws an exception if an error situation is found.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
bool m_colorFromModule
By default it is false.
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:52
GLAPI void GLAPIENTRY glPushAttrib(GLbitfield mask)
const float R
void composePoint(double lx, double ly, double lz, double &gx, double &gy, double &gz, mrpt::math::CMatrixFixedNumeric< double, 3, 3 > *out_jacobian_df_dpoint=nullptr, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dpose=nullptr, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dse3=nullptr, bool use_small_rot_approx=false) const
An alternative, slightly more efficient way of doing with G and L being 3D points and P this 6D pose...
Definition: CPose3D.cpp:379
GLuint in
Definition: glext.h:7274
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:15
GLAPI void GLAPIENTRY glEnd(void)
void render_dl() const override
Render.
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
#define GL_LINES
Definition: glew.h:273
mrpt::img::TColor m_maxspeed_color
Color associated to fields whose module is equal or larger than &#39;m_maxspeed&#39;.
bool m_antiAliasing
By default it is true.
Lightweight 3D point.
GLAPI void GLAPIENTRY glDisable(GLenum cap)
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...
void readFromStreamRender(mrpt::serialization::CArchive &in)
#define GL_LINE_BIT
Definition: glew.h:253
uint8_t A
Definition: TColor.h:46
mrpt::math::CMatrix x_p
X coordinate of the points at which the vector field is plotted.



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020