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-2020, 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 
13 #include <mrpt/opengl/opengl_api.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  m_point_color = m_color;
28  m_field_color = m_color;
29  m_still_color = m_color;
30  m_maxspeed_color = m_color;
31  m_maxspeed = 1.f;
32 }
33 
34 /** Constructor with a initial set of lines. */
36  CMatrixFloat x_vf_ini, CMatrixFloat y_vf_ini, CMatrixFloat z_vf_ini,
37  CMatrixFloat x_p_ini, CMatrixFloat y_p_ini, CMatrixFloat z_p_ini)
38  : m_colorFromModule(false), m_showPoints(true)
39 {
40  x_vf = x_vf_ini;
41  y_vf = y_vf_ini;
42  z_vf = z_vf_ini;
43  x_p = x_p_ini;
44  y_p = y_p_ini;
45  z_p = z_p_ini;
50  m_maxspeed = 1.f;
51 }
52 
54 {
55  switch (rc.shader_id)
56  {
59  break;
62  break;
63  };
64 }
66 {
69 }
70 
72 {
75  vbd.clear();
76  cbd.clear();
77 
78  vbd.reserve(x_vf.size());
79  cbd.reserve(x_vf.size());
80 
81  for (int i = 0; i < x_vf.cols(); i++)
82  for (int j = 0; j < x_vf.rows(); j++)
83  {
84  vbd.emplace_back(x_p(j, i), y_p(j, i), z_p(j, i));
85  vbd.emplace_back(
86  x_p(j, i) + x_vf(j, i), y_p(j, i) + y_vf(j, i),
87  z_p(j, i) + z_vf(j, i));
88 
89  if (!m_colorFromModule)
90  {
91  cbd.emplace_back(m_field_color);
92  }
93  else
94  {
95  // Compute color
97  const float module = sqrt(
98  square(x_vf(j, i)) + square(y_vf(j, i)) +
99  square(z_vf(j, i)));
100  if (module > m_maxspeed)
101  col = m_maxspeed_color;
102  else
103  {
104  const float f = (m_maxspeed - module) / m_maxspeed;
105  const float f2 = module / m_maxspeed;
106  col = mrpt::img::TColorf(
107  f * m_still_color.R + f2 * m_maxspeed_color.R,
108  f * m_still_color.G + f2 * m_maxspeed_color.G,
109  f * m_still_color.B + f2 * m_maxspeed_color.B,
110  f * m_still_color.A + f2 * m_maxspeed_color.A)
111  .asTColor();
112  }
113  cbd.emplace_back(col);
114  }
115  }
116 
117  cbd.assign(vbd.size(), m_field_color);
118 }
119 
121 {
124 
125  vbd.clear();
126  vbd.reserve(x_p.size());
127 
128  for (int i = 0; i < x_p.cols(); i++)
129  for (int j = 0; j < x_p.rows(); j++)
130  vbd.emplace_back(x_p(j, i), y_p(j, i), z_p(j, i));
131 
132  cbd.assign(vbd.size(), m_point_color);
133 }
134 uint8_t CVectorField3D::serializeGetVersion() const { return 0; }
136 {
138 
139  out << x_vf << y_vf << z_vf;
140  out << x_p << y_p << z_p;
141  out << m_lineWidth;
142  out << m_pointSize;
143  out << m_antiAliasing;
144  out << m_point_color;
145  out << m_field_color;
146 }
148  mrpt::serialization::CArchive& in, uint8_t version)
149 {
150  switch (version)
151  {
152  case 0:
154 
155  in >> x_vf >> y_vf >> z_vf;
156  in >> x_p >> y_p >> z_p;
157  in >> m_lineWidth;
158  in >> m_pointSize;
159  in >> m_antiAliasing;
160  in >> m_point_color;
161  in >> m_field_color;
162  break;
163 
164  default:
166  break;
167  };
169 }
170 
173 {
174  bb_min.x = 10e10;
175  bb_min.y = 10e10;
176  bb_min.z = 10e10;
177  bb_max.x = -10e10;
178  bb_max.y = -10e10;
179  bb_max.z = -10e10;
180 
181  for (int i = 0; i < x_p.cols(); i++)
182  for (int j = 0; j < x_p.rows(); j++)
183  {
184  // Minimum values
185  if (x_p(j, i) < bb_min.x) bb_min.x = x_p(j, i);
186 
187  if (x_p(j, i) + x_vf(j, i) < bb_min.x)
188  bb_min.x = x_p(j, i) + x_vf(j, i);
189 
190  if (y_p(j, i) < bb_min.y) bb_min.y = y_p(j, i);
191 
192  if (y_p(j, i) + y_vf(j, i) < bb_min.y)
193  bb_min.y = y_p(j, i) + y_vf(j, i);
194 
195  if (z_p(j, i) < bb_min.z) bb_min.z = z_p(j, i);
196 
197  if (z_p(j, i) + z_vf(j, i) < bb_min.z)
198  bb_min.z = z_p(j, i) + z_vf(j, i);
199 
200  // Maximum values
201  if (x_p(j, i) > bb_max.x) bb_max.x = x_p(j, i);
202 
203  if (x_p(j, i) + x_vf(j, i) > bb_max.x)
204  bb_max.x = x_p(j, i) + x_vf(j, i);
205 
206  if (y_p(j, i) > bb_max.y) bb_max.y = y_p(j, i);
207 
208  if (y_p(j, i) + y_vf(j, i) > bb_max.y)
209  bb_max.y = y_p(j, i) + y_vf(j, i);
210 
211  if (z_p(j, i) > bb_max.z) bb_max.z = z_p(j, i);
212 
213  if (z_p(j, i) + z_vf(j, i) > bb_max.z)
214  bb_max.z = z_p(j, i) + z_vf(j, i);
215  }
216 
217  // Convert to coordinates of my parent:
220 }
std::vector< mrpt::math::TPoint3Df > m_vertex_buffer_data
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
mrpt::img::TColor m_still_color
Color associated to fields with null module.
void notifyChange() const
Call to enable calling renderUpdateBuffers() before the next render() rendering iteration.
void render(const RenderContext &rc) const override
Implements the rendering of 3D objects in each class derived from CRenderizable.
The base class of 3D objects that can be directly rendered through OpenGL.
Definition: CRenderizable.h:48
STL namespace.
mrpt::math::CMatrixF z_vf
Z component of the vector field.
uint8_t B
Definition: TColor.h:51
uint8_t G
Definition: TColor.h:51
float m_maxspeed
Value of the module of the motion field which will correspond to &#39;m_maxspeed_color&#39;.
mrpt::poses::CPose3D m_pose
6D pose wrt the parent coordinate reference.
Definition: CRenderizable.h:62
void renderUpdateBuffers() const override
Called whenever m_outdatedBuffers is true: used to re-generate OpenGL vertex buffers, etc.
Context for calls to render()
std::vector< mrpt::math::TPoint3Df > m_vertex_buffer_data
void onUpdateBuffers_Wireframe() override
Must be implemented in derived classes to update the geometric entities to be drawn in "m_*_buffer" f...
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
bool m_showPoints
By default it is true.
std::vector< mrpt::img::TColor > m_color_buffer_data
This base provides a set of functions for maths stuff.
mrpt::img::TColor m_point_color
void writeToStreamRender(mrpt::serialization::CArchive &out) const
static constexpr shader_id_t WIREFRAME
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.
void renderUpdateBuffers() const override
Called whenever m_outdatedBuffers is true: used to re-generate OpenGL vertex buffers, etc.
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:51
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:59
return_t square(const num_t x)
Inline function for the square of a number.
std::vector< mrpt::img::TColor > m_color_buffer_data
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.
mrpt::vision::TStereoCalibResults out
mrpt::math::CMatrixF x_p
X coordinate of the points at which the vector field is plotted.
matrix_size_t size() const
Get a 2-vector with [NROWS NCOLS] (as in MATLAB command size(x))
An RGBA color - floats in the range [0,1].
Definition: TColor.h:88
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:13
const auto bb_max
void readFromStreamRender(mrpt::serialization::CArchive &in)
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.
TColor asTColor() const
Returns the 0-255 integer version of this color: RGBA_u8.
Definition: TColor.h:101
mrpt::img::TColor m_maxspeed_color
Color associated to fields whose module is equal or larger than &#39;m_maxspeed&#39;.
A RGB color - 8bit.
Definition: TColor.h:25
This template class provides the basic functionality for a general 2D any-size, resizable container o...
void render(const RenderContext &rc) const override
Implements the rendering of 3D objects in each class derived from CRenderizable.
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...
static constexpr shader_id_t POINTS
uint8_t A
Definition: TColor.h:51
void onUpdateBuffers_Points() override
Must be implemented in derived classes to update the geometric entities to be drawn in "m_*_buffer" f...
void renderUpdateBuffers() const override
Called whenever m_outdatedBuffers is true: used to re-generate OpenGL vertex buffers, etc.



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020