Main MRPT website > C++ reference for MRPT 1.5.7
CSetOfTriangles.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
11 
12 
14 #include "opengl_internals.h"
16 #include <mrpt/utils/CStream.h>
17 
18 using namespace mrpt;
19 using namespace mrpt::opengl;
20 using namespace mrpt::poses;
21 using namespace mrpt::utils;
22 using namespace mrpt::math;
23 using namespace std;
24 
26 
27 
28 /*---------------------------------------------------------------
29  render
30  ---------------------------------------------------------------*/
31 void CSetOfTriangles::render_dl() const
32 {
33 #if MRPT_HAS_OPENGL_GLUT
34 
35  if (m_enableTransparency)
36  {
37  //glDisable(GL_DEPTH_TEST);
40  }
41  else
42  {
45  }
46 
48 
49  glEnable(GL_NORMALIZE); // Normalize normals
51 
52  for (it=m_triangles.begin();it!=m_triangles.end();++it)
53  {
54  // Compute the normal vector:
55  // ---------------------------------
56  float ax= it->x[1] - it->x[0];
57  float ay= it->y[1] - it->y[0];
58  float az= it->z[1] - it->z[0];
59 
60  float bx= it->x[2] - it->x[0];
61  float by= it->y[2] - it->y[0];
62  float bz= it->z[2] - it->z[0];
63 
64  glNormal3f(ay*bz-az*by,-ax*bz+az*bx,ax*by-ay*bx);
65 
66  glColor4f( it->r[0],it->g[0],it->b[0],it->a[0] );
67  glVertex3f(it->x[0],it->y[0],it->z[0]);
68 
69  glColor4f( it->r[1],it->g[1],it->b[1],it->a[1] );
70  glVertex3f(it->x[1],it->y[1],it->z[1]);
71 
72  glColor4f( it->r[2],it->g[2],it->b[2],it->a[2] );
73  glVertex3f(it->x[2],it->y[2],it->z[2]);
74  }
75 
76  glEnd();
78 
80 #endif
81 }
82 
84 {
88 
93 }
95 {
99 
100  i.ReadBufferFixEndianness(t.r,3);
101  i.ReadBufferFixEndianness(t.g,3);
102  i.ReadBufferFixEndianness(t.b,3);
103  i.ReadBufferFixEndianness(t.a,3);
104 }
105 
106 
107 /*---------------------------------------------------------------
108  Implements the writing to a CStream capability of
109  CSerializable objects
110  ---------------------------------------------------------------*/
112 {
113 
114  if (version)
115  *version = 1;
116  else
117  {
118  writeToStreamRender(out);
119  uint32_t n = (uint32_t)m_triangles.size();
120  out << n;
121  for (size_t i=0;i<n;i++)
122  triangle_writeToStream(out,m_triangles[i]);
123 
124  // Version 1:
125  out << m_enableTransparency;
126  }
127 }
128 
129 /*---------------------------------------------------------------
130  Implements the reading from a CStream capability of
131  CSerializable objects
132  ---------------------------------------------------------------*/
134 {
135  switch(version)
136  {
137  case 0:
138  case 1:
139  {
140  readFromStreamRender(in);
141  uint32_t n;
142  in >> n;
143  m_triangles.assign(n,TTriangle());
144  for (size_t i=0;i<n;i++)
145  triangle_readFromStream(in,m_triangles[i]);
146 
147  if (version>=1)
148  in >> m_enableTransparency;
149  else m_enableTransparency = true;
150  } break;
151  default:
153 
154  };
155  polygonsUpToDate=false;
157 }
158 
159 bool CSetOfTriangles::traceRay(const mrpt::poses::CPose3D &o,double &dist) const {
160  if (!polygonsUpToDate) updatePolygons();
161  return mrpt::math::traceRay(tmpPolygons,o-this->m_pose,dist);
162 }
163 
164 //Helper function. Given two 2D points (y1,z1) and (y2,z2), returns three coefficients A, B and C so that both points
165 //verify Ay+Bz+C=0
166 //returns true if the coefficients have actually been calculated
167 /*
168 inline bool lineCoefs(const float &y1,const float &z1,const float &y2,const float &z2,float coefs[3]) {
169  if ((y1==y2)&(z1==z2)) return false; //Both points are the same
170  if (y1==y2) {
171  //Equation is y-y1=0
172  coefs[0]=1;
173  coefs[1]=0;
174  coefs[2]=-y1;
175  return true;
176  } else {
177  //Equation is:
178  // z1 - z2 /z2 - z1 \ .
179  // -------y + z + |-------y1 - z1| = 0
180  // y2 - y1 \y2 - y1 /
181  coefs[0]=(z1-z2)/(y2-y1);
182  coefs[1]=1;
183  coefs[2]=((z2-z1)/(y2-y1))*y1-z1;
184  return true;
185  }
186 }
187 */
188 /*
189 bool CSetOfTriangles::traceRayTriangle(const mrpt::poses::CPose3D &transf,double &dist,const float xb[3],const float yb[3],const float zb[3]) {
190  //Computation of the actual coordinates in the beam's system.
191  float x[3];
192  float y[3];
193  float z[3];
194  for (int i=0;i<3;i++) transf.composePoint(xb[i],yb[i],zb[i],x[i],y[i],z[i]);
195 
196  //If the triangle is parallel to the beam, no collision is posible.
197  //The triangle is parallel to the beam if the projection of the triangle to the YZ plane results in a line.
198  float lCoefs[3];
199  if (!lineCoefs(y[0],z[0],y[1],z[1],lCoefs)) return false;
200  else if (lCoefs[0]*y[2]+lCoefs[1]*z[2]+lCoefs[2]==0) return false;
201  //Basic sign check
202  if (x[0]<0&&x[1]<0&&x[2]<0) return false;
203  if (y[0]<0&&y[1]<0&&y[2]<0) return false;
204  if (z[0]<0&&z[1]<0&&z[2]<0) return false;
205  if (y[0]>0&&y[1]>0&&y[2]>0) return false;
206  if (z[0]>0&&z[1]>0&&z[2]>0) return false;
207  //Let M be the following matrix:
208  // /p1\ /x1 y1 z1\ .
209  //M=|p2|=|x2 y2 z2|
210  // \p3/ \x3 y3 z3/
211  //If M has rank 3, then (p1,p2,p3) conform a plane which does not contain the origin (0,0,0).
212  //If M has rank 2, then (p1,p2,p3) may conform either a line or a plane which contains the origin.
213  //If M has a lesser rank, then (p1,p2,p3) do not conform a plane.
214  //Let N be the following matrix:
215  //N=/p2-p1\ = /x1 y1 z1\ .
216  // \p3-p1/ \x3 y3 z3/
217  //Given that the rank of M is 2, if the rank of N is still 2 then (p1,p2,p3) conform a plane; either, a line.
218  float mat[9];
219  for (int i=0;i<3;i++) {
220  mat[3*i]=x[i];
221  mat[3*i+1]=y[i];
222  mat[3*i+2]=z[i];
223  }
224  CMatrixTemplateNumeric<float> M=CMatrixTemplateNumeric<float>(3,3,mat);
225  float d2=0;
226  float mat2[6];
227  switch (M.rank()) {
228  case 3:
229  //M's rank is 3, so the triangle is inside a plane which doesn't pass through (0,0,0).
230  //This plane's equation is Ax+By+Cz+1=0. Since the point we're searching for verifies y=0 and z=0, we
231  //only need to compute A (x=-1/A). We do this using Cramer's method.
232  for (int i=0;i<9;i+=3) mat[i]=1;
233  d2=(CMatrixTemplateNumeric<float>(3,3,mat)).det();
234  if (d2==0) return false;
235  else dist=M.det()/d2;
236  break;
237  case 2:
238  //if N's rank is 2, the triangle is inside a plane containing (0,0,0).
239  //Otherwise, (p1,p2,p3) don't conform a plane.
240  for (int i=0;i<2;i++) {
241  mat2[3*i]=x[i+1]-x[0];
242  mat2[3*i+1]=y[i+1]-y[0];
243  mat2[3*i+2]=z[i+1]-z[0];
244  }
245  if (CMatrixTemplateNumeric<float>(2,3,mat2).rank()==2) dist=0;
246  else return false;
247  break;
248  default:
249  return false;
250  }
251  if (dist<0) return false;
252  //We've already determined the collision point between the beam and the plane, but we need to check if this
253  //point is actually inside the triangle. We do this by projecting the scene into a <x=constant> plane, so
254  //that the triangle is defined by three 2D lines and the beam is the point (y,z)=(0,0).
255 
256  //For each pair of points, we compute the line that they conform, and then we check if the other point's
257  //sign in that line's equation equals that of the origin (that is, both points are on the same side of the line).
258  //If this holds for each one of the three possible combinations, then the point is inside the triangle.
259  //Furthermore, if any of the three equations verify f(0,0)=0, then the point is in the verge of the line, which
260  //is considered as being inside. Note, whichever is the case, that f(0,0)=lCoefs[2].
261 
262  //lineCoefs already contains the coefficients for the first line.
263  if (lCoefs[2]==0) return true;
264  else if (((lCoefs[0]*y[2]+lCoefs[1]*z[2]+lCoefs[2])>0)!=(lCoefs[2]>0)) return false;
265  lineCoefs(y[0],z[0],y[2],z[2],lCoefs);
266  if (lCoefs[2]==0) return true;
267  else if (((lCoefs[0]*y[1]+lCoefs[1]*z[1]+lCoefs[2])>0)!=(lCoefs[2]>0)) return false;
268  lineCoefs(y[1],z[1],y[2],z[2],lCoefs);
269  if (lCoefs[2]==0) return true;
270  else return ((lCoefs[0]*y[0]+lCoefs[1]*z[0]+lCoefs[2])>0)==(lCoefs[2]>0);
271 }
272 */
273 
276  m_color=c;
277  mrpt::utils::TColorf col(c);
278  for (std::vector<TTriangle>::iterator it=m_triangles.begin();it!=m_triangles.end();++it) for (size_t i=0;i<3;i++) {
279  it->r[i]=col.R;
280  it->g[i]=col.G;
281  it->b[i]=col.B;
282  it->a[i]=col.A;
283  }
284  return *this;
285 }
286 
289  m_color.R=r;
290  const float col = r/255.f;
291  for (std::vector<TTriangle>::iterator it=m_triangles.begin();it!=m_triangles.end();++it) for (size_t i=0;i<3;i++) it->r[i]=col;
292  return *this;
293 }
294 
297  m_color.G=g;
298  const float col = g/255.f;
299  for (std::vector<TTriangle>::iterator it=m_triangles.begin();it!=m_triangles.end();++it) for (size_t i=0;i<3;i++) it->g[i]=col;
300  return *this;
301 }
302 
305  m_color.B=b;
306  const float col = b/255.f;
307  for (std::vector<TTriangle>::iterator it=m_triangles.begin();it!=m_triangles.end();++it) for (size_t i=0;i<3;i++) it->b[i]=col;
308  return *this;
309 }
310 
313  m_color.A=a;
314  const float col = a/255.f;
315  for (std::vector<TTriangle>::iterator it=m_triangles.begin();it!=m_triangles.end();++it) for (size_t i=0;i<3;i++) it->a[i]=col;
316  return *this;
317 }
318 
319 void CSetOfTriangles::getPolygons(std::vector<mrpt::math::TPolygon3D> &polys) const {
320  if (!polygonsUpToDate) updatePolygons();
321  size_t N=tmpPolygons.size();
322  for (size_t i=0;i<N;i++) polys[i]=tmpPolygons[i].poly;
323 }
324 
326  TPolygon3D tmp(3);
327  size_t N=m_triangles.size();
328  tmpPolygons.resize(N);
329  for (size_t i=0;i<N;i++) for (size_t j=0;j<3;j++) {
330  const TTriangle &t=m_triangles[i];
331  tmp[j].x=t.x[j];
332  tmp[j].y=t.y[j];
333  tmp[j].z=t.z[j];
334  tmpPolygons[i]=tmp;
335  }
336  polygonsUpToDate=true;
338 }
339 
341 {
342  bb_min = mrpt::math::TPoint3D(std::numeric_limits<double>::max(),std::numeric_limits<double>::max(), std::numeric_limits<double>::max());
343  bb_max = mrpt::math::TPoint3D(-std::numeric_limits<double>::max(),-std::numeric_limits<double>::max(),-std::numeric_limits<double>::max());
344 
345  for (size_t i=0;i<m_triangles.size();i++)
346  {
347  const TTriangle &t=m_triangles[i];
348 
349  keep_min(bb_min.x, t.x[0]); keep_max(bb_max.x, t.x[0]);
350  keep_min(bb_min.y, t.y[0]); keep_max(bb_max.y, t.y[0]);
351  keep_min(bb_min.z, t.z[0]); keep_max(bb_max.z, t.z[0]);
352 
353  keep_min(bb_min.x, t.x[1]); keep_max(bb_max.x, t.x[1]);
354  keep_min(bb_min.y, t.y[1]); keep_max(bb_max.y, t.y[1]);
355  keep_min(bb_min.z, t.z[1]); keep_max(bb_max.z, t.z[1]);
356 
357  keep_min(bb_min.x, t.x[2]); keep_max(bb_max.x, t.x[2]);
358  keep_min(bb_min.y, t.y[2]); keep_max(bb_max.y, t.y[2]);
359  keep_min(bb_min.z, t.z[2]); keep_max(bb_max.z, t.z[2]);
360  }
361 
362  // Convert to coordinates of my parent:
363  m_pose.composePoint(bb_min, bb_min);
364  m_pose.composePoint(bb_max, bb_max);
365 }
366 
367 void CSetOfTriangles::insertTriangles(const CSetOfTrianglesPtr &p) {
368  reserve(m_triangles.size()+p->m_triangles.size());
369  m_triangles.insert(m_triangles.end(),p->m_triangles.begin(),p->m_triangles.end());
370  polygonsUpToDate=false;
372 }
CRenderizable & setColorB_u8(const uint8_t b) MRPT_OVERRIDE
Color components in the range [0,255].
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
GLdouble GLdouble t
Definition: glext.h:3610
void writeToStream(mrpt::utils::CStream &out, int *getVersion) const
Introduces a pure virtual method responsible for writing to a CStream.
GLbyte GLbyte bz
Definition: glext.h:5451
GLAPI void GLAPIENTRY glEnable(GLenum cap)
void getPolygons(std::vector< mrpt::math::TPolygon3D > &polys) const
Gets the polygon cache.
#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&#39; properties.
Definition: geometry.cpp:1989
GLAPI void GLAPIENTRY glNormal3f(GLfloat nx, GLfloat ny, GLfloat nz)
GLenum GLsizei n
Definition: glext.h:4618
Scalar * iterator
Definition: eigen_plugins.h:23
#define GL_TRIANGLES
Definition: glew.h:272
The base class of 3D objects that can be directly rendered through OpenGL.
Definition: CRenderizable.h:44
EIGEN_STRONG_INLINE void notifyChange() const
Must be called to notify that the object has changed (so, the display list must be updated) ...
STL namespace.
#define GL_NORMALIZE
Definition: glew.h:412
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.
#define GL_DEPTH_TEST
Definition: glew.h:397
static void triangle_readFromStream(mrpt::utils::CStream &i, CSetOfTriangles::TTriangle &t)
GLAPI void GLAPIENTRY glBlendFunc(GLenum sfactor, GLenum dfactor)
unsigned char uint8_t
Definition: rptypes.h:43
A renderizable object suitable for rendering with OpenGL&#39;s display lists.
void WriteBufferFixEndianness(const T *ptr, size_t ElementCount)
Writes a sequence of elemental datatypes, taking care of reordering their bytes from the running arch...
Definition: CStream.h:139
static void triangle_writeToStream(mrpt::utils::CStream &o, const CSetOfTriangles::TTriangle &t)
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
const GLubyte * c
Definition: glext.h:5590
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
CRenderizable & setColorA_u8(const uint8_t a) MRPT_OVERRIDE
Color components in the range [0,255].
GLubyte g
Definition: glext.h:5575
A RGB color - 8bit.
Definition: TColor.h:26
GLubyte GLubyte b
Definition: glext.h:5575
int version
Definition: mrpt_jpeglib.h:898
GLAPI void GLAPIENTRY glBegin(GLenum mode)
#define GL_BLEND
Definition: glew.h:428
void getBoundingBox(mrpt::math::TPoint3D &bb_min, mrpt::math::TPoint3D &bb_max) const MRPT_OVERRIDE
Evaluates the bounding box of this object (including possible children) in the coordinate frame of th...
CRenderizable & setColorR_u8(const uint8_t r) MRPT_OVERRIDE
Color components in the range [0,255].
void keep_max(T &var, const K test_val)
If the second argument is above the first one, set the first argument to this higher value...
Definition: bits.h:176
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
GLAPI void GLAPIENTRY glVertex3f(GLfloat x, GLfloat y, GLfloat z)
bool traceRay(const mrpt::poses::CPose3D &o, double &dist) const MRPT_OVERRIDE
Ray tracing.
void updatePolygons() const
Polygon cache updating.
#define GL_SRC_ALPHA
Definition: glew.h:282
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
GLdouble GLdouble GLdouble r
Definition: glext.h:3618
void readFromStream(mrpt::utils::CStream &in, int version)
Introduces a pure virtual method responsible for loading from a CStream This can not be used directly...
void keep_min(T &var, const K test_val)
If the second argument is below the first one, set the first argument to this lower value...
Definition: bits.h:171
size_t ReadBufferFixEndianness(T *ptr, size_t ElementCount)
Reads a sequence of elemental datatypes, taking care of reordering their bytes from the MRPT stream s...
Definition: CStream.h:95
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
GLuint in
Definition: glext.h:6301
The namespace for 3D scene representation and rendering.
A RGB color - floats in the range [0,1].
Definition: TColor.h:80
GLAPI void GLAPIENTRY glEnd(void)
GLbyte by
Definition: glext.h:5451
A set of colored triangles.
typedef void(APIENTRYP PFNGLBLENDCOLORPROC)(GLclampf red
GLAPI void GLAPIENTRY glColor4f(GLfloat red, GLfloat green, GLfloat blue, GLfloat alpha)
Lightweight 3D point.
unsigned __int32 uint32_t
Definition: rptypes.h:49
GLAPI void GLAPIENTRY glDisable(GLenum cap)
GLubyte GLubyte GLubyte a
Definition: glext.h:5575
GLfloat GLfloat p
Definition: glext.h:5587
void insertTriangles(const InputIterator &begin, const InputIterator &end)
Inserts a set of triangles, bounded by iterators, into this set.
CRenderizable & setColor_u8(const mrpt::utils::TColor &c) MRPT_OVERRIDE
Changes the default object color.
3D polygon, inheriting from std::vector<TPoint3D>
CRenderizable & setColorG_u8(const uint8_t g) MRPT_OVERRIDE
Color components in the range [0,255].



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019