MRPT  1.9.9
CVectorField2D.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 
15 #include "opengl_internals.h"
16 
17 using namespace mrpt;
18 using namespace mrpt::opengl;
19 
20 using namespace mrpt::math;
21 using namespace std;
22 
24 
25 /** Constructor */
26 CVectorField2D::CVectorField2D() : xcomp(0, 0), ycomp(0, 0)
27 
28 {
29  m_point_color = m_color;
30  m_field_color = m_color;
31 }
32 
33 /** Constructor with a initial set of lines. */
35  CMatrixFloat Matrix_x, CMatrixFloat Matrix_y, float xmin, float xmax,
36  float ymin, float ymax)
37  : m_LineWidth(1.0), m_pointSize(1.0), m_antiAliasing(true)
38 {
39  MRPT_UNUSED_PARAM(Matrix_x);
40  MRPT_UNUSED_PARAM(Matrix_y);
41  MRPT_UNUSED_PARAM(xmin);
42  MRPT_UNUSED_PARAM(xmax);
43  MRPT_UNUSED_PARAM(ymin);
44  MRPT_UNUSED_PARAM(ymax);
47 }
48 
49 /*---------------------------------------------------------------
50  render
51  ---------------------------------------------------------------*/
53 {
54 #if MRPT_HAS_OPENGL_GLUT
55 
56  // Enable antialiasing:
58  if (m_antiAliasing || m_color.A != 255)
59  {
62  }
63  if (m_antiAliasing)
64  {
67  }
68 
71 
73 
74  glDisable(GL_LIGHTING); // Disable lights when drawing lines
76  glColor4ub(
78 
79  const float x_cell_size = (xMax - xMin) / (xcomp.cols() - 1);
80  const float y_cell_size = (yMax - yMin) / (ycomp.rows() - 1);
81 
82  for (int i = 0; i < xcomp.cols(); i++)
83  for (int j = 0; j < xcomp.rows(); j++)
84  {
85  glVertex3f(xMin + i * x_cell_size, yMin + j * y_cell_size, 0);
86  }
87 
88  glEnd();
89 
91  glColor4ub(
93  for (int i = 0; i < xcomp.cols(); i++)
94  for (int j = 0; j < xcomp.rows(); j++)
95  {
96  glVertex3f(xMin + i * x_cell_size, yMin + j * y_cell_size, 0);
97  glVertex3f(
98  xMin + i * x_cell_size + xcomp(j, i),
99  yMin + j * y_cell_size + ycomp(j, i), 0);
100  }
101  glEnd();
102 
104  glColor4ub(
106  for (int i = 0; i < xcomp.cols(); i++)
107  for (int j = 0; j < xcomp.rows(); j++)
108  {
109  const float tri_side =
110  0.25 *
111  sqrt(xcomp(j, i) * xcomp(j, i) + ycomp(j, i) * ycomp(j, i));
112  const float ang = ::atan2(ycomp(j, i), xcomp(j, i)) - 1.5708;
113  glVertex3f(
114  -sin(ang) * 0.866 * tri_side + xMin + i * x_cell_size +
115  xcomp(j, i),
116  cos(ang) * 0.866 * tri_side + yMin + j * y_cell_size +
117  ycomp(j, i),
118  0);
119  glVertex3f(
120  cos(ang) * 0.5 * tri_side + xMin + i * x_cell_size +
121  xcomp(j, i),
122  sin(ang) * 0.5 * tri_side + yMin + j * y_cell_size +
123  ycomp(j, i),
124  0);
125  glVertex3f(
126  -cos(ang) * 0.5 * tri_side + xMin + i * x_cell_size +
127  xcomp(j, i),
128  -sin(ang) * 0.5 * tri_side + yMin + j * y_cell_size +
129  ycomp(j, i),
130  0);
131  }
132  glEnd();
133 
135  glEnable(GL_LIGHTING); // Disable lights when drawing lines
136 
137  // End of antialiasing:
138  glPopAttrib();
139 
140 #endif
141 }
142 
143 /*---------------------------------------------------------------
144  Implements the writing to a CStream capability of
145  CSerializable objects
146  ---------------------------------------------------------------*/
147 uint8_t CVectorField2D::serializeGetVersion() const { return 0; }
149 {
151 
152  out << xcomp << ycomp;
153  out << xMin << xMax << yMin << yMax;
154  out << m_LineWidth;
155  out << m_pointSize;
156  out << m_antiAliasing;
157  out << m_point_color;
158  out << m_field_color;
159 }
160 
162  mrpt::serialization::CArchive& in, uint8_t version)
163 {
164  switch (version)
165  {
166  case 0:
168 
169  in >> xcomp >> ycomp;
170  in >> xMin >> xMax >> yMin >> yMax;
171  in >> m_LineWidth;
172  in >> m_pointSize;
173  in >> m_antiAliasing;
174  in >> m_point_color;
175  in >> m_field_color;
176  break;
177 
178  default:
180  break;
181  };
183 }
184 
187 {
188  bb_min.x = xMin;
189  bb_min.y = yMin;
190  bb_min.z = 0;
191 
192  bb_max.x = xMax;
193  bb_max.y = yMax;
194  bb_max.z = 0;
195 
196  const float x_cell_size = (xMax - xMin) / (xcomp.cols() - 1);
197  const float y_cell_size = (yMax - yMin) / (ycomp.rows() - 1);
198 
199  for (int i = 0; i < xcomp.cols(); i++)
200  for (int j = 0; j < xcomp.rows(); j++)
201  {
202  const float tri_side =
203  0.25 *
204  sqrt(xcomp(j, i) * xcomp(j, i) + ycomp(j, i) * ycomp(j, i));
205  const float ang = ::atan2(ycomp(j, i), xcomp(j, i)) - 1.5708;
206 
207  if (-sin(ang) * 0.866 * tri_side + xMin + i * x_cell_size +
208  xcomp(j, i) <
209  bb_min.x)
210  bb_min.x = -sin(ang) * 0.866 * tri_side + xMin +
211  i * x_cell_size + xcomp(j, i);
212 
213  if (cos(ang) * 0.866 * tri_side + yMin + j * y_cell_size +
214  ycomp(j, i) <
215  bb_min.y)
216  bb_min.y = cos(ang) * 0.866 * tri_side + yMin +
217  j * y_cell_size + ycomp(j, i);
218 
219  if (-sin(ang) * 0.866 * tri_side + xMin + i * x_cell_size +
220  xcomp(j, i) >
221  bb_max.x)
222  bb_max.x = -sin(ang) * 0.866 * tri_side + xMin +
223  i * x_cell_size + xcomp(j, i);
224 
225  if (cos(ang) * 0.866 * tri_side + yMin + j * y_cell_size +
226  ycomp(j, i) >
227  bb_max.y)
228  bb_max.y = cos(ang) * 0.866 * tri_side + yMin +
229  j * y_cell_size + ycomp(j, i);
230  }
231 
232  // Convert to coordinates of my parent:
235 }
236 
238 {
239  ASSERT_(xcomp.size() > 0);
240 
241  const float ratio_xp =
242  xcomp.maxCoeff() * (xcomp.cols() - 1) / (xMax - xMin);
243  const float ratio_xn =
244  xcomp.minCoeff() * (xcomp.cols() - 1) / (xMax - xMin);
245  const float ratio_yp =
246  ycomp.maxCoeff() * (ycomp.rows() - 1) / (yMax - yMin);
247  const float ratio_yn =
248  ycomp.minCoeff() * (ycomp.rows() - 1) / (yMax - yMin);
249  const float norm_factor =
250  0.85 / max(max(ratio_xp, abs(ratio_xn)), max(ratio_yp, abs(ratio_yn)));
251 
252  xcomp *= norm_factor;
253  ycomp *= norm_factor;
255 }
Scalar maxCoeff() const
Maximum value in the matrix/vector.
void notifyChange() const
Must be called to notify that the object has changed (so, the display list must be updated) ...
GLAPI void GLAPIENTRY glEnable(GLenum cap)
GLAPI void GLAPIENTRY glPointSize(GLfloat size)
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
mrpt::math::CMatrixF ycomp
Y component of the vector field.
#define GL_TRIANGLES
Definition: glew.h:277
mrpt::img::TColor m_point_color
STL namespace.
uint8_t B
Definition: TColor.h:46
#define GL_ONE_MINUS_SRC_ALPHA
Definition: glew.h:288
uint8_t G
Definition: TColor.h:46
GLAPI void GLAPIENTRY glPopAttrib(void)
#define GL_LIGHTING
Definition: glew.h:386
GLAPI void GLAPIENTRY glLineWidth(GLfloat width)
mrpt::poses::CPose3D m_pose
6D pose wrt the parent coordinate reference.
Definition: CRenderizable.h:54
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
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
#define GL_COLOR_BUFFER_BIT
Definition: glew.h:266
This base provides a set of functions for maths stuff.
mrpt::math::CMatrixF xcomp
X component of the vector field.
void writeToStreamRender(mrpt::serialization::CArchive &out) const
#define GL_LINE_SMOOTH
Definition: glew.h:368
#define GL_POINT_SMOOTH
Definition: glew.h:364
GLAPI void GLAPIENTRY glBegin(GLenum mode)
#define GL_BLEND
Definition: glew.h:433
#define GL_POINTS
Definition: glew.h:273
size_type rows() const
Number of rows in the matrix.
size_type cols() const
Number of columns in the matrix.
A 2D vector field representation, consisting of points and arrows drawn on a plane (invisible grid)...
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)
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...
mrpt::img::TColor m_color
Color components in the range [0,255].
Definition: CRenderizable.h:51
#define GL_SRC_ALPHA
Definition: glew.h:287
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
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.
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
bool m_antiAliasing
By default is true.
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
GLAPI void GLAPIENTRY glPushAttrib(GLbitfield mask)
mrpt::vision::TStereoCalibResults out
matrix_size_t size() const
Get a 2-vector with [NROWS NCOLS] (as in MATLAB command size(x))
GLuint in
Definition: glext.h:7391
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:15
const auto bb_max
void adjustVectorFieldToGrid()
Adjust the vector field in the scene (vectors magnitude) according to the grid size.
GLAPI void GLAPIENTRY glEnd(void)
Scalar minCoeff() const
Minimum value in the matrix/vector.
float m_pointSize
By default is 1.0.
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 render_dl() const override
Render.
#define GL_LINES
Definition: glew.h:274
mrpt::img::TColor m_field_color
Lightweight 3D point.
Definition: TPoint3D.h:90
GLAPI void GLAPIENTRY glDisable(GLenum cap)
This template class provides the basic functionality for a general 2D any-size, resizable container o...
void readFromStreamRender(mrpt::serialization::CArchive &in)
float m_LineWidth
By default is 1.0.
#define GL_LINE_BIT
Definition: glew.h:254
uint8_t A
Definition: TColor.h:46
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ce444d842 Fri Dec 6 19:35:10 2019 +0100 at vie dic 6 19:45:12 CET 2019