MRPT  1.9.9
CVectorField3D.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 
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), y_vf(0, 0), z_vf(0, 0), x_p(0, 0), y_p(0, 0), z_p(0, 0)
26 
27 {
28  m_point_color = m_color;
29  m_field_color = m_color;
30  m_still_color = m_color;
31  m_maxspeed_color = m_color;
32  m_maxspeed = 1.f;
33 }
34 
35 /** Constructor with a initial set of lines. */
37  CMatrixFloat x_vf_ini, CMatrixFloat y_vf_ini, CMatrixFloat z_vf_ini,
38  CMatrixFloat x_p_ini, CMatrixFloat y_p_ini, CMatrixFloat z_p_ini)
39  : m_LineWidth(1.0),
40  m_pointSize(1.0),
41  m_antiAliasing(true),
42  m_colorFromModule(false),
43  m_showPoints(true)
44 {
45  x_vf = x_vf_ini;
46  y_vf = y_vf_ini;
47  z_vf = z_vf_ini;
48  x_p = x_p_ini;
49  y_p = y_p_ini;
50  z_p = z_p_ini;
55  m_maxspeed = 1.f;
56 }
57 
58 /*---------------------------------------------------------------
59  render
60  ---------------------------------------------------------------*/
62 {
63 #if MRPT_HAS_OPENGL_GLUT
64 
65  // Enable antialiasing:
66  glPushAttrib(GL_COLOR_BUFFER_BIT | GL_LINE_BIT);
67  if (m_antiAliasing || m_color.A != 255)
68  {
69  glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
70  glEnable(GL_BLEND);
71  }
72  if (m_antiAliasing)
73  {
74  glEnable(GL_LINE_SMOOTH);
75  glEnable(GL_POINT_SMOOTH);
76  }
77 
78  glLineWidth(m_LineWidth);
79  glPointSize(m_pointSize);
80 
82 
83  glDisable(GL_LIGHTING); // Disable lights when drawing lines
84 
85  if (m_showPoints)
86  {
87  glBegin(GL_POINTS);
88  glColor4ub(
90 
91  for (int i = 0; i < x_p.cols(); i++)
92  for (int j = 0; j < x_p.rows(); j++)
93  {
94  glVertex3f(x_p(j, i), y_p(j, i), z_p(j, i));
95  }
96 
97  glEnd();
98  }
99 
100  glBegin(GL_LINES);
101  if (m_colorFromModule == false)
102  {
103  glColor4ub(
105  for (int i = 0; i < x_vf.cols(); i++)
106  for (int j = 0; j < x_vf.rows(); j++)
107  {
108  glVertex3f(x_p(j, i), y_p(j, i), z_p(j, i));
109  glVertex3f(
110  x_p(j, i) + x_vf(j, i), y_p(j, i) + y_vf(j, i),
111  z_p(j, i) + z_vf(j, i));
112  }
113  }
114  else
115  {
116  for (int i = 0; i < x_vf.cols(); i++)
117  for (int j = 0; j < x_vf.rows(); j++)
118  {
119  // Compute color
120  const float module = sqrt(
121  square(x_vf(j, i)) + square(y_vf(j, i)) +
122  square(z_vf(j, i)));
123  if (module > m_maxspeed)
124  glColor4ub(
127  else
128  {
129  const float R =
130  (m_maxspeed - module) * m_still_color.R / m_maxspeed +
131  module * m_maxspeed_color.R / m_maxspeed;
132  const float G =
133  (m_maxspeed - module) * m_still_color.G / m_maxspeed +
134  module * m_maxspeed_color.G / m_maxspeed;
135  const float B =
136  (m_maxspeed - module) * m_still_color.B / m_maxspeed +
137  module * m_maxspeed_color.B / m_maxspeed;
138  const float A =
139  (m_maxspeed - module) * m_still_color.A / m_maxspeed +
140  module * m_maxspeed_color.A / m_maxspeed;
141  glColor4ub(R, G, B, A);
142  }
143 
144  glVertex3f(x_p(j, i), y_p(j, i), z_p(j, i));
145  glVertex3f(
146  x_p(j, i) + x_vf(j, i), y_p(j, i) + y_vf(j, i),
147  z_p(j, i) + z_vf(j, i));
148  }
149  }
150  glEnd();
151 
152  //******** Future ************
153  // glBegin(GL_TRIANGLES);
154  // glColor4ub( m_field_color.R, m_field_color.G, m_field_color.B,
155  // m_field_color.A);
156  // for (unsigned int i=0; i<xcomp.cols(); i++)
157  // for (unsigned int j=0; j<xcomp.rows(); j++)
158  // {
159  // const float tri_side = 0.25*sqrt(xcomp(j,i)*xcomp(j,i) +
160  // ycomp(j,i)*ycomp(j,i));
161  // const float ang = ::atan2(ycomp(j,i), xcomp(j,i)) - 1.5708;
162  // glVertex3f( -sin(ang)*0.866*tri_side + xMin+i*x_cell_size +
163  // xcomp(j,i), cos(ang)*0.866*tri_side + yMin+j*y_cell_size + ycomp(j,i),
164  // 0);
165  // glVertex3f( cos(ang)*0.5*tri_side + xMin+i*x_cell_size +
166  // xcomp(j,i),
167  // sin(ang)*0.5*tri_side + yMin+j*y_cell_size + ycomp(j,i), 0);
168  // glVertex3f( -cos(ang)*0.5*tri_side + xMin+i*x_cell_size +
169  // xcomp(j,i), -sin(ang)*0.5*tri_side + yMin+j*y_cell_size + ycomp(j,i), 0);
170  // }
171  // glEnd();
172 
174  glEnable(GL_LIGHTING); // Disable lights when drawing lines
175 
176  // End of antialiasing:
177  glPopAttrib();
178 
179 #endif
180 }
181 
182 uint8_t CVectorField3D::serializeGetVersion() const { return 0; }
184 {
186 
187  out << x_vf << y_vf << z_vf;
188  out << x_p << y_p << z_p;
189  out << m_LineWidth;
190  out << m_pointSize;
191  out << m_antiAliasing;
192  out << m_point_color;
193  out << m_field_color;
194 }
196  mrpt::serialization::CArchive& in, uint8_t version)
197 {
198  switch (version)
199  {
200  case 0:
202 
203  in >> x_vf >> y_vf >> z_vf;
204  in >> x_p >> y_p >> z_p;
205  in >> m_LineWidth;
206  in >> m_pointSize;
207  in >> m_antiAliasing;
208  in >> m_point_color;
209  in >> m_field_color;
210  break;
211 
212  default:
214  break;
215  };
217 }
218 
221 {
222  bb_min.x = 10e10;
223  bb_min.y = 10e10;
224  bb_min.z = 10e10;
225  bb_max.x = -10e10;
226  bb_max.y = -10e10;
227  bb_max.z = -10e10;
228 
229  for (int i = 0; i < x_p.cols(); i++)
230  for (int j = 0; j < x_p.rows(); j++)
231  {
232  // Minimum values
233  if (x_p(j, i) < bb_min.x) bb_min.x = x_p(j, i);
234 
235  if (x_p(j, i) + x_vf(j, i) < bb_min.x)
236  bb_min.x = x_p(j, i) + x_vf(j, i);
237 
238  if (y_p(j, i) < bb_min.y) bb_min.y = y_p(j, i);
239 
240  if (y_p(j, i) + y_vf(j, i) < bb_min.y)
241  bb_min.y = y_p(j, i) + y_vf(j, i);
242 
243  if (z_p(j, i) < bb_min.z) bb_min.z = z_p(j, i);
244 
245  if (z_p(j, i) + z_vf(j, i) < bb_min.z)
246  bb_min.z = z_p(j, i) + z_vf(j, i);
247 
248  // Maximum values
249  if (x_p(j, i) > bb_max.x) bb_max.x = x_p(j, i);
250 
251  if (x_p(j, i) + x_vf(j, i) > bb_max.x)
252  bb_max.x = x_p(j, i) + x_vf(j, i);
253 
254  if (y_p(j, i) > bb_max.y) bb_max.y = y_p(j, i);
255 
256  if (y_p(j, i) + y_vf(j, i) > bb_max.y)
257  bb_max.y = y_p(j, i) + y_vf(j, i);
258 
259  if (z_p(j, i) > bb_max.z) bb_max.z = z_p(j, i);
260 
261  if (z_p(j, i) + z_vf(j, i) > bb_max.z)
262  bb_max.z = z_p(j, i) + z_vf(j, i);
263  }
264 
265  // Convert to coordinates of my parent:
268 }
void notifyChange() const
Must be called to notify that the object has changed (so, the display list must be updated) ...
float m_LineWidth
By default it is 1.0.
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
const double G
mrpt::img::TColor m_still_color
Color associated to fields with null module.
STL namespace.
mrpt::math::CMatrixF z_vf
Z component of the vector field.
uint8_t B
Definition: TColor.h:46
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;.
float m_pointSize
By default it is 1.0.
mrpt::poses::CPose3D m_pose
6D pose wrt the parent coordinate reference.
Definition: CRenderizable.h:54
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
bool m_showPoints
By default it is true.
This base provides a set of functions for maths stuff.
mrpt::img::TColor m_point_color
void writeToStreamRender(mrpt::serialization::CArchive &out) const
T square(const T x)
Inline function for the square of a number.
mrpt::math::CMatrixF x_vf
X component of the vector field.
mrpt::math::CMatrixF z_p
Z coordinate of the points at which the vector field is plotted.
mrpt::math::CMatrixF y_p
Y coordinate of the points at which the vector field is plotted.
mrpt::img::TColor m_field_color
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
A 3D vector field representation, consisting of points and arrows drawn at any spatial position...
size_type rows() const
Number of rows in the matrix.
size_type cols() const
Number of columns in the matrix.
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.
mrpt::img::TColor m_color
Color components in the range [0,255].
Definition: CRenderizable.h:51
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:54
mrpt::math::CMatrixF y_vf
Y component of the vector field.
const float R
mrpt::vision::TStereoCalibResults out
mrpt::math::CMatrixF x_p
X coordinate of the points at which the vector field is plotted.
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:15
const auto bb_max
void render_dl() const override
Render.
void composePoint(double lx, double ly, double lz, double &gx, double &gy, double &gz, mrpt::optional_ref< mrpt::math::CMatrixDouble33 > out_jacobian_df_dpoint=std::nullopt, mrpt::optional_ref< mrpt::math::CMatrixDouble36 > out_jacobian_df_dpose=std::nullopt, mrpt::optional_ref< mrpt::math::CMatrixDouble36 > out_jacobian_df_dse3=std::nullopt, 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...
const auto bb_min
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
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.
Definition: TPoint3D.h:90
This template class provides the basic functionality for a general 2D any-size, resizable container o...
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)
uint8_t A
Definition: TColor.h:46



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7e629e01a Sat Dec 14 00:05:55 2019 +0100 at sáb dic 14 00:15:10 CET 2019