MRPT  1.9.9
CVectorField2D.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 
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 */
27  : xcomp(0, 0),
28  ycomp(0, 0),
29  xMin(-1.0),
30  xMax(1.0),
31  yMin(-1.0),
32  yMax(1.0),
33  m_LineWidth(1.0),
34  m_pointSize(1.0),
35  m_antiAliasing(true)
36 {
37  m_point_color = m_color;
38  m_field_color = m_color;
39 }
40 
41 /** Constructor with a initial set of lines. */
43  CMatrixFloat Matrix_x, CMatrixFloat Matrix_y, float xmin, float xmax,
44  float ymin, float ymax)
45  : m_LineWidth(1.0), m_pointSize(1.0), m_antiAliasing(true)
46 {
47  MRPT_UNUSED_PARAM(Matrix_x);
48  MRPT_UNUSED_PARAM(Matrix_y);
49  MRPT_UNUSED_PARAM(xmin);
50  MRPT_UNUSED_PARAM(xmax);
51  MRPT_UNUSED_PARAM(ymin);
52  MRPT_UNUSED_PARAM(ymax);
55 }
56 
57 /*---------------------------------------------------------------
58  render
59  ---------------------------------------------------------------*/
61 {
62 #if MRPT_HAS_OPENGL_GLUT
63 
64  // Enable antialiasing:
66  if (m_antiAliasing || m_color.A != 255)
67  {
70  }
71  if (m_antiAliasing)
72  {
75  }
76 
79 
81 
82  glDisable(GL_LIGHTING); // Disable lights when drawing lines
84  glColor4ub(
86 
87  const float x_cell_size = (xMax - xMin) / (xcomp.cols() - 1);
88  const float y_cell_size = (yMax - yMin) / (ycomp.rows() - 1);
89 
90  for (unsigned int i = 0; i < xcomp.cols(); i++)
91  for (unsigned int j = 0; j < xcomp.rows(); j++)
92  {
93  glVertex3f(xMin + i * x_cell_size, yMin + j * y_cell_size, 0);
94  }
95 
96  glEnd();
97 
99  glColor4ub(
101  for (unsigned int i = 0; i < xcomp.cols(); i++)
102  for (unsigned int j = 0; j < xcomp.rows(); j++)
103  {
104  glVertex3f(xMin + i * x_cell_size, yMin + j * y_cell_size, 0);
105  glVertex3f(
106  xMin + i * x_cell_size + xcomp(j, i),
107  yMin + j * y_cell_size + ycomp(j, i), 0);
108  }
109  glEnd();
110 
112  glColor4ub(
114  for (unsigned int i = 0; i < xcomp.cols(); i++)
115  for (unsigned int j = 0; j < xcomp.rows(); j++)
116  {
117  const float tri_side =
118  0.25 *
119  sqrt(xcomp(j, i) * xcomp(j, i) + ycomp(j, i) * ycomp(j, i));
120  const float ang = ::atan2(ycomp(j, i), xcomp(j, i)) - 1.5708;
121  glVertex3f(
122  -sin(ang) * 0.866 * tri_side + xMin + i * x_cell_size +
123  xcomp(j, i),
124  cos(ang) * 0.866 * tri_side + yMin + j * y_cell_size +
125  ycomp(j, i),
126  0);
127  glVertex3f(
128  cos(ang) * 0.5 * tri_side + xMin + i * x_cell_size +
129  xcomp(j, i),
130  sin(ang) * 0.5 * tri_side + yMin + j * y_cell_size +
131  ycomp(j, i),
132  0);
133  glVertex3f(
134  -cos(ang) * 0.5 * tri_side + xMin + i * x_cell_size +
135  xcomp(j, i),
136  -sin(ang) * 0.5 * tri_side + yMin + j * y_cell_size +
137  ycomp(j, i),
138  0);
139  }
140  glEnd();
141 
143  glEnable(GL_LIGHTING); // Disable lights when drawing lines
144 
145  // End of antialiasing:
146  glPopAttrib();
147 
148 #endif
149 }
150 
151 /*---------------------------------------------------------------
152  Implements the writing to a CStream capability of
153  CSerializable objects
154  ---------------------------------------------------------------*/
157 {
158  writeToStreamRender(out);
159 
160  out << xcomp << ycomp;
161  out << xMin << xMax << yMin << yMax;
162  out << m_LineWidth;
163  out << m_pointSize;
164  out << m_antiAliasing;
165  out << m_point_color;
166  out << m_field_color;
167 }
168 
171 {
172  switch (version)
173  {
174  case 0:
176 
177  in >> xcomp >> ycomp;
178  in >> xMin >> xMax >> yMin >> yMax;
179  in >> m_LineWidth;
180  in >> m_pointSize;
181  in >> m_antiAliasing;
182  in >> m_point_color;
183  in >> m_field_color;
184  break;
185 
186  default:
188  break;
189  };
191 }
192 
194  mrpt::math::TPoint3D& bb_min, mrpt::math::TPoint3D& bb_max) const
195 {
196  bb_min.x = xMin;
197  bb_min.y = yMin;
198  bb_min.z = 0;
199 
200  bb_max.x = xMax;
201  bb_max.y = yMax;
202  bb_max.z = 0;
203 
204  const float x_cell_size = (xMax - xMin) / (xcomp.cols() - 1);
205  const float y_cell_size = (yMax - yMin) / (ycomp.rows() - 1);
206 
207  for (unsigned int i = 0; i < xcomp.cols(); i++)
208  for (unsigned int j = 0; j < xcomp.rows(); j++)
209  {
210  const float tri_side =
211  0.25 *
212  sqrt(xcomp(j, i) * xcomp(j, i) + ycomp(j, i) * ycomp(j, i));
213  const float ang = ::atan2(ycomp(j, i), xcomp(j, i)) - 1.5708;
214 
215  if (-sin(ang) * 0.866 * tri_side + xMin + i * x_cell_size +
216  xcomp(j, i) <
217  bb_min.x)
218  bb_min.x = -sin(ang) * 0.866 * tri_side + xMin +
219  i * x_cell_size + xcomp(j, i);
220 
221  if (cos(ang) * 0.866 * tri_side + yMin + j * y_cell_size +
222  ycomp(j, i) <
223  bb_min.y)
224  bb_min.y = cos(ang) * 0.866 * tri_side + yMin +
225  j * y_cell_size + ycomp(j, i);
226 
227  if (-sin(ang) * 0.866 * tri_side + xMin + i * x_cell_size +
228  xcomp(j, i) >
229  bb_max.x)
230  bb_max.x = -sin(ang) * 0.866 * tri_side + xMin +
231  i * x_cell_size + xcomp(j, i);
232 
233  if (cos(ang) * 0.866 * tri_side + yMin + j * y_cell_size +
234  ycomp(j, i) >
235  bb_max.y)
236  bb_max.y = cos(ang) * 0.866 * tri_side + yMin +
237  j * y_cell_size + ycomp(j, i);
238  }
239 
240  // Convert to coordinates of my parent:
241  m_pose.composePoint(bb_min, bb_min);
242  m_pose.composePoint(bb_max, bb_max);
243 }
244 
246 {
247  ASSERT_(xcomp.size() > 0);
248 
249  const float ratio_xp =
250  xcomp.maxCoeff() * (xcomp.cols() - 1) / (xMax - xMin);
251  const float ratio_xn =
252  xcomp.minCoeff() * (xcomp.cols() - 1) / (xMax - xMin);
253  const float ratio_yp =
254  ycomp.maxCoeff() * (ycomp.rows() - 1) / (yMax - yMin);
255  const float ratio_yn =
256  ycomp.minCoeff() * (ycomp.rows() - 1) / (yMax - yMin);
257  const float norm_factor =
258  0.85 / max(max(ratio_xp, abs(ratio_xn)), max(ratio_yp, abs(ratio_yn)));
259 
260  xcomp *= norm_factor;
261  ycomp *= norm_factor;
263 }
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.
mrpt::math::CMatrix ycomp
Y component of the vector field.
#define GL_TRIANGLES
Definition: glew.h:276
EIGEN_STRONG_INLINE void notifyChange() const
Must be called to notify that the object has changed (so, the display list must be updated) ...
mrpt::img::TColor m_point_color
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
mrpt::math::CMatrix xcomp
X component of the vector field.
GLAPI void GLAPIENTRY glPopAttrib(void)
#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
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
#define GL_COLOR_BUFFER_BIT
Definition: glew.h:265
This base provides a set of functions for maths stuff.
void writeToStreamRender(mrpt::serialization::CArchive &out) const
#define GL_LINE_SMOOTH
Definition: glew.h:367
double x
X,Y,Z coordinates.
#define GL_POINT_SMOOTH
Definition: glew.h:363
GLAPI void GLAPIENTRY glBegin(GLenum mode)
#define GL_BLEND
Definition: glew.h:432
#define GL_POINTS
Definition: glew.h:272
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:52
#define GL_SRC_ALPHA
Definition: glew.h:286
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:52
GLAPI void GLAPIENTRY glPushAttrib(GLbitfield mask)
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
void adjustVectorFieldToGrid()
Adjust the vector field in the scene (vectors magnitude) according to the grid size.
GLAPI void GLAPIENTRY glEnd(void)
float m_pointSize
By default is 1.0.
void render_dl() const override
Render.
#define GL_LINES
Definition: glew.h:273
mrpt::img::TColor m_field_color
Lightweight 3D point.
GLAPI void GLAPIENTRY glDisable(GLenum cap)
void readFromStreamRender(mrpt::serialization::CArchive &in)
float m_LineWidth
By default is 1.0.
#define GL_LINE_BIT
Definition: glew.h:253
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: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020