MRPT  1.9.9
CGeneralizedCylinder.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 
12 #include <mrpt/math/geometry.h>
13 #include <mrpt/math/ops_matrices.h> // for extract*()
15 #include <mrpt/poses/CPose3D.h>
18 #include <Eigen/Dense>
19 #include "opengl_internals.h"
20 
21 using namespace mrpt;
22 using namespace mrpt::math;
23 using namespace mrpt::opengl;
24 using namespace mrpt::poses;
25 
26 using namespace std;
27 
30 
31 void CGeneralizedCylinder::TQuadrilateral::calculateNormal()
32 {
33  double ax = points[1].x - points[0].x;
34  double ay = points[1].y - points[0].y;
35  double az = points[1].z - points[0].z;
36  double bx = points[2].x - points[0].x;
37  double by = points[2].y - points[0].y;
38  double bz = points[2].z - points[0].z;
39  normal[0] = az * by - ay * bz;
40  normal[1] = ax * bz - az * bx;
41  normal[2] = ay * bx - ax * by;
42  double s = 0;
43  for (double i : normal) s += i * i;
44  s = sqrt(s);
45  for (double& i : normal) i /= s;
46 }
47 
48 #if MRPT_HAS_OPENGL_GLUT
49 class FQuadrilateralRenderer
50 {
51  private:
52  const mrpt::img::TColor& color;
53 
54  public:
55  void operator()(const CGeneralizedCylinder::TQuadrilateral& t) const
56  {
57  glNormal3d(t.normal[0], t.normal[1], t.normal[2]);
58  for (const auto& point : t.points)
59  glVertex3d(point.x, point.y, point.z);
60  }
61  FQuadrilateralRenderer(const mrpt::img::TColor& c) : color(c) {}
62  ~FQuadrilateralRenderer() = default;
63 };
64 #endif
65 
66 void CGeneralizedCylinder::getMeshIterators(
67  const vector<TQuadrilateral>& m,
68  vector<TQuadrilateral>::const_iterator& begin,
69  vector<TQuadrilateral>::const_iterator& end) const
70 {
71  if (fullyVisible)
72  {
73  begin = m.begin();
74  end = m.end();
75  }
76  else
77  {
78  ASSERT_(getNumberOfSections() > 0);
79  size_t qps =
80  m.size() / getNumberOfSections(); // quadrilaterals per section
81  begin = m.begin() + qps * firstSection;
82  end = m.begin() + qps * lastSection;
83  }
84 }
85 
86 void CGeneralizedCylinder::render_dl() const
87 {
88 #if MRPT_HAS_OPENGL_GLUT
89  if (!meshUpToDate) updateMesh();
93 
95  glColor4ub(m_color.R, m_color.G, m_color.B, m_color.A);
96  vector<TQuadrilateral>::const_iterator begin, end;
97  getMeshIterators(mesh, begin, end);
98  for_each(begin, end, FQuadrilateralRenderer(m_color));
99  glEnd();
100  if (m_color.A != 1.0) glDisable(GL_BLEND);
101 
102 #endif
103 }
104 
105 inline void createMesh(
106  const CMatrixDynamic<TPoint3D_data>& pointsMesh, size_t R, size_t C,
107  vector<CGeneralizedCylinder::TQuadrilateral>& mesh)
108 {
109  mesh.reserve(R * C);
110  for (size_t i = 0; i < R; i++)
111  for (size_t j = 0; j < C; j++)
112  mesh.emplace_back(
113  pointsMesh(i, j), pointsMesh(i, j + 1),
114  pointsMesh(i + 1, j + 1), pointsMesh(i + 1, j));
115 }
116 
117 bool CGeneralizedCylinder::traceRay(const CPose3D& o, double& dist) const
118 {
119  if (!meshUpToDate || !polysUpToDate) updatePolys();
120  return math::traceRay(polys, (o - this->m_pose).asTPose(), dist);
121 }
122 
123 void CGeneralizedCylinder::updateMesh() const
124 {
125  CRenderizableDisplayList::notifyChange();
126 
127  size_t A = axis.size();
128  vector<TPoint3D> genX = generatrix;
129  if (closed && genX.size() > 2) genX.push_back(genX[0]);
130  size_t G = genX.size();
131  mesh.clear();
132  if (A > 1 && G > 1)
133  {
134  pointsMesh = CMatrixDynamic<TPoint3D_data>(A, G);
135  for (size_t i = 0; i < A; i++)
136  for (size_t j = 0; j < G; j++)
137  pointsMesh(i, j) = axis[i].composePoint(genX[j]);
138  createMesh(pointsMesh, A - 1, G - 1, mesh);
139  }
140  meshUpToDate = true;
141  polysUpToDate = false;
142 }
143 
144 uint8_t CGeneralizedCylinder::serializeGetVersion() const { return 1; }
145 void CGeneralizedCylinder::serializeTo(mrpt::serialization::CArchive& out) const
146 {
147  writeToStreamRender(out);
148  out << axis << generatrix; // In version 0, axis was a vector<TPoint3D>. In
149  // version 1, it is a vector<CPose3D>.
150 }
151 
152 void CGeneralizedCylinder::serializeFrom(
153  mrpt::serialization::CArchive& in, uint8_t version)
154 {
155  switch (version)
156  {
157  case 0:
158  {
159  readFromStreamRender(in);
160  vector<TPoint3D> a;
161  in >> a >> generatrix;
162  generatePoses(a, axis);
163  meshUpToDate = false;
164  polysUpToDate = false;
165  break;
166  }
167  case 1:
168  readFromStreamRender(in);
169  // version 0
170  in >> axis >> generatrix;
171  meshUpToDate = false;
172  polysUpToDate = false;
173  break;
174  default:
176  };
177  CRenderizableDisplayList::notifyChange();
178 }
179 
181  CPolyhedron::Ptr& poly, const vector<TPoint3D>& profile,
182  const CPose3D& pose)
183 {
184  math::TPolygon3D p(profile.size());
185  for (size_t i = 0; i < profile.size(); i++)
186  pose.composePoint(
187  profile[i].x, profile[i].y, profile[i].z, p[i].x, p[i].y, p[i].z);
188  vector<math::TPolygon3D> convexPolys;
189  if (!math::splitInConvexComponents(p, convexPolys))
190  convexPolys.push_back(p);
191  poly = std::make_shared<CPolyhedron>(convexPolys);
192 }
193 
194 void CGeneralizedCylinder::getOrigin(CPolyhedron::Ptr& poly) const
195 {
196  if (!meshUpToDate) updateMesh();
197  if (axis.size() < 2 || generatrix.size() < 3)
198  throw std::logic_error("Not enough points.");
199  size_t i = fullyVisible ? 0 : firstSection;
200  generatePolygon(poly, generatrix, axis[i]);
201  poly->setPose(this->m_pose);
202  poly->setColor(getColor());
203 }
204 
205 void CGeneralizedCylinder::getEnd(CPolyhedron::Ptr& poly) const
206 {
207  if (!meshUpToDate) updateMesh();
208  if (axis.size() < 2 || generatrix.size() < 3)
209  throw std::logic_error("Not enough points.");
210  size_t i = (fullyVisible ? axis.size() : lastSection) - 1;
211  generatePolygon(poly, generatrix, axis[i]);
212  poly->setPose(this->m_pose);
213  poly->setColor(getColor());
214 }
215 
216 void CGeneralizedCylinder::generateSetOfPolygons(
217  std::vector<TPolygon3D>& res) const
218 {
219  if (!meshUpToDate || !polysUpToDate) updatePolys();
220  size_t N = polys.size();
221  res.resize(N);
222  for (size_t i = 0; i < N; i++) res[i] = polys[i].poly;
223 }
224 
225 void CGeneralizedCylinder::getClosedSection(
226  size_t index1, size_t index2, mrpt::opengl::CPolyhedron::Ptr& poly) const
227 {
228  if (index1 > index2) swap(index1, index2);
229  if (index2 >= axis.size() - 1) throw std::logic_error("Out of range");
230  if (!meshUpToDate) updateMesh();
231  auto ROIpoints = CMatrixDynamic<TPoint3D_data>(
232  pointsMesh.asEigen().block(index1, 0, index2 + 1, pointsMesh.cols()));
233 
234  // At this point, ROIpoints contains a matrix of TPoints in which the number
235  // of rows equals (index2-index1)+2 and there is a column
236  // for each vertex in the generatrix.
237  if (!closed)
238  {
239  CVectorDynamic<TPoint3D_data> vec(ROIpoints.rows());
240  vec.asEigen() = ROIpoints.col(0);
241  ROIpoints.appendCol(vec);
242  }
243  vector<TPoint3D> vertices;
244  ROIpoints.asVector(vertices);
245  size_t nr = ROIpoints.rows() - 1;
246  size_t nc = ROIpoints.cols() - 1;
247  vector<vector<uint32_t>> faces;
248  faces.reserve(nr * nc + 2);
249  vector<uint32_t> tmp(4);
250  for (size_t i = 0; i < nr; i++)
251  for (size_t j = 0; j < nc; j++)
252  {
253  size_t base = (nc + 1) * i + j;
254  tmp[0] = base;
255  tmp[1] = base + 1;
256  tmp[2] = base + nc + 2;
257  tmp[3] = base + nc + 1;
258  faces.push_back(tmp);
259  }
260  tmp.resize(nr + 1);
261  for (size_t i = 0; i < nr + 1; i++) tmp[i] = i * (nc + 1);
262  faces.push_back(tmp);
263  for (size_t i = 0; i < nr + 1; i++) tmp[i] = i * (nc + 2) - 1;
264  poly = CPolyhedron::Create(vertices, faces);
265 }
266 
267 void CGeneralizedCylinder::removeVisibleSectionAtStart()
268 {
269  CRenderizableDisplayList::notifyChange();
270  if (fullyVisible)
271  {
272  if (!getNumberOfSections()) throw std::logic_error("No more sections");
273  fullyVisible = false;
274  firstSection = 1;
275  lastSection = getNumberOfSections();
276  }
277  else if (firstSection >= lastSection)
278  throw std::logic_error("No more sections");
279  else
280  firstSection++;
281 }
282 void CGeneralizedCylinder::removeVisibleSectionAtEnd()
283 {
284  CRenderizableDisplayList::notifyChange();
285  if (fullyVisible)
286  {
287  if (!getNumberOfSections()) throw std::logic_error("No more sections");
288  fullyVisible = false;
289  firstSection = 0;
290  lastSection = getNumberOfSections() - 1;
291  }
292  else if (firstSection >= lastSection)
293  throw std::logic_error("No more sections");
294  else
295  lastSection--;
296 }
297 
298 void CGeneralizedCylinder::updatePolys() const
299 {
300  CRenderizableDisplayList::notifyChange();
301 
302  if (!meshUpToDate) updateMesh();
303  size_t N = mesh.size();
304  polys.resize(N);
305  TPolygon3D tmp(4);
306  for (size_t i = 0; i < N; i++)
307  {
308  for (size_t j = 0; j < 4; j++) tmp[j] = mesh[i].points[j];
309  polys[i] = tmp;
310  }
311  polysUpToDate = true;
312 }
313 
314 void CGeneralizedCylinder::generatePoses(
315  const vector<TPoint3D>& pIn, std::vector<mrpt::poses::CPose3D>& pOut)
316 {
317  size_t N = pIn.size();
318  if (N == 0)
319  {
320  pOut.resize(0);
321  return;
322  }
323  vector<double> yaws;
324  yaws.reserve(N);
325  vector<TPoint3D>::const_iterator it1 = pIn.begin(), it2;
326  for (;;)
327  if ((it2 = it1 + 1) == pIn.end())
328  break;
329  else
330  {
331  yaws.push_back(atan2(it2->y - it1->y, it2->x - it1->x));
332  it1 = it2;
333  }
334  yaws.push_back(*yaws.rbegin());
335  pOut.resize(N);
336  for (size_t i = 0; i < N; i++)
337  {
338  const TPoint3D& p = pIn[i];
339  pOut[i] = CPose3D(p.x, p.y, p.z, yaws[i], 0, 0);
340  }
341 }
342 
343 bool CGeneralizedCylinder::getFirstSectionPose(CPose3D& p)
344 {
345  if (axis.size() <= 0) return false;
346  p = axis[0];
347  return true;
348 }
349 
350 bool CGeneralizedCylinder::getLastSectionPose(CPose3D& p)
351 {
352  if (axis.size() <= 0) return false;
353  p = *axis.rbegin();
354  return true;
355 }
356 
357 bool CGeneralizedCylinder::getFirstVisibleSectionPose(CPose3D& p)
358 {
359  if (fullyVisible) return getFirstSectionPose(p);
360  if (getVisibleSections() <= 0) return false;
361  p = axis[firstSection];
362  return true;
363 }
364 
365 bool CGeneralizedCylinder::getLastVisibleSectionPose(CPose3D& p)
366 {
367  if (fullyVisible) return getLastSectionPose(p);
368  if (getVisibleSections() <= 0) return false;
369  p = axis[lastSection];
370  return true;
371 }
372 
373 void CGeneralizedCylinder::getBoundingBox(
375 {
376  bb_min = TPoint3D(0, 0, 0);
377  bb_max = TPoint3D(0, 0, 0);
378 
379  // Convert to coordinates of my parent:
380  m_pose.composePoint(bb_min, bb_min);
381  m_pose.composePoint(bb_max, bb_max);
382 }
GLdouble GLdouble t
Definition: glext.h:3695
bool splitInConvexComponents(const TPolygon2D &poly, std::vector< TPolygon2D > &components)
Splits a 2D polygon into convex components.
Definition: geometry.cpp:2376
GLbyte GLbyte bz
Definition: glext.h:6193
GLAPI void GLAPIENTRY glEnable(GLenum cap)
Template for column vectors of dynamic size, compatible with Eigen.
bool traceRay(const std::vector< TPolygonWithPlane > &vec, const mrpt::math::TPose3D &pose, double &dist)
Fast ray tracing method using polygons&#39; properties.
Definition: geometry.cpp:2569
const double G
This file implements miscelaneous matrix and matrix/vector operations, and internal functions in mrpt...
GLAPI void GLAPIENTRY glVertex3d(GLdouble x, GLdouble y, GLdouble z)
STL namespace.
#define GL_ONE_MINUS_SRC_ALPHA
Definition: glew.h:288
GLdouble s
Definition: glext.h:3682
GLAPI void GLAPIENTRY glBlendFunc(GLenum sfactor, GLenum dfactor)
GLsizei const GLfloat * points
Definition: glext.h:5414
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
GLuint color
Definition: glext.h:8459
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
This base provides a set of functions for maths stuff.
#define GL_QUADS
Definition: glew.h:280
const GLubyte * c
Definition: glext.h:6406
void createMesh(const CMatrixDynamic< TPoint3D_data > &pointsMesh, size_t R, size_t C, vector< CGeneralizedCylinder::TQuadrilateral > &mesh)
GLuint GLuint end
Definition: glext.h:3532
GLAPI void GLAPIENTRY glNormal3d(GLdouble nx, GLdouble ny, GLdouble nz)
void generatePolygon(CPolyhedron::Ptr &poly, const vector< TPoint3D > &profile, const CPose3D &pose)
GLAPI void GLAPIENTRY glBegin(GLenum mode)
IMPLEMENTS_SERIALIZABLE(CGeneralizedCylinder, CRenderizableDisplayList, mrpt::opengl) void CGeneralizedCylinder
#define GL_BLEND
Definition: glew.h:433
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
GLAPI void GLAPIENTRY glColor4ub(GLubyte red, GLubyte green, GLubyte blue, GLubyte alpha)
#define GL_SRC_ALPHA
Definition: glew.h:287
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
const_iterator begin() const
Definition: ts_hash_map.h:240
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
This object represents any figure obtained by extruding any profile along a given axis...
const float R
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::vision::TStereoCalibResults out
void checkOpenGLError()
Checks glGetError and throws an exception if an error situation is found.
Definition: gl_utils.cpp:155
GLuint in
Definition: glext.h:7391
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:15
const auto bb_max
GLAPI void GLAPIENTRY glEnd(void)
GLbyte by
Definition: glext.h:6193
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
EIGEN_MAP asEigen()
Get as an Eigen-compatible Eigen::Map object.
GLuint res
Definition: glext.h:7385
A RGB color - 8bit.
Definition: TColor.h:20
Lightweight 3D point.
Definition: TPoint3D.h:90
Auxiliary struct holding any quadrilateral, represented by foour points.
GLAPI void GLAPIENTRY glDisable(GLenum cap)
This template class provides the basic functionality for a general 2D any-size, resizable container o...
GLubyte GLubyte GLubyte a
Definition: glext.h:6372
GLfloat GLfloat p
Definition: glext.h:6398
3D polygon, inheriting from std::vector<TPoint3D>
Definition: TPolygon3D.h:18



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 9b18308f3 Mon Nov 18 23:39:25 2019 +0100 at lun nov 18 23:45:12 CET 2019