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);
36  glEnable(GL_BLEND);
37  glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
38  }
39  else
40  {
41  glEnable(GL_DEPTH_TEST);
42  glDisable(GL_BLEND);
43  }
44 
45  vector<TTriangle>::const_iterator it;
46 
47  glEnable(GL_NORMALIZE); // Normalize normals
48  glBegin(GL_TRIANGLES);
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();
75  glDisable(GL_NORMALIZE);
76 
77  glDisable(GL_BLEND);
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 
106 uint8_t CSetOfTriangles::serializeGetVersion() const { return 1; }
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 }
118  mrpt::serialization::CArchive& in, uint8_t version)
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) ...
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...
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.
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:2565
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.
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:133
The base class of 3D objects that can be directly rendered through OpenGL.
Definition: CRenderizable.h:40
STL namespace.
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].
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 serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
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...
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
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...
void updatePolygons() const
Polygon cache updating.
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
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::vision::TStereoCalibResults out
A RGB color - floats in the range [0,1].
Definition: TColor.h:78
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:15
const auto bb_max
A set of colored triangles.
const auto bb_min
A RGB color - 8bit.
Definition: TColor.h:20
Lightweight 3D point.
Definition: TPoint3D.h:90
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:94
CRenderizable & setColorB_u8(const uint8_t b) override
Color components in the range [0,255].
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: 7e629e01a Sat Dec 14 00:05:55 2019 +0100 at sáb dic 14 00:15:10 CET 2019