MRPT  1.9.9
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-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
11 
13 #include "opengl_internals.h"
16 
17 using namespace mrpt;
18 using namespace mrpt::opengl;
19 using namespace mrpt::poses;
20 
21 using namespace mrpt::math;
22 using namespace std;
23 
25 
26 /*---------------------------------------------------------------
27  render
28  ---------------------------------------------------------------*/
29 void CSetOfTriangles::render_dl() const
30 {
31 #if MRPT_HAS_OPENGL_GLUT
32 
33  if (m_enableTransparency)
34  {
35  // glDisable(GL_DEPTH_TEST);
38  }
39  else
40  {
43  }
44 
46 
47  glEnable(GL_NORMALIZE); // Normalize normals
49 
50  for (it = m_triangles.begin(); it != m_triangles.end(); ++it)
51  {
52  // Compute the normal vector:
53  // ---------------------------------
54  float ax = it->x[1] - it->x[0];
55  float ay = it->y[1] - it->y[0];
56  float az = it->z[1] - it->z[0];
57 
58  float bx = it->x[2] - it->x[0];
59  float by = it->y[2] - it->y[0];
60  float bz = it->z[2] - it->z[0];
61 
62  glNormal3f(ay * bz - az * by, -ax * bz + az * bx, ax * by - ay * bx);
63 
64  glColor4f(it->r[0], it->g[0], it->b[0], it->a[0]);
65  glVertex3f(it->x[0], it->y[0], it->z[0]);
66 
67  glColor4f(it->r[1], it->g[1], it->b[1], it->a[1]);
68  glVertex3f(it->x[1], it->y[1], it->z[1]);
69 
70  glColor4f(it->r[2], it->g[2], it->b[2], it->a[2]);
71  glVertex3f(it->x[2], it->y[2], it->z[2]);
72  }
73 
74  glEnd();
76 
78 #endif
79 }
80 
83 {
87 
92 }
95 {
96  i.ReadBufferFixEndianness(t.x, 3);
97  i.ReadBufferFixEndianness(t.y, 3);
98  i.ReadBufferFixEndianness(t.z, 3);
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 
108 {
109  writeToStreamRender(out);
110  uint32_t n = (uint32_t)m_triangles.size();
111  out << n;
112  for (size_t i = 0; i < n; i++) triangle_writeToStream(out, m_triangles[i]);
113 
114  // Version 1:
115  out << m_enableTransparency;
116 }
119 {
120  switch (version)
121  {
122  case 0:
123  case 1:
124  {
125  readFromStreamRender(in);
126  uint32_t n;
127  in >> n;
128  m_triangles.assign(n, TTriangle());
129  for (size_t i = 0; i < n; i++)
130  triangle_readFromStream(in, m_triangles[i]);
131 
132  if (version >= 1)
133  in >> m_enableTransparency;
134  else
135  m_enableTransparency = true;
136  }
137  break;
138  default:
140  };
141  polygonsUpToDate = false;
143 }
144 
146  const mrpt::poses::CPose3D& o, double& dist) const
147 {
148  if (!polygonsUpToDate) updatePolygons();
149  return mrpt::math::traceRay(
150  tmpPolygons, (o - this->m_pose).asTPose(), dist);
151 }
152 
153 // Helper function. Given two 2D points (y1,z1) and (y2,z2), returns three
154 // coefficients A, B and C so that both points
155 // verify Ay+Bz+C=0
156 // returns true if the coefficients have actually been calculated
157 /*
158 inline bool lineCoefs(const float &y1,const float &z1,const float &y2,const
159 float &z2,float coefs[3]) {
160  if ((y1==y2)&(z1==z2)) return false; //Both points are the same
161  if (y1==y2) {
162  //Equation is y-y1=0
163  coefs[0]=1;
164  coefs[1]=0;
165  coefs[2]=-y1;
166  return true;
167  } else {
168  //Equation is:
169  // z1 - z2 /z2 - z1 \ .
170  // -------y + z + |-------y1 - z1| = 0
171  // y2 - y1 \y2 - y1 /
172  coefs[0]=(z1-z2)/(y2-y1);
173  coefs[1]=1;
174  coefs[2]=((z2-z1)/(y2-y1))*y1-z1;
175  return true;
176  }
177 }
178 */
179 /*
180 bool CSetOfTriangles::traceRayTriangle(const mrpt::poses::CPose3D &transf,double
181 &dist,const float xb[3],const float yb[3],const float zb[3]) {
182  //Computation of the actual coordinates in the beam's system.
183  float x[3];
184  float y[3];
185  float z[3];
186  for (int i=0;i<3;i++) transf.composePoint(xb[i],yb[i],zb[i],x[i],y[i],z[i]);
187 
188  //If the triangle is parallel to the beam, no collision is posible.
189  //The triangle is parallel to the beam if the projection of the triangle to
190 the YZ plane results in a line.
191  float lCoefs[3];
192  if (!lineCoefs(y[0],z[0],y[1],z[1],lCoefs)) return false;
193  else if (lCoefs[0]*y[2]+lCoefs[1]*z[2]+lCoefs[2]==0) return false;
194  //Basic sign check
195  if (x[0]<0&&x[1]<0&&x[2]<0) return false;
196  if (y[0]<0&&y[1]<0&&y[2]<0) return false;
197  if (z[0]<0&&z[1]<0&&z[2]<0) return false;
198  if (y[0]>0&&y[1]>0&&y[2]>0) return false;
199  if (z[0]>0&&z[1]>0&&z[2]>0) return false;
200  //Let M be the following matrix:
201  // /p1\ /x1 y1 z1\ .
202  //M=|p2|=|x2 y2 z2|
203  // \p3/ \x3 y3 z3/
204  //If M has rank 3, then (p1,p2,p3) conform a plane which does not contain
205 the origin (0,0,0).
206  //If M has rank 2, then (p1,p2,p3) may conform either a line or a plane
207 which contains the origin.
208  //If M has a lesser rank, then (p1,p2,p3) do not conform a plane.
209  //Let N be the following matrix:
210  //N=/p2-p1\ = /x1 y1 z1\ .
211  // \p3-p1/ \x3 y3 z3/
212  //Given that the rank of M is 2, if the rank of N is still 2 then (p1,p2,p3)
213 conform a plane; either, a line.
214  float mat[9];
215  for (int i=0;i<3;i++) {
216  mat[3*i]=x[i];
217  mat[3*i+1]=y[i];
218  mat[3*i+2]=z[i];
219  }
220  CMatrixTemplateNumeric<float> M=CMatrixTemplateNumeric<float>(3,3,mat);
221  float d2=0;
222  float mat2[6];
223  switch (M.rank()) {
224  case 3:
225  //M's rank is 3, so the triangle is inside a plane which doesn't
226 pass through (0,0,0).
227  //This plane's equation is Ax+By+Cz+1=0. Since the point we're
228 searching for verifies y=0 and z=0, we
229  //only need to compute A (x=-1/A). We do this using Cramer's method.
230  for (int i=0;i<9;i+=3) mat[i]=1;
231  d2=(CMatrixTemplateNumeric<float>(3,3,mat)).det();
232  if (d2==0) return false;
233  else dist=M.det()/d2;
234  break;
235  case 2:
236  //if N's rank is 2, the triangle is inside a plane containing
237 (0,0,0).
238  //Otherwise, (p1,p2,p3) don't conform a plane.
239  for (int i=0;i<2;i++) {
240  mat2[3*i]=x[i+1]-x[0];
241  mat2[3*i+1]=y[i+1]-y[0];
242  mat2[3*i+2]=z[i+1]-z[0];
243  }
244  if (CMatrixTemplateNumeric<float>(2,3,mat2).rank()==2) dist=0;
245  else return false;
246  break;
247  default:
248  return false;
249  }
250  if (dist<0) return false;
251  //We've already determined the collision point between the beam and the
252 plane, but we need to check if this
253  //point is actually inside the triangle. We do this by projecting the scene
254 into a <x=constant> plane, so
255  //that the triangle is defined by three 2D lines and the beam is the point
256 (y,z)=(0,0).
257 
258  //For each pair of points, we compute the line that they conform, and then
259 we check if the other point's
260  //sign in that line's equation equals that of the origin (that is, both
261 points are on the same side of the line).
262  //If this holds for each one of the three possible combinations, then the
263 point is inside the triangle.
264  //Furthermore, if any of the three equations verify f(0,0)=0, then the point
265 is in the verge of the line, which
266  //is considered as being inside. Note, whichever is the case, that
267 f(0,0)=lCoefs[2].
268 
269  //lineCoefs already contains the coefficients for the first line.
270  if (lCoefs[2]==0) return true;
271  else if (((lCoefs[0]*y[2]+lCoefs[1]*z[2]+lCoefs[2])>0)!=(lCoefs[2]>0))
272 return false;
273  lineCoefs(y[0],z[0],y[2],z[2],lCoefs);
274  if (lCoefs[2]==0) return true;
275  else if (((lCoefs[0]*y[1]+lCoefs[1]*z[1]+lCoefs[2])>0)!=(lCoefs[2]>0))
276 return false;
277  lineCoefs(y[1],z[1],y[2],z[2],lCoefs);
278  if (lCoefs[2]==0) return true;
279  else return ((lCoefs[0]*y[0]+lCoefs[1]*z[0]+lCoefs[2])>0)==(lCoefs[2]>0);
280 }
281 */
282 
284 {
286  m_color = c;
287  mrpt::img::TColorf col(c);
288  for (std::vector<TTriangle>::iterator it = m_triangles.begin();
289  it != m_triangles.end(); ++it)
290  for (size_t i = 0; i < 3; i++)
291  {
292  it->r[i] = col.R;
293  it->g[i] = col.G;
294  it->b[i] = col.B;
295  it->a[i] = col.A;
296  }
297  return *this;
298 }
299 
301 {
303  m_color.R = r;
304  const float col = r / 255.f;
305  for (std::vector<TTriangle>::iterator it = m_triangles.begin();
306  it != m_triangles.end(); ++it)
307  for (size_t i = 0; i < 3; i++) it->r[i] = col;
308  return *this;
309 }
310 
312 {
314  m_color.G = g;
315  const float col = g / 255.f;
316  for (std::vector<TTriangle>::iterator it = m_triangles.begin();
317  it != m_triangles.end(); ++it)
318  for (size_t i = 0; i < 3; i++) it->g[i] = col;
319  return *this;
320 }
321 
323 {
325  m_color.B = b;
326  const float col = b / 255.f;
327  for (std::vector<TTriangle>::iterator it = m_triangles.begin();
328  it != m_triangles.end(); ++it)
329  for (size_t i = 0; i < 3; i++) it->b[i] = col;
330  return *this;
331 }
332 
334 {
336  m_color.A = a;
337  const float col = a / 255.f;
338  for (std::vector<TTriangle>::iterator it = m_triangles.begin();
339  it != m_triangles.end(); ++it)
340  for (size_t i = 0; i < 3; i++) it->a[i] = col;
341  return *this;
342 }
343 
345  std::vector<mrpt::math::TPolygon3D>& polys) const
346 {
347  if (!polygonsUpToDate) updatePolygons();
348  size_t N = tmpPolygons.size();
349  for (size_t i = 0; i < N; i++) polys[i] = tmpPolygons[i].poly;
350 }
351 
353 {
354  TPolygon3D tmp(3);
355  size_t N = m_triangles.size();
356  tmpPolygons.resize(N);
357  for (size_t i = 0; i < N; i++)
358  for (size_t j = 0; j < 3; j++)
359  {
360  const TTriangle& t = m_triangles[i];
361  tmp[j].x = t.x[j];
362  tmp[j].y = t.y[j];
363  tmp[j].z = t.z[j];
364  tmpPolygons[i] = tmp;
365  }
366  polygonsUpToDate = true;
368 }
369 
371  mrpt::math::TPoint3D& bb_min, mrpt::math::TPoint3D& bb_max) const
372 {
373  bb_min = mrpt::math::TPoint3D(
374  std::numeric_limits<double>::max(), std::numeric_limits<double>::max(),
375  std::numeric_limits<double>::max());
376  bb_max = mrpt::math::TPoint3D(
377  -std::numeric_limits<double>::max(),
378  -std::numeric_limits<double>::max(),
379  -std::numeric_limits<double>::max());
380 
381  for (size_t i = 0; i < m_triangles.size(); i++)
382  {
383  const TTriangle& t = m_triangles[i];
384 
385  keep_min(bb_min.x, t.x[0]);
386  keep_max(bb_max.x, t.x[0]);
387  keep_min(bb_min.y, t.y[0]);
388  keep_max(bb_max.y, t.y[0]);
389  keep_min(bb_min.z, t.z[0]);
390  keep_max(bb_max.z, t.z[0]);
391 
392  keep_min(bb_min.x, t.x[1]);
393  keep_max(bb_max.x, t.x[1]);
394  keep_min(bb_min.y, t.y[1]);
395  keep_max(bb_max.y, t.y[1]);
396  keep_min(bb_min.z, t.z[1]);
397  keep_max(bb_max.z, t.z[1]);
398 
399  keep_min(bb_min.x, t.x[2]);
400  keep_max(bb_max.x, t.x[2]);
401  keep_min(bb_min.y, t.y[2]);
402  keep_max(bb_max.y, t.y[2]);
403  keep_min(bb_min.z, t.z[2]);
404  keep_max(bb_max.z, t.z[2]);
405  }
406 
407  // Convert to coordinates of my parent:
408  m_pose.composePoint(bb_min, bb_min);
409  m_pose.composePoint(bb_max, bb_max);
410 }
411 
413 {
414  reserve(m_triangles.size() + p->m_triangles.size());
415  m_triangles.insert(
416  m_triangles.end(), p->m_triangles.begin(), p->m_triangles.end());
417  polygonsUpToDate = false;
419 }
Scalar * iterator
Definition: eigen_plugins.h:26
GLdouble GLdouble t
Definition: glext.h:3689
CRenderizable & setColorG_u8(const uint8_t g) override
Color components in the range [0,255].
bool traceRay(const mrpt::poses::CPose3D &o, double &dist) const override
Ray tracing.
GLbyte GLbyte bz
Definition: glext.h:6105
GLAPI void GLAPIENTRY glEnable(GLenum cap)
void getPolygons(std::vector< mrpt::math::TPolygon3D > &polys) const
Gets the polygon cache.
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
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
GLAPI void GLAPIENTRY glNormal3f(GLfloat nx, GLfloat ny, GLfloat nz)
GLenum GLsizei n
Definition: glext.h:5074
#define GL_TRIANGLES
Definition: glew.h:276
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: CArchive.h:127
The base class of 3D objects that can be directly rendered through OpenGL.
Definition: CRenderizable.h:41
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:416
#define GL_ONE_MINUS_SRC_ALPHA
Definition: glew.h:287
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...
#define GL_DEPTH_TEST
Definition: glew.h:401
CRenderizable & setColor_u8(const mrpt::img::TColor &c) override
Changes the default object color.
static void triangle_writeToStream(mrpt::serialization::CArchive &o, const CSetOfTriangles::TTriangle &t)
CRenderizable & setColorR_u8(const uint8_t r) override
Color components in the range [0,255].
GLAPI void GLAPIENTRY glBlendFunc(GLenum sfactor, GLenum dfactor)
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
This base provides a set of functions for maths stuff.
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...
const GLubyte * c
Definition: glext.h:6313
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
GLubyte g
Definition: glext.h:6279
GLubyte GLubyte b
Definition: glext.h:6279
double x
X,Y,Z coordinates.
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...
GLAPI void GLAPIENTRY glBegin(GLenum mode)
#define GL_BLEND
Definition: glew.h:432
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
GLAPI void GLAPIENTRY glVertex3f(GLfloat x, GLfloat y, GLfloat z)
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
void updatePolygons() const
Polygon cache updating.
#define GL_SRC_ALPHA
Definition: glew.h:286
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
static void triangle_readFromStream(mrpt::serialization::CArchive &i, CSetOfTriangles::TTriangle &t)
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:52
GLdouble GLdouble GLdouble r
Definition: glext.h:3705
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:86
A RGB color - floats in the range [0,1].
Definition: TColor.h:77
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
A set of colored triangles.
typedef void(APIENTRYP PFNGLBLENDCOLORPROC)(GLclampf red
GLAPI void GLAPIENTRY glColor4f(GLfloat red, GLfloat green, GLfloat blue, GLfloat alpha)
A RGB color - 8bit.
Definition: TColor.h:20
Lightweight 3D point.
unsigned __int32 uint32_t
Definition: rptypes.h:47
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: CArchive.h:88
GLAPI void GLAPIENTRY glDisable(GLenum cap)
CRenderizable & setColorB_u8(const uint8_t b) override
Color components in the range [0,255].
GLubyte GLubyte GLubyte a
Definition: glext.h:6279
GLfloat GLfloat p
Definition: glext.h:6305
CRenderizable & setColorA_u8(const uint8_t a) override
Color components in the range [0,255].
void insertTriangles(const InputIterator &begin, const InputIterator &end)
Inserts a set of triangles, bounded by iterators, into this set.
const Scalar * const_iterator
Definition: eigen_plugins.h:27
3D polygon, inheriting from std::vector<TPoint3D>



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020