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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019