Main MRPT website > C++ reference for MRPT 1.5.6
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-2017, 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*()
15 #include <mrpt/utils/CStream.h>
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 using namespace mrpt::utils;
25 using namespace std;
26 
28 
29 CGeneralizedCylinderPtr CGeneralizedCylinder::Create(const std::vector<TPoint3D> &axis,const std::vector<TPoint3D> &generatrix)
30 {
31  return CGeneralizedCylinderPtr(new CGeneralizedCylinder(axis,generatrix));
32 }
33 void CGeneralizedCylinder::TQuadrilateral::calculateNormal() {
34  double ax=points[1].x-points[0].x;
35  double ay=points[1].y-points[0].y;
36  double az=points[1].z-points[0].z;
37  double bx=points[2].x-points[0].x;
38  double by=points[2].y-points[0].y;
39  double bz=points[2].z-points[0].z;
40  normal[0]=az*by-ay*bz;
41  normal[1]=ax*bz-az*bx;
42  normal[2]=ay*bx-ax*by;
43  double s=0;
44  for (size_t i=0;i<3;i++) s+=normal[i]*normal[i];
45  s=sqrt(s);
46  for (size_t i=0;i<3;i++) normal[i]/=s;
47 }
48 
49 #if MRPT_HAS_OPENGL_GLUT
50 class FQuadrilateralRenderer {
51 private:
53 public:
54  void operator()(const CGeneralizedCylinder::TQuadrilateral &t) const {
55  glNormal3d(t.normal[0],t.normal[1],t.normal[2]);
56  for (int i=0;i<4;i++) glVertex3d(t.points[i].x,t.points[i].y,t.points[i].z);
57  }
58  FQuadrilateralRenderer(const mrpt::utils::TColor &c):color(c) {}
59  ~FQuadrilateralRenderer() {}
60 };
61 #endif
62 
63 void CGeneralizedCylinder::getMeshIterators(const vector<TQuadrilateral> &m,vector<TQuadrilateral>::const_iterator &begin,vector<TQuadrilateral>::const_iterator &end) const {
64  if (fullyVisible) {
65  begin=m.begin();
66  end=m.end();
67  } else {
68  size_t qps=m.size()/getNumberOfSections(); //quadrilaterals per section
69  begin=m.begin()+qps*firstSection;
70  end=m.begin()+qps*lastSection;
71  }
72 }
73 
74 void CGeneralizedCylinder::render_dl() const {
75 #if MRPT_HAS_OPENGL_GLUT
76  if (!meshUpToDate) updateMesh();
80 
82  glColor4ub(m_color.R,m_color.G,m_color.B,m_color.A);
84  getMeshIterators(mesh,begin,end);
85  for_each(begin,end,FQuadrilateralRenderer(m_color));
86  glEnd();
87  if (m_color.A!=1.0) glDisable(GL_BLEND);
88 
89 #endif
90 }
91 
92 inline void createMesh(const CMatrixTemplate<TPoint3D> &pointsMesh,size_t R,size_t C,vector<CGeneralizedCylinder::TQuadrilateral> &mesh) {
93  mesh.reserve(R*C);
94  for (size_t i=0;i<R;i++) for (size_t j=0;j<C;j++) mesh.push_back(CGeneralizedCylinder::TQuadrilateral(pointsMesh(i,j),pointsMesh(i,j+1),pointsMesh(i+1,j+1),pointsMesh(i+1,j)));
95 }
96 
97 /*void transformMesh(const CPose3D &pose,const CMatrixTemplate<TPoint3D> &in,CMatrixTemplate<TPoint3D> &out) {
98  size_t R=in.getRowCount();
99  size_t C=in.getColCount();
100  out.setSize(R,C);
101  for (size_t i=0;i<R;i++) for (size_t j=0;j<C;j++) {
102  TPoint3D pIn=in.get_unsafe(i,j);
103  TPoint3D &pOut=out.get_unsafe(i,j);
104  pose.composePoint(pIn.x,pIn.y,pIn.z,pOut.x,pOut.y,pOut.z);
105  }
106 }*/
107 
108 bool CGeneralizedCylinder::traceRay(const CPose3D &o,double &dist) const {
109  if (!meshUpToDate||!polysUpToDate) updatePolys();
110  return math::traceRay(polys,o-this->m_pose,dist);
111 }
112 
113 void CGeneralizedCylinder::updateMesh() const {
114  CRenderizableDisplayList::notifyChange();
115 
116  size_t A=axis.size();
117  vector<TPoint3D> genX=generatrix;
118  if (closed&&genX.size()>2) genX.push_back(genX[0]);
119  size_t G=genX.size();
120  mesh.clear();
121  if (A>1&&G>1) {
122  pointsMesh=CMatrixTemplate<TPoint3D>(A,G);
123  for (size_t i=0;i<A;i++) for (size_t j=0;j<G;j++) axis[i].composePoint(genX[j],pointsMesh.get_unsafe(i,j));
124  createMesh(pointsMesh,A-1,G-1,mesh);
125  }
126  meshUpToDate=true;
127  polysUpToDate=false;
128 }
129 
130 void CGeneralizedCylinder::writeToStream(mrpt::utils::CStream &out,int *version) const {
131  if (version) *version=1;
132  else {
133  writeToStreamRender(out);
134  out<<axis<<generatrix; //In version 0, axis was a vector<TPoint3D>. In version 1, it is a vector<CPose3D>.
135  }
136 }
137 
138 void CGeneralizedCylinder::readFromStream(mrpt::utils::CStream &in,int version) {
139  switch (version) {
140  case 0: {
141  readFromStreamRender(in);
142  vector<TPoint3D> a;
143  in>>a>>generatrix;
144  generatePoses(a,axis);
145  meshUpToDate=false;
146  polysUpToDate=false;
147  break;
148  } case 1:
149  readFromStreamRender(in);
150  //version 0
151  in>>axis>>generatrix;
152  meshUpToDate=false;
153  polysUpToDate=false;
154  break;
155  default:
157  };
158  CRenderizableDisplayList::notifyChange();
159 }
160 
161 void generatePolygon(CPolyhedronPtr &poly,const vector<TPoint3D> &profile,const CPose3D &pose) {
162  math::TPolygon3D p(profile.size());
163  for (size_t i=0;i<profile.size();i++) pose.composePoint(profile[i].x,profile[i].y,profile[i].z,p[i].x,p[i].y,p[i].z);
164  vector<math::TPolygon3D> convexPolys;
165  if (!math::splitInConvexComponents(p,convexPolys)) convexPolys.push_back(p);
166  poly=CPolyhedron::Create(convexPolys);
167 }
168 
169 void CGeneralizedCylinder::getOrigin(CPolyhedronPtr &poly) const {
170  if (!meshUpToDate) updateMesh();
171  if (axis.size()<2||generatrix.size()<3) throw std::logic_error("Not enough points.");
172  size_t i=fullyVisible?0:firstSection;
173  generatePolygon(poly,generatrix,axis[i]);
174  poly->setPose(this->m_pose);
175  poly->setColor(getColor());
176 }
177 
178 void CGeneralizedCylinder::getEnd(CPolyhedronPtr &poly) const {
179  if (!meshUpToDate) updateMesh();
180  if (axis.size()<2||generatrix.size()<3) throw std::logic_error("Not enough points.");
181  size_t i=(fullyVisible?axis.size():lastSection)-1;
182  generatePolygon(poly,generatrix,axis[i]);
183  poly->setPose(this->m_pose);
184  poly->setColor(getColor());
185 }
186 
187 void CGeneralizedCylinder::generateSetOfPolygons(std::vector<TPolygon3D> &res) const {
188  if (!meshUpToDate||!polysUpToDate) updatePolys();
189  size_t N=polys.size();
190  res.resize(N);
191  for (size_t i=0;i<N;i++) res[i]=polys[i].poly;
192 }
193 
194 void CGeneralizedCylinder::getClosedSection(size_t index1,size_t index2,mrpt::opengl::CPolyhedronPtr &poly) const {
195  if (index1>index2) swap(index1,index2);
196  if (index2>=axis.size()-1) throw std::logic_error("Out of range");
197  CMatrixTemplate<TPoint3D> ROIpoints;
198  if (!meshUpToDate) updateMesh();
199  pointsMesh.extractRows(index1,index2+1,ROIpoints);
200  //At this point, ROIpoints contains a matrix of TPoints in which the number of rows equals (index2-index1)+2 and there is a column
201  //for each vertex in the generatrix.
202  if (!closed) {
203  vector<TPoint3D> vec;
204  ROIpoints.extractCol(0,vec);
205  ROIpoints.appendCol(vec);
206  }
207  vector<TPoint3D> vertices;
208  ROIpoints.getAsVector(vertices);
209  size_t nr=ROIpoints.getRowCount()-1;
210  size_t nc=ROIpoints.getColCount()-1;
211  vector<vector<uint32_t> > faces;
212  faces.reserve(nr*nc+2);
213  vector<uint32_t> tmp(4);
214  for (size_t i=0;i<nr;i++) for (size_t j=0;j<nc;j++) {
215  size_t base=(nc+1)*i+j;
216  tmp[0]=base;
217  tmp[1]=base+1;
218  tmp[2]=base+nc+2;
219  tmp[3]=base+nc+1;
220  faces.push_back(tmp);
221  }
222  tmp.resize(nr+1);
223  for (size_t i=0;i<nr+1;i++) tmp[i]=i*(nc+1);
224  faces.push_back(tmp);
225  for (size_t i=0;i<nr+1;i++) tmp[i]=i*(nc+2)-1;
226  poly=CPolyhedron::Create(vertices,faces);
227 }
228 
229 void CGeneralizedCylinder::removeVisibleSectionAtStart() {
230  CRenderizableDisplayList::notifyChange();
231  if (fullyVisible) {
232  if (!getNumberOfSections()) throw std::logic_error("No more sections");
233  fullyVisible=false;
234  firstSection=1;
235  lastSection=getNumberOfSections();
236  } else if (firstSection>=lastSection) throw std::logic_error("No more sections");
237  else firstSection++;
238 }
239 void CGeneralizedCylinder::removeVisibleSectionAtEnd() {
240  CRenderizableDisplayList::notifyChange();
241  if (fullyVisible) {
242  if (!getNumberOfSections()) throw std::logic_error("No more sections");
243  fullyVisible=false;
244  firstSection=0;
245  lastSection=getNumberOfSections()-1;
246  } else if (firstSection>=lastSection) throw std::logic_error("No more sections");
247  else lastSection--;
248 }
249 
250 void CGeneralizedCylinder::updatePolys() const {
251  CRenderizableDisplayList::notifyChange();
252 
253  if (!meshUpToDate) updateMesh();
254  size_t N=mesh.size();
255  polys.resize(N);
256  TPolygon3D tmp(4);
257  for (size_t i=0;i<N;i++) {
258  for (size_t j=0;j<4;j++) tmp[j]=mesh[i].points[j];
259  polys[i]=tmp;
260  }
261  polysUpToDate=true;
262 }
263 
264 void CGeneralizedCylinder::generatePoses(const vector<TPoint3D> &pIn,mrpt::aligned_containers<mrpt::poses::CPose3D>::vector_t &pOut) {
265  size_t N=pIn.size();
266  if (N==0) {
267  pOut.resize(0);
268  return;
269  }
270  vector<double> yaws;
271  yaws.reserve(N);
272  vector<TPoint3D>::const_iterator it1=pIn.begin(),it2;
273  for (;;) if ((it2=it1+1)==pIn.end()) break;
274  else {
275  yaws.push_back(atan2(it2->y-it1->y,it2->x-it1->x));
276  it1=it2;
277  }
278  yaws.push_back(*yaws.rbegin());
279  pOut.resize(N);
280  for (size_t i=0;i<N;i++) {
281  const TPoint3D &p=pIn[i];
282  pOut[i]=CPose3D(p.x,p.y,p.z,yaws[i],0,0);
283  }
284 }
285 
286 bool CGeneralizedCylinder::getFirstSectionPose(CPose3D &p) {
287  if (axis.size()<=0) return false;
288  p=axis[0];
289  return true;
290 }
291 
292 bool CGeneralizedCylinder::getLastSectionPose(CPose3D &p) {
293  if (axis.size()<=0) return false;
294  p=*axis.rbegin();
295  return true;
296 }
297 
298 bool CGeneralizedCylinder::getFirstVisibleSectionPose(CPose3D &p) {
299  if (fullyVisible) return getFirstSectionPose(p);
300  if (getVisibleSections()<=0) return false;
301  p=axis[firstSection];
302  return true;
303 }
304 
305 bool CGeneralizedCylinder::getLastVisibleSectionPose(CPose3D &p) {
306  if (fullyVisible) return getLastSectionPose(p);
307  if (getVisibleSections()<=0) return false;
308  p=axis[lastSection];
309  return true;
310 }
311 
312 void CGeneralizedCylinder::getBoundingBox(mrpt::math::TPoint3D &bb_min, mrpt::math::TPoint3D &bb_max) const
313 {
314  bb_min = TPoint3D(0,0,0);
315  bb_max = TPoint3D(0,0,0);
316 
317  // Convert to coordinates of my parent:
318  m_pose.composePoint(bb_min, bb_min);
319  m_pose.composePoint(bb_max, bb_max);
320 }
GLboolean GLboolean GLboolean GLboolean a
Definition: glew.h:5406
bool BASE_IMPEXP splitInConvexComponents(const TPolygon2D &poly, std::vector< TPolygon2D > &components)
Splits a 2D polygon into convex components.
Definition: geometry.cpp:1861
GLbyte GLbyte bz
Definition: glext.h:5451
GLAPI void GLAPIENTRY glEnable(GLenum cap)
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
bool BASE_IMPEXP traceRay(const std::vector< TPolygonWithPlane > &vec, const mrpt::poses::CPose3D &pose, double &dist)
Fast ray tracing method using polygons' properties.
Definition: geometry.cpp:1996
const GLfloat * c
Definition: glew.h:10088
GLdouble GLdouble t
Definition: glew.h:1303
void extractCol(size_t nCol, std::vector< T > &out, int startingRow=0) const
Returns a given column to a vector (without modifying the matrix) On index out of bounds...
EIGEN_STRONG_INLINE iterator begin()
Definition: eigen_plugins.h:26
This file implements miscelaneous matrix and matrix/vector operations, and internal functions in mrpt...
GLAPI void GLAPIENTRY glVertex3d(GLdouble x, GLdouble y, GLdouble z)
const Scalar * const_iterator
Definition: eigen_plugins.h:24
#define GL_ONE_MINUS_SRC_ALPHA
Definition: glew.h:283
double z
X,Y,Z coordinates.
GLuint in
Definition: glew.h:7146
GLAPI void GLAPIENTRY glBlendFunc(GLenum sfactor, GLenum dfactor)
A renderizable object suitable for rendering with OpenGL's display lists.
mrpt::math::TPoint3D points[4]
Quadrilateral`'s points.
GLdouble s
Definition: glew.h:1295
GLuint GLenum GLsizei GLsizei GLboolean void * points
Definition: glew.h:7748
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
GLuint GLuint end
Definition: glew.h:1167
#define GL_QUADS
Definition: glew.h:275
void getAsVector(std::vector< T > &out) const
Returns a vector containing the matrix's values.
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
GLAPI void GLAPIENTRY glNormal3d(GLdouble nx, GLdouble ny, GLdouble nz)
A RGB color - 8bit.
Definition: TColor.h:26
int version
Definition: mrpt_jpeglib.h:898
GLAPI void GLAPIENTRY glBegin(GLenum mode)
#define GL_BLEND
Definition: glew.h:428
GLfloat GLfloat p
Definition: glew.h:10113
GLuint color
Definition: glew.h:5778
GLAPI void GLAPIENTRY glColor4ub(GLubyte red, GLubyte green, GLubyte blue, GLubyte alpha)
void generatePolygon(CPolyhedronPtr &poly, const vector< TPoint3D > &profile, const CPose3D &pose)
GLuint res
Definition: glew.h:7143
class OPENGL_IMPEXP CGeneralizedCylinder
This template class provides the basic functionality for a general 2D any-size, resizable container o...
#define GL_SRC_ALPHA
Definition: glew.h:282
void composePoint(double lx, double ly, double lz, double &gx, double &gy, double &gz, mrpt::math::CMatrixFixedNumeric< double, 3, 3 > *out_jacobian_df_dpoint=NULL, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dpose=NULL, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dse3=NULL, 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:427
size_t getColCount() const
Number of columns in the matrix.
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:72
void appendCol(const std::vector< T > &in)
Appends a new column to the matrix from a vector.
size_t getRowCount() const
Number of rows in the matrix.
void OPENGL_IMPEXP checkOpenGLError()
Checks glGetError and throws an exception if an error situation is found.
Definition: gl_utils.cpp:134
GLAPI void GLAPIENTRY glEnd(void)
const GLdouble * m
Definition: glew.h:5094
GLbyte by
Definition: glext.h:5451
Lightweight 3D point.
Auxiliary struct holding any quadrilateral, represented by foour points.
GLAPI void GLAPIENTRY glDisable(GLenum cap)
std::vector< TYPE1, Eigen::aligned_allocator< TYPE1 > > vector_t
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.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018