MRPT  1.9.9
CSetOfTriangles.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 
15 #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 
45  vector<TTriangle>::const_iterator it;
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  auto 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  CMatrixDynamic<float> M=CMatrixDynamic<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=(CMatrixDynamic<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 (CMatrixDynamic<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 (auto& m_triangle : m_triangles)
289  for (size_t i = 0; i < 3; i++)
290  {
291  m_triangle.r[i] = col.R;
292  m_triangle.g[i] = col.G;
293  m_triangle.b[i] = col.B;
294  m_triangle.a[i] = col.A;
295  }
296  return *this;
297 }
298 
300 {
302  m_color.R = r;
303  const float col = r / 255.f;
304  for (auto& m_triangle : m_triangles)
305  for (size_t i = 0; i < 3; i++) m_triangle.r[i] = col;
306  return *this;
307 }
308 
310 {
312  m_color.G = g;
313  const float col = g / 255.f;
314  for (auto& m_triangle : m_triangles)
315  for (size_t i = 0; i < 3; i++) m_triangle.g[i] = col;
316  return *this;
317 }
318 
320 {
322  m_color.B = b;
323  const float col = b / 255.f;
324  for (auto& m_triangle : m_triangles)
325  for (size_t i = 0; i < 3; i++) m_triangle.b[i] = col;
326  return *this;
327 }
328 
330 {
332  m_color.A = a;
333  const float col = a / 255.f;
334  for (auto& m_triangle : m_triangles)
335  for (size_t i = 0; i < 3; i++) m_triangle.a[i] = col;
336  return *this;
337 }
338 
340  std::vector<mrpt::math::TPolygon3D>& polys) const
341 {
342  if (!polygonsUpToDate) updatePolygons();
343  size_t N = tmpPolygons.size();
344  for (size_t i = 0; i < N; i++) polys[i] = tmpPolygons[i].poly;
345 }
346 
348 {
349  TPolygon3D tmp(3);
350  size_t N = m_triangles.size();
351  tmpPolygons.resize(N);
352  for (size_t i = 0; i < N; i++)
353  for (size_t j = 0; j < 3; j++)
354  {
355  const TTriangle& t = m_triangles[i];
356  tmp[j].x = t.x[j];
357  tmp[j].y = t.y[j];
358  tmp[j].z = t.z[j];
359  tmpPolygons[i] = tmp;
360  }
361  polygonsUpToDate = true;
363 }
364 
367 {
369  std::numeric_limits<double>::max(), std::numeric_limits<double>::max(),
370  std::numeric_limits<double>::max());
372  -std::numeric_limits<double>::max(),
373  -std::numeric_limits<double>::max(),
374  -std::numeric_limits<double>::max());
375 
376  for (const auto& t : m_triangles)
377  {
378  keep_min(bb_min.x, t.x[0]);
379  keep_max(bb_max.x, t.x[0]);
380  keep_min(bb_min.y, t.y[0]);
381  keep_max(bb_max.y, t.y[0]);
382  keep_min(bb_min.z, t.z[0]);
383  keep_max(bb_max.z, t.z[0]);
384 
385  keep_min(bb_min.x, t.x[1]);
386  keep_max(bb_max.x, t.x[1]);
387  keep_min(bb_min.y, t.y[1]);
388  keep_max(bb_max.y, t.y[1]);
389  keep_min(bb_min.z, t.z[1]);
390  keep_max(bb_max.z, t.z[1]);
391 
392  keep_min(bb_min.x, t.x[2]);
393  keep_max(bb_max.x, t.x[2]);
394  keep_min(bb_min.y, t.y[2]);
395  keep_max(bb_max.y, t.y[2]);
396  keep_min(bb_min.z, t.z[2]);
397  keep_max(bb_max.z, t.z[2]);
398  }
399 
400  // Convert to coordinates of my parent:
401  m_pose.composePoint(bb_min, bb_min);
402  m_pose.composePoint(bb_max, bb_max);
403 }
404 
406 {
407  reserve(m_triangles.size() + p->m_triangles.size());
408  m_triangles.insert(
409  m_triangles.end(), p->m_triangles.begin(), p->m_triangles.end());
410  polygonsUpToDate = false;
412 }
void notifyChange() const
Must be called to notify that the object has changed (so, the display list must be updated) ...
GLdouble GLdouble t
Definition: glext.h:3695
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:6193
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:2569
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
GLAPI void GLAPIENTRY glNormal3f(GLfloat nx, GLfloat ny, GLfloat nz)
GLenum GLsizei n
Definition: glext.h:5136
#define GL_TRIANGLES
Definition: glew.h:277
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:129
The base class of 3D objects that can be directly rendered through OpenGL.
Definition: CRenderizable.h:40
STL namespace.
#define GL_NORMALIZE
Definition: glew.h:417
#define GL_ONE_MINUS_SRC_ALPHA
Definition: glew.h:288
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:402
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:44
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
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:6406
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
GLubyte g
Definition: glext.h:6372
GLubyte GLubyte b
Definition: glext.h:6372
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:433
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:287
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:54
GLdouble GLdouble GLdouble r
Definition: glext.h:3711
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:84
A RGB color - floats in the range [0,1].
Definition: TColor.h:77
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
A set of colored triangles.
typedef void(APIENTRYP PFNGLBLENDCOLORPROC)(GLclampf red
const auto bb_min
GLAPI void GLAPIENTRY glColor4f(GLfloat red, GLfloat green, GLfloat blue, GLfloat alpha)
A RGB color - 8bit.
Definition: TColor.h:20
Lightweight 3D point.
Definition: TPoint3D.h:90
unsigned __int32 uint32_t
Definition: rptypes.h:50
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:90
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:6372
GLfloat GLfloat p
Definition: glext.h:6398
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.
3D polygon, inheriting from std::vector<TPoint3D>
Definition: TPolygon3D.h:18



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 928d0abbd Sun Oct 13 02:28:49 2019 +0200 at dom oct 13 02:30:11 CEST 2019