MRPT  1.9.9
CPolyhedron.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
13 #include <mrpt/math/CMatrix.h>
14 #include <mrpt/poses/CPose3D.h>
15 #include <mrpt/math/geometry.h>
16 #include <mrpt/math/ops_containers.h> // dotProduct()
17 #include <mrpt/random.h>
20 
21 #include "opengl_internals.h"
22 
23 using namespace mrpt;
24 using namespace mrpt::math;
25 using namespace mrpt::opengl;
26 using namespace mrpt::poses;
27 using namespace std;
29 
31 
32 // Auxiliary data and code
33 template <class T>
35 {
36  public:
37  const vector<TPoint3D>& verts;
38  FCreatePolygonFromFace(const vector<TPoint3D>& v) : verts(v) {}
41  {
42  p = TPolygon3D(f.vertices.size());
43  for (size_t i = 0; i < f.vertices.size(); i++)
44  p[i] = verts[f.vertices[i]];
45  return p;
46  }
47 };
48 
50  const vector<math::TPolygon3D>& polys, vector<TPoint3D>& vertices,
51  vector<CPolyhedron::TPolyhedronFace>& faces)
52 {
53  vertices.reserve(4 * polys.size());
54  faces.reserve(polys.size());
55  for (std::vector<math::TPolygon3D>::const_iterator it = polys.begin();
56  it != polys.end(); ++it)
57  {
58  size_t N = it->size();
59  if (N < 3) return false;
61  f.vertices.resize(N);
62  for (size_t i = 0; i < N; i++)
63  {
65  find(vertices.begin(), vertices.end(), (*it)[i]);
66  if (it2 == vertices.end())
67  {
68  f.vertices[i] = vertices.size();
69  vertices.push_back((*it)[i]);
70  }
71  else
72  f.vertices[i] = it2 - vertices.begin();
73  }
74  faces.push_back(f);
75  }
76  return true;
77 }
78 
80 {
89  PRISM = 6,
90  ANTIPRISM = 7,
95 };
97  const std::string& components, uint32_t numBaseEdges,
98  vector<JohnsonBodyPart>& parts)
99 {
100  size_t N = components.length();
101  size_t i = 0;
102  bool rot = false;
103  while (i < N)
104  {
105  switch (components[i])
106  {
107  case 'A':
108  case 'a':
109  if (parts.size() == 0) parts.push_back(INF_NO_BODY);
110  parts.push_back(ANTIPRISM);
111  break;
112  case 'C':
113  case 'c':
114  if (numBaseEdges & 1) return false;
115  if (i == N - 1) return false;
116  i++;
117  if (components[i] == '+')
118  {
119  if (i != N - 1) return false;
120  if (parts.size() == 0) parts.push_back(INF_NO_BODY);
121  parts.push_back(
123  rot = false;
124  }
125  else if (components[i] == '-')
126  {
127  if (parts.size() > 0) return false;
128  parts.push_back(
130  rot = false;
131  }
132  else
133  return false;
134  break;
135  case 'R':
136  case 'r':
137  if (numBaseEdges != 10) return false;
138  if (i == N - 1) return false;
139  i++;
140  if (components[i] == '+')
141  {
142  if (i != N - 1) return false;
143  if (parts.size() == 0) parts.push_back(INF_NO_BODY);
144  parts.push_back(
146  rot = false;
147  }
148  else if (components[i] == '-')
149  {
150  if (parts.size() > 0) return false;
151  parts.push_back(
153  rot = false;
154  }
155  else
156  return false;
157  break;
158  case 'G':
159  case 'g':
160  if (i == N - 1) return false;
161  i++;
162  if (components[i] == 'C' || components[i] == 'R')
163  {
164  rot = true;
165  continue;
166  }
167  else
168  return false;
169  case 'P':
170  case 'p':
171  if (i == N - 1) return false;
172  i++;
173  switch (components[i])
174  {
175  case '+':
176  if (numBaseEdges > 5) return false;
177  if (i != N - 1) return false;
178  if (parts.size() == 0) parts.push_back(INF_NO_BODY);
179  parts.push_back(UPWARDS_PYRAMID);
180  break;
181  case '-':
182  if (numBaseEdges > 5) return false;
183  if (i != 1) return false;
184  parts.push_back(DOWNWARDS_PYRAMID);
185  break;
186  case 'R':
187  case 'r':
188  if (parts.size() > 0 && (*parts.rbegin() == PRISM))
189  return false;
190  if (parts.size() == 0) parts.push_back(INF_NO_BODY);
191  parts.push_back(PRISM);
192  break;
193  default:
194  return false;
195  }
196  break;
197  default:
198  return false;
199  }
200  i++;
201  }
202  if (parts.size() == 0) return false;
203  JohnsonBodyPart p = *parts.rbegin();
204  if (p != UPWARDS_PYRAMID && p != UPWARDS_CUPOLA &&
207  parts.push_back(SUP_NO_BODY);
208  return true;
209 }
210 inline size_t additionalVertices(JohnsonBodyPart j, uint32_t numBaseEdges)
211 {
212  if (j == INF_NO_BODY || j == SUP_NO_BODY)
213  return 0;
214  else if (j < UPWARDS_CUPOLA)
215  return 1; // j is a pyramid
216  else if (j < PRISM)
217  return numBaseEdges >> 1; // j is a cupola
218  else if (j < UPWARDS_ROTUNDA)
219  return numBaseEdges; // j is a prism or antiprism
220  else
221  return 10; // j is a rotunda
222 }
224  size_t numBaseEdges, double angleShift, double baseRadius,
225  double edgeLength, bool isRotated, bool isUpwards, size_t base,
226  vector<TPoint3D>& verts, vector<CPolyhedron::TPolyhedronFace>& faces)
227 {
228  size_t edges2 = numBaseEdges >> 1;
229  double minorRadius =
230  baseRadius * sin(M_PI / numBaseEdges) / sin(M_PI / edges2);
231  //"Proper base"'s apothem=base radius*cos(2*pi/(2*numBaseEdges))
232  //"Small base"'s apothem=small radius*cos(2*pi/2*(numBaseEdges/2))
233  // Cupola's height is so that (Da^2+Height^2=Edge length^2, where Da is the
234  // difference between both apothems.
235  double h = sqrt(
236  square(edgeLength) - square(
237  baseRadius * cos(M_PI / numBaseEdges) -
238  minorRadius * cos(M_PI / edges2)));
239  double height = verts[base].z + (isUpwards ? h : -h);
240  angleShift += M_PI / edges2 +
241  (isRotated ? -M_PI / numBaseEdges : M_PI / numBaseEdges);
242  size_t minorBase = verts.size();
243  for (size_t i = 0; i < edges2; i++)
244  {
245  double ang = angleShift + 2 * M_PI * i / edges2;
246  verts.push_back(
247  TPoint3D(minorRadius * cos(ang), minorRadius * sin(ang), height));
248  }
249  CPolyhedron::TPolyhedronFace tri, quad, cBase;
250  tri.vertices.resize(3);
251  quad.vertices.resize(4);
252  cBase.vertices.resize(edges2);
253  size_t iq = isRotated ? 1 : 2, it = 0;
254  for (size_t i = 0; i < edges2; i++)
255  {
256  cBase.vertices[i] = it + minorBase;
257  size_t iiq = (iq + 1) % numBaseEdges + base;
258  size_t iiiq = (iiq + 1) % numBaseEdges + base;
259  size_t iit = (it + 1) % edges2 + minorBase;
260  quad.vertices[0] = it + minorBase;
261  quad.vertices[1] = iit;
262  quad.vertices[2] = iiq;
263  quad.vertices[3] = iq + base;
264  tri.vertices[0] = iit;
265  tri.vertices[1] = iiq;
266  tri.vertices[2] = iiiq;
267  iq = (iq + 2) % numBaseEdges;
268  it = (it + 1) % edges2;
269  faces.push_back(tri);
270  faces.push_back(quad);
271  }
272  if (edges2 >= 3) faces.push_back(cBase);
273 }
275  double angleShift, double baseRadius, bool isRotated, bool isUpwards,
276  size_t base, vector<TPoint3D>& verts,
277  vector<CPolyhedron::TPolyhedronFace>& faces)
278 {
279  double R1 = baseRadius * sqrt((5.0 - sqrt(5.0)) / 10.0);
280  double R2 = baseRadius * sqrt((5.0 + sqrt(5.0)) / 10.0);
281  double baseHeight = verts[base].z;
282  TPoint3D p1[5], p2[5];
283  angleShift += M_PI / 10;
284  if (isRotated) angleShift += M_PI / 5;
285  for (size_t i = 0; i < 5; i++)
286  {
287  double a = (i + i + 1) * M_PI / 5 + angleShift;
288  double b = (i + i) * M_PI / 5 + angleShift;
289  double ca = cos(a), sa = sin(a), cb = cos(b), sb = sin(b);
290  p1[i].x = R1 * ca;
291  p1[i].y = R1 * sa;
292  p1[i].z = baseHeight + (isUpwards ? R2 : -R2);
293  p2[i].x = R2 * cb;
294  p2[i].y = R2 * sb;
295  p2[i].z = baseHeight + (isUpwards ? R1 : -R1);
296  }
297  size_t newBase = verts.size();
298  for (size_t i = 0; i < 5; i++) verts.push_back(p1[i]);
299  for (size_t i = 0; i < 5; i++) verts.push_back(p2[i]);
301  f.vertices.resize(3);
302  g.vertices.resize(5);
303  size_t baseStart = isRotated ? 2 : 1;
304  for (size_t i = 0; i < 5; i++)
305  {
306  size_t ii = (i + 1) % 5;
307  f.vertices[0] = newBase + i;
308  f.vertices[1] = newBase + ii;
309  f.vertices[2] = newBase + ii + 5;
310  faces.push_back(f);
311  f.vertices[0] = newBase + i + 5;
312  f.vertices[1] = ((i + i + baseStart) % 10) + base;
313  f.vertices[2] = ((i + i + 9 + baseStart) % 10) + base;
314  faces.push_back(f);
315  g.vertices[0] = newBase + (ii % 5) + 5;
316  g.vertices[1] = newBase + i;
317  g.vertices[2] = newBase + i + 5;
318  g.vertices[3] = (i + i + baseStart) % 10 + base;
319  g.vertices[4] = (i + i + baseStart + 1) % 10 + base;
320  faces.push_back(g);
321  }
322  for (size_t i = 0; i < 5; i++) g.vertices[i] = i + newBase;
323  faces.push_back(g);
324  return;
325 }
326 inline size_t additionalFaces(JohnsonBodyPart j, uint32_t numBaseEdges)
327 {
328  if (j == INF_NO_BODY || j == SUP_NO_BODY)
329  return 1; // j is a base
330  else if (j < UPWARDS_CUPOLA)
331  return numBaseEdges; // j is a pyramid
332  else if (j < PRISM)
333  return numBaseEdges + ((numBaseEdges >= 6) ? 1 : 0); // j is a cupola
334  else if (j == PRISM)
335  return numBaseEdges;
336  else if (j == ANTIPRISM)
337  return numBaseEdges << 1;
338  else
339  return 16; // j is a rotunda
340 }
341 
342 inline bool faceContainsEdge(
345 {
346  char hm = 0;
347  for (vector<uint32_t>::const_iterator it = f.vertices.begin();
348  it != f.vertices.end(); ++it)
349  if (*it == e.v1 || *it == e.v2) hm++;
350  return hm == 2;
351 }
352 
353 bool getPlanesIntersection(const vector<const TPlane*>& planes, TPoint3D& pnt)
354 {
355  if (planes.size() < 3) return false;
356  char o = 0;
357  TPlane pl = *planes[0];
358  TLine3D l;
359  TObject3D obj;
360  for (size_t i = 1; i < planes.size(); i++) switch (o)
361  {
362  case 0:
363  if (!intersect(pl, *planes[i], obj)) return false;
364  if (obj.getPlane(pl))
365  o = 0;
366  else if (obj.getLine(l))
367  o = 1;
368  else if (obj.getPoint(pnt))
369  o = 2;
370  else
371  return false;
372  break;
373  case 1:
374  if (!intersect(l, *planes[i], obj)) return false;
375  if (obj.getLine(l))
376  o = 1;
377  else if (obj.getPoint(pnt))
378  o = 2;
379  else
380  return false;
381  break;
382  case 2:
383  if (!planes[i]->contains(pnt)) return false;
384  break;
385  default:
386  return false;
387  }
388  return o == 2;
389 }
390 
392  const vector<CPolyhedron::TPolyhedronFace>& fs, uint32_t v1, uint32_t v2,
393  uint32_t v3)
394 {
396  it != fs.end(); ++it)
397  {
398  const vector<uint32_t>& f = it->vertices;
399  size_t hmf = 0;
400  for (vector<uint32_t>::const_iterator it2 = f.begin(); it2 != f.end();
401  ++it2)
402  {
403  if (*it2 == v1)
404  hmf |= 1;
405  else if (*it2 == v2)
406  hmf |= 2;
407  else if (*it2 == v3)
408  hmf |= 4;
409  }
410  if (hmf == 7) return true;
411  }
412  return false;
413 }
415  const vector<CPolyhedron::TPolyhedronEdge>& es, uint32_t v1, uint32_t v2,
416  size_t& where)
417 {
418  for (where = 0; where < es.size(); where++)
419  {
420  const CPolyhedron::TPolyhedronEdge& e = es[where];
421  if (e.v1 == v1 && e.v2 == v2)
422  return true;
423  else if (e.v1 == v2 && e.v2 == v1)
424  return false;
425  }
426  throw std::logic_error("Internal error. Edge not found");
427 }
432 {
434  it != end; ++it)
435  {
436  const vector<uint32_t>& f = it->vertices;
437  char res = 0;
438  for (vector<uint32_t>::const_iterator it2 = f.begin(); it2 != f.end();
439  ++it2)
440  if (*it2 == v1)
441  res |= 1;
442  else if (*it2 == v2)
443  res |= 2;
444  if (res == 3) return true;
445  }
446  return false;
447 }
448 double getHeight(const TPolygon3D& p, const TPoint3D& c)
449 {
450  size_t N = p.size();
451  if (N > 5 || N < 3)
452  throw std::logic_error("Faces must have exactly 3, 4 or 5 vertices.");
453  double r = mrpt::math::distance(p[0], c);
454  double l = mrpt::math::distance(p[0], p[1]);
455  for (size_t i = 1; i < N; i++)
456  if (abs(mrpt::math::distance(p[i], c) - r) >= mrpt::math::getEpsilon())
457  throw std::logic_error("There is a non-regular polygon.");
458  else if (
459  abs(mrpt::math::distance(p[i], p[(i + 1) % N]) - l) >=
461  throw std::logic_error("There is a non-regular polygon.");
462  return sqrt(square(l) - square(r));
463 }
465 {
466  double d[3];
467  double mod;
468  inline double& operator[](size_t i) { return d[i]; }
469  inline double operator[](size_t i) const { return d[i]; }
470 };
471 // End of auxiliary data and code
472 
474  const vector<TPoint3D>& vertices) const
475 {
476  return mrpt::math::distance(vertices[v1], vertices[v2]);
477 }
478 
479 double CPolyhedron::TPolyhedronFace::area(const vector<TPoint3D>& vs) const
480 {
481  // Calculate as fan of triangles.
482  size_t N = vertices.size();
483  vector<SegmentVector> d(N - 1);
484  for (size_t i = 1; i < N; i++)
485  {
486  d[i - 1].mod = 0;
487  for (size_t j = 0; j < 3; j++)
488  {
489  d[i - 1][j] = vs[vertices[i]][j] - vs[vertices[0]][j];
490  d[i - 1].mod += square(d[i - 1][j]);
491  }
492  d[i - 1].mod = sqrt(d[i - 1].mod);
493  }
494  double res = 0;
495  for (size_t i = 1; i < N - 1; i++)
496  res += sqrt(
497  square(d[i - 1].mod * d[i].mod) -
498  square(dotProduct<3, double>(d[i - 1], d[i])));
499  return res / 2;
500 }
501 
502 void CPolyhedron::TPolyhedronFace::getCenter(
503  const vector<TPoint3D>& vrts, TPoint3D& p) const
504 {
505  p.x = p.y = p.z = 0.0;
506  for (vector<uint32_t>::const_iterator it = vertices.begin();
507  it != vertices.end(); ++it)
508  {
509  p.x += vrts[*it].x;
510  p.y += vrts[*it].y;
511  p.z += vrts[*it].z;
512  }
513  size_t N = vertices.size();
514  p.x /= N;
515  p.y /= N;
516  p.z /= N;
517 }
518 
519 CPolyhedron::CPolyhedron(const std::vector<math::TPolygon3D>& polys)
520  : mEdges(), mWireframe(false), mLineWidth(1), polygonsUpToDate(false)
521 {
522  std::vector<TPoint3D> vertices(0);
523  std::vector<TPolyhedronFace> faces;
524  if (!getVerticesAndFaces(polys, vertices, faces))
525  throw std::logic_error("Can't create CPolygon");
526  mVertices = std::move(vertices);
527  mFaces = std::move(faces);
528 
529  InitFromVertAndFaces(vertices, faces);
530 }
531 
533  const vector<TPoint3D>& vertices, const vector<vector<uint32_t>>& faces)
534 {
535  vector<TPolyhedronFace> aux;
536  for (vector<vector<uint32_t>>::const_iterator it = faces.begin();
537  it != faces.end(); ++it)
538  {
539  TPolyhedronFace f;
540  f.vertices = *it;
541  aux.push_back(f);
542  }
543  InitFromVertAndFaces(vertices, aux);
544 }
545 
547  double x1, double x2, double y1, double y2, double z1, double z2)
548 {
549  vector<TPoint3D> verts;
550  vector<TPolyhedronFace> faces;
551  for (int i = 0; i < 8; i++)
552  verts.push_back(
553  TPoint3D((i & 1) ? x2 : x1, (i & 2) ? y2 : y1, (i & 4) ? z2 : z1));
554  static uint32_t faceVertices[] = {0, 1, 5, 4, 2, 3, 7, 6, 0, 2, 6, 4,
555  1, 3, 7, 5, 0, 1, 3, 2, 4, 5, 7, 6};
556  TPolyhedronFace f;
557  for (uint32_t* p = reinterpret_cast<uint32_t*>(&faceVertices);
558  p < 24 + reinterpret_cast<uint32_t*>(&faceVertices); p += 4)
559  {
560  f.vertices.insert(f.vertices.begin(), p, p + 4);
561  faces.push_back(f);
562  f.vertices.clear();
563  }
564  return CreateNoCheck(verts, faces);
565 }
566 
568  const vector<TPoint2D>& baseVertices, double height)
569 {
570  uint32_t n = baseVertices.size();
571  if (baseVertices.size() < 3) throw std::logic_error("Not enought vertices");
572  vector<TPoint3D> verts;
573  vector<TPolyhedronFace> faces;
574  verts.push_back(TPoint3D(0, 0, height));
575  for (vector<TPoint2D>::const_iterator it = baseVertices.begin();
576  it != baseVertices.end(); ++it)
577  verts.push_back(TPoint3D(it->x, it->y, 0));
578  TPolyhedronFace f, g;
579  f.vertices.push_back(0);
580  f.vertices.push_back(n);
581  f.vertices.push_back(1);
582  g.vertices.push_back(1);
583  faces.push_back(f);
584  for (uint32_t i = 2; i <= n; i++)
585  {
586  f.vertices.erase(f.vertices.begin() + 1);
587  f.vertices.push_back(i);
588  faces.push_back(f);
589  g.vertices.push_back(i);
590  }
591  faces.push_back(g);
592  return CreateNoCheck(verts, faces);
593 }
594 
596  const vector<TPoint2D>& baseVertices, double height1, double height2)
597 {
598  uint32_t N = baseVertices.size();
599  if (N < 3) throw std::logic_error("Not enought vertices");
600  vector<TPoint3D> verts;
601  verts.reserve(N + 2);
602  vector<TPolyhedronFace> faces;
603  faces.reserve(N << 1);
604  verts.push_back(TPoint3D(0, 0, height1));
605  for (vector<TPoint2D>::const_iterator it = baseVertices.begin();
606  it != baseVertices.end(); ++it)
607  verts.push_back(TPoint3D(it->x, it->y, 0));
608  verts.push_back(TPoint3D(0, 0, -height2));
609  TPolyhedronFace f, g;
610  f.vertices.resize(3);
611  g.vertices.resize(3);
612  f.vertices[0] = 0;
613  g.vertices[0] = N + 1;
614  for (uint32_t i = 1; i < N; i++)
615  {
616  f.vertices[1] = g.vertices[1] = i;
617  f.vertices[2] = g.vertices[2] = i + 1;
618  faces.push_back(f);
619  faces.push_back(g);
620  }
621  f.vertices[1] = g.vertices[1] = 1;
622  faces.push_back(f);
623  faces.push_back(g);
624  return CreateNoCheck(verts, faces);
625 }
626 
628  const vector<TPoint2D>& baseVertices, double height, double ratio)
629 {
630  uint32_t n = baseVertices.size();
631  if (n < 3) throw std::logic_error("Not enough vertices");
632  vector<TPoint3D> verts(n + n);
633  vector<TPolyhedronFace> faces(n + 2);
634  TPolyhedronFace f, g, h;
635  f.vertices.resize(4);
636  g.vertices.resize(n);
637  h.vertices.resize(n);
638  for (uint32_t i = 0; i < n; i++)
639  {
640  verts[i] = TPoint3D(baseVertices[i].x, baseVertices[i].y, 0);
641  verts[i + n] = TPoint3D(
642  baseVertices[i].x * ratio, baseVertices[i].y * ratio, height);
643  uint32_t ii = (i + 1) % n;
644  f.vertices[0] = i;
645  f.vertices[1] = ii;
646  f.vertices[2] = ii + n;
647  f.vertices[3] = i + n;
648  faces[i] = f;
649  g.vertices[i] = i;
650  h.vertices[i] = i + n;
651  }
652  faces[n] = g;
653  faces[n + 1] = h;
654  return CreateNoCheck(verts, faces);
655 }
656 
658  const vector<TPoint2D>& bottomBase, const vector<TPoint2D>& topBase,
659  const double height)
660 {
661  uint32_t n = bottomBase.size();
662  if (n < 3) throw std::logic_error("Not enough vertices");
663  if (n != topBase.size())
664  throw std::logic_error("Bases' number of vertices do not match");
665  vector<TPoint3D> verts(n + n);
666  vector<TPolyhedronFace> faces(n + n + 2);
667  TPolyhedronFace f, g, h;
668  f.vertices.resize(3);
669  g.vertices.resize(n);
670  h.vertices.resize(n);
671  for (uint32_t i = 0; i < n; i++)
672  {
673  verts[i] = TPoint3D(bottomBase[i].x, bottomBase[i].y, 0);
674  verts[n + i] = TPoint3D(topBase[i].x, topBase[i].y, height);
675  uint32_t ii = (i + 1) % n;
676  f.vertices[0] = i;
677  f.vertices[1] = ii;
678  f.vertices[2] = i + n;
679  faces[i] = f;
680  f.vertices[0] = i + n;
681  f.vertices[1] = ii + n;
682  f.vertices[2] = ii;
683  faces[n + i] = f;
684  g.vertices[i] = i;
685  h.vertices[i] = n + i;
686  }
687  faces[n + n] = g;
688  faces[n + n + 1] = h;
689  return CreateNoCheck(verts, faces);
690 }
691 
693  const TPoint3D& base, const TPoint3D& v1, const TPoint3D& v2,
694  const TPoint3D& v3)
695 {
696  vector<TPoint3D> verts(8);
697  vector<TPolyhedronFace> faces(6);
698  for (uint32_t i = 0; i < 8; i++)
699  {
700  verts[i] = base;
701  if (i & 1) verts[i] = verts[i] + v1;
702  if (i & 2) verts[i] = verts[i] + v2;
703  if (i & 4) verts[i] = verts[i] + v3;
704  }
705  TPolyhedronFace f;
706  f.vertices.resize(4);
707  f.vertices[0] = 0;
708  f.vertices[1] = 1;
709  f.vertices[2] = 3;
710  f.vertices[3] = 2;
711  faces[0] = f;
712  // f.vertices[0]=0;
713  // f.vertices[1]=1;
714  f.vertices[2] = 5;
715  f.vertices[3] = 4;
716  faces[1] = f;
717  // f.vertices[0]=0;
718  f.vertices[1] = 2;
719  f.vertices[2] = 6;
720  // f.vertices[3]=4;
721  faces[2] = f;
722  for (uint32_t i = 0; i < 3; i++)
723  {
724  uint32_t valueAdd = 4 >> i;
725  faces[i + 3].vertices.resize(4);
726  for (uint32_t j = 0; j < 4; j++)
727  faces[i + 3].vertices[j] = faces[i].vertices[j] + valueAdd;
728  }
729  return CreateNoCheck(verts, faces);
730 }
731 
733  const vector<TPoint2D>& baseVertices, double height1, double ratio1,
734  double height2, double ratio2)
735 {
736  // TODO: check special case in which ratio=0.
737  size_t N = baseVertices.size();
738  vector<TPoint3D> verts(3 * N);
739  size_t N2 = N + N;
740  for (size_t i = 0; i < N; i++)
741  {
742  double x = baseVertices[i].x;
743  double y = baseVertices[i].y;
744  verts[i].x = x;
745  verts[i].y = y;
746  verts[i].z = 0;
747  verts[i + N].x = x * ratio1;
748  verts[i + N].y = y * ratio1;
749  verts[i + N].z = height1;
750  verts[i + N2].x = x * ratio2;
751  verts[i + N2].y = y * ratio2;
752  verts[i + N2].z = -height2; // This is not an error. This way, two
753  // positive heights produce an actual
754  // bifrustum.
755  }
756  vector<TPolyhedronFace> faces(N2 + 2);
757  TPolyhedronFace f, g, h;
758  f.vertices.resize(4);
759  g.vertices.resize(N);
760  h.vertices.resize(N);
761  for (size_t i = 0; i < N; i++)
762  {
763  size_t i2 = (i + 1) % N;
764  f.vertices[0] = i;
765  f.vertices[1] = i2;
766  f.vertices[2] = i2 + N;
767  f.vertices[3] = i + N;
768  faces[i] = f;
769  f.vertices[2] = i2 + N2;
770  f.vertices[3] = i + N2;
771  faces[i + N] = f;
772  g.vertices[i] = i + N;
773  h.vertices[i] = i + N2;
774  }
775  faces[N2] = g;
776  faces[N2 + 1] = h;
777  return CreateNoCheck(verts, faces);
778 }
779 
781  uint32_t numBaseEdges, double baseRadius, double basesDistance)
782 {
783  if (numBaseEdges < 3) throw std::logic_error("Not enough vertices");
784  if (basesDistance == 0 || baseRadius == 0) return CreateEmpty();
785  size_t numBaseEdges2 = numBaseEdges << 1;
786  vector<TPoint3D> verts(numBaseEdges2 + 2);
787  double space = 2 * M_PI / numBaseEdges;
788  double shift = space / 2;
789  double height1 = basesDistance / 2;
790  double cospii = cos(M_PI / numBaseEdges);
791  double height2 =
792  height1 * (cospii + 1) /
793  (1 - cospii); // Apex height, calculated so each face conforms a plane
794  for (size_t i = 0; i < numBaseEdges; i++)
795  {
796  double ang = space * i;
797  double ang2 = ang + shift;
798  size_t ii = i + numBaseEdges;
799  verts[i].x = baseRadius * cos(ang);
800  verts[i].y = baseRadius * sin(ang);
801  verts[i].z = -height1;
802  verts[ii].x = baseRadius * cos(ang2);
803  verts[ii].y = baseRadius * sin(ang2);
804  verts[ii].z = height1;
805  }
806  verts[numBaseEdges2].x = 0;
807  verts[numBaseEdges2].y = 0;
808  verts[numBaseEdges2].z = -height2;
809  verts[numBaseEdges2 + 1].x = 0;
810  verts[numBaseEdges2 + 1].y = 0;
811  verts[numBaseEdges2 + 1].z = height2;
812  vector<TPolyhedronFace> faces(numBaseEdges2);
813  TPolyhedronFace f, g;
814  f.vertices.resize(4);
815  g.vertices.resize(4);
816  f.vertices[3] = numBaseEdges2;
817  g.vertices[3] = numBaseEdges2 + 1;
818  for (size_t i = 0; i < numBaseEdges; i++)
819  {
820  size_t ii = (i + 1) % numBaseEdges;
821  size_t i2 = i + numBaseEdges;
822  f.vertices[0] = i;
823  f.vertices[1] = i2;
824  f.vertices[2] = ii;
825  g.vertices[0] = i2;
826  g.vertices[1] = ii;
827  g.vertices[2] = ii + numBaseEdges;
828  faces[i] = f;
829  faces[i + numBaseEdges] = g;
830  }
831  return CreateNoCheck(verts, faces);
832 }
833 
835  uint32_t numBaseEdges, double baseRadius, const std::string& components,
836  size_t shifts)
837 {
838  if (baseRadius == 0) return CreateEmpty();
839  if (numBaseEdges < 3) throw std::logic_error("Not enough vertices");
840  vector<JohnsonBodyPart> parts;
841  if (!analyzeJohnsonPartsString(components, numBaseEdges, parts))
842  throw std::logic_error("Invalid string");
843  // Some common values are computed
844  size_t nParts = parts.size();
845  double edgeLength = 2 * baseRadius * sin(M_PI / numBaseEdges);
846  double antiPrismHeight = sqrt(
847  square(edgeLength) -
848  square(baseRadius) * (2 - 2 * cos(M_PI / numBaseEdges)));
849  // Vertices' and faces' vectors are computed
850  size_t nVerts = numBaseEdges * (nParts - 1) +
851  additionalVertices(parts[0], numBaseEdges) +
852  additionalVertices(*parts.rbegin(), numBaseEdges);
853  size_t nFaces = 0;
854  for (size_t i = 0; i < nParts; i++)
855  nFaces += additionalFaces(parts[i], numBaseEdges);
856  vector<TPoint3D> verts;
857  verts.reserve(nVerts);
858  vector<TPolyhedronFace> faces;
859  faces.reserve(nFaces);
860  // Each base's position is computed. Also, the height is set so that the
861  // polyhedron is vertically centered in z=0.
862  double h, mHeight = 0;
863  vector<pair<double, size_t>> basePositionInfo(nParts - 1);
864  for (size_t i = 0; i < nParts - 1; i++)
865  {
866  if (parts[i] == PRISM)
867  h = edgeLength;
868  else if (parts[i] == ANTIPRISM)
869  {
870  h = antiPrismHeight;
871  shifts++;
872  }
873  else
874  h = 0;
875  basePositionInfo[i] = make_pair(mHeight += h, shifts);
876  }
877  mHeight /= 2;
878  double semi = M_PI / numBaseEdges;
879  // All the bases are generated and inserted into the vertices' vector.
880  for (vector<pair<double, size_t>>::const_iterator it =
881  basePositionInfo.begin();
882  it != basePositionInfo.end(); ++it)
884  numBaseEdges, baseRadius, it->first - mHeight, semi * it->second,
885  verts);
886  size_t initialBase = 0, endBase = 0;
888  face.vertices.reserve(numBaseEdges);
889  // Each body is inserted.
890  for (size_t p = 0; p < nParts; p++)
891  {
892  switch (parts[p])
893  {
894  case INF_NO_BODY:
895  // Inferior base.
896  face.vertices.resize(numBaseEdges);
897  for (size_t i = 0; i < numBaseEdges; i++)
898  face.vertices[i] = endBase + i;
899  faces.push_back(face);
900  break;
901  case SUP_NO_BODY:
902  // Superior base.
903  face.vertices.resize(numBaseEdges);
904  for (size_t i = 0; i < numBaseEdges; i++)
905  face.vertices[i] = initialBase + i;
906  faces.push_back(face);
907  break;
908  case UPWARDS_PYRAMID:
909  {
910  // Upwards-pointing pyramid. There must be 5 or less vertices.
911  double apexHeight =
912  baseRadius * sqrt(4 * square(sin(M_PI / numBaseEdges)) - 1);
913  face.vertices.resize(3);
914  face.vertices[0] = verts.size();
915  face.vertices[1] = initialBase + numBaseEdges - 1;
916  face.vertices[2] = initialBase;
917  do
918  {
919  faces.push_back(face);
920  face.vertices[1] = face.vertices[2];
921  face.vertices[2]++;
922  } while (face.vertices[2] < initialBase + numBaseEdges);
923  verts.push_back(
924  TPoint3D(0, 0, verts[initialBase].z + apexHeight));
925  break;
926  }
927  case DOWNWARDS_PYRAMID:
928  {
929  // Downwards-pointing pyramid. There must be 5 or less vertices.
930  double apexHeight =
931  baseRadius * sqrt(4 * square(sin(M_PI / numBaseEdges)) - 1);
932  face.vertices.resize(3);
933  face.vertices[0] = verts.size();
934  face.vertices[1] = endBase + numBaseEdges - 1;
935  face.vertices[2] = endBase;
936  do
937  {
938  faces.push_back(face);
939  face.vertices[1] = face.vertices[2];
940  face.vertices[2]++;
941  } while (face.vertices[2] < endBase + numBaseEdges);
942  verts.push_back(TPoint3D(0, 0, verts[endBase].z - apexHeight));
943  break;
944  }
945  case UPWARDS_CUPOLA:
946  // Upwards-pointing cupola. There must be an even amount of
947  // vertices.
948  insertCupola(
949  numBaseEdges, basePositionInfo.rbegin()->second * semi,
950  baseRadius, edgeLength, false, true, initialBase, verts,
951  faces);
952  break;
953  case DOWNWARDS_CUPOLA:
954  // Downwards-pointing cupola. There must be an even amount of
955  // vertices.
956  insertCupola(
957  numBaseEdges, basePositionInfo[0].second * semi, baseRadius,
958  edgeLength, false, false, endBase, verts, faces);
959  break;
961  // Upwards-pointing, slightly rotated, cupola. There must be an
962  // even amount of vertices.
963  insertCupola(
964  numBaseEdges, basePositionInfo.rbegin()->second * semi,
965  baseRadius, edgeLength, true, true, initialBase, verts,
966  faces);
967  break;
969  // Downwards-pointing, slightly rotated, cupola. There must be
970  // an even amount of vertices.
971  insertCupola(
972  numBaseEdges, basePositionInfo[0].second * semi, baseRadius,
973  edgeLength, true, false, endBase, verts, faces);
974  break;
975  case PRISM:
976  // Archimedean prism.
977  face.vertices.resize(4);
978  for (size_t i = 0; i < numBaseEdges; i++)
979  {
980  size_t ii = (i + 1) % numBaseEdges;
981  face.vertices[0] = initialBase + i;
982  face.vertices[1] = endBase + i;
983  face.vertices[2] = endBase + ii;
984  face.vertices[3] = initialBase + ii;
985  faces.push_back(face);
986  }
987  break;
988  case ANTIPRISM:
989  {
990  // Archimedean antiprism.
991  face.vertices.resize(3);
992  face.vertices[0] = initialBase;
993  face.vertices[1] = endBase;
994  face.vertices[2] = initialBase + 1;
995  bool nextIsEnd = true;
996  size_t nextEnd = 1;
997  size_t nextInitial = 2;
998  for (size_t i = 0; i < numBaseEdges << 1; i++)
999  {
1000  faces.push_back(face);
1001  face.vertices[0] = face.vertices[1];
1002  face.vertices[1] = face.vertices[2];
1003  if (nextIsEnd)
1004  {
1005  face.vertices[2] = endBase + nextEnd;
1006  nextEnd = (nextEnd + 1) % numBaseEdges;
1007  }
1008  else
1009  {
1010  face.vertices[2] = initialBase + nextInitial;
1011  nextInitial = (nextInitial + 1) % numBaseEdges;
1012  }
1013  nextIsEnd = !nextIsEnd;
1014  }
1015  break;
1016  }
1017  case UPWARDS_ROTUNDA:
1018  // Upwards-pointing pentagonal rotunda. Only for bases of
1019  // exactly 10 vertices.
1020  insertRotunda(
1021  basePositionInfo.rbegin()->second * semi, baseRadius, false,
1022  true, initialBase, verts, faces);
1023  break;
1024  case DOWNWARDS_ROTUNDA:
1025  // Downwards-pointing pentagonal rotunda. Only for bases of
1026  // exactly 10 vertices.
1027  insertRotunda(
1028  basePositionInfo[0].second * semi, baseRadius, false, false,
1029  endBase, verts, faces);
1030  break;
1032  // Upwards-pointing, slightly rotated, pentagonal rotunda. Only
1033  // for bases of exactly 10 vertices.
1034  insertRotunda(
1035  basePositionInfo.rbegin()->second * semi, baseRadius, true,
1036  true, initialBase, verts, faces);
1037  break;
1039  // Downwards-pointing, slightly rotated, pentagonal rotunda.
1040  // Only for bases of exactly 10 vertices.
1041  insertRotunda(
1042  basePositionInfo[0].second * semi, baseRadius, true, false,
1043  endBase, verts, faces);
1044  break;
1045  default:
1046  throw std::logic_error("Internal error");
1047  }
1048  initialBase = endBase;
1049  endBase += numBaseEdges;
1050  }
1051  return CreateNoCheck(verts, faces);
1052 }
1053 
1054 /*---------------------------------------------------------------
1055  render
1056  ---------------------------------------------------------------*/
1057 
1059 {
1060 #if MRPT_HAS_OPENGL_GLUT
1061  if (mWireframe)
1062  {
1063  glDisable(GL_LIGHTING); // Disable lights when drawing lines
1064 
1066  checkOpenGLError();
1068  glBegin(GL_LINES);
1070  it != mEdges.end(); ++it)
1071  {
1072  TPoint3D p = mVertices[it->v1];
1073  glVertex3f(p.x, p.y, p.z);
1074  p = mVertices[it->v2];
1075  glVertex3f(p.x, p.y, p.z);
1076  }
1077  glEnd();
1078  glEnable(GL_LIGHTING); // Disable lights when drawing lines
1079  }
1080  else
1081  {
1082  checkOpenGLError();
1083  glEnable(GL_BLEND);
1085 
1088  it != mFaces.end(); ++it)
1089  {
1091  glNormal3f(it->normal[0], it->normal[1], it->normal[2]);
1092  for (vector<uint32_t>::const_iterator it2 = it->vertices.begin();
1093  it2 != it->vertices.end(); ++it2)
1094  {
1095  const TPoint3D& p = mVertices[*it2];
1096  glVertex3f(p.x, p.y, p.z);
1097  }
1098  glEnd();
1099  }
1101  }
1102 #endif
1103 }
1104 
1105 bool CPolyhedron::traceRay(const mrpt::poses::CPose3D& o, double& dist) const
1106 {
1108  return math::traceRay(tempPolygons, (o - this->m_pose).asTPose(), dist);
1109 }
1110 
1111 void CPolyhedron::getEdgesLength(std::vector<double>& lengths) const
1112 {
1113  lengths.resize(mEdges.size());
1114  std::vector<double>::iterator it2 = lengths.begin();
1116  it != mEdges.end(); ++it, ++it2)
1117  *it2 = it->length(mVertices);
1118 }
1119 
1120 void CPolyhedron::getFacesArea(std::vector<double>& areas) const
1121 {
1122  areas.resize(mFaces.size());
1123  std::vector<double>::iterator it2 = areas.begin();
1125  it != mFaces.end(); ++it, ++it2)
1126  *it2 = it->area(mVertices);
1127 }
1128 
1130 {
1131  // TODO. Calculate as set of pyramids whose apices are situated in the
1132  // center of the polyhedron (will work only with convex polyhedrons).
1133  // Pyramid volume=V=1/3*base area*height. Height=abs((A-V)·N), where A is
1134  // the apex, V is any other vertex, N is the base's normal vector and (·) is
1135  // the scalar product.
1136  TPoint3D center;
1137  getCenter(center);
1138  double res = 0;
1141  vector<double> areas(mFaces.size());
1142  getFacesArea(areas);
1143  vector<double>::const_iterator itA = areas.begin();
1145  it != mFaces.end(); ++it, ++itP, ++itA)
1146  res += abs(itP->plane.distance(center)) * (*itA);
1147  return res / 3;
1148 }
1149 
1150 void CPolyhedron::getSetOfPolygons(std::vector<math::TPolygon3D>& vec) const
1151 {
1153  size_t N = tempPolygons.size();
1154  vec.resize(N);
1155  for (size_t i = 0; i < N; i++) vec[i] = tempPolygons[i].poly;
1156 }
1157 
1159  std::vector<math::TPolygon3D>& vec) const
1160 {
1161  vec.resize(mFaces.size());
1162  size_t N = mVertices.size();
1163  vector<TPoint3D> nVerts;
1164  nVerts.resize(N);
1165  CPose3D pose = this->m_pose;
1166  for (size_t i = 0; i < N; i++) pose.composePoint(mVertices[i], nVerts[i]);
1167  transform(
1168  mFaces.begin(), mFaces.end(), vec.begin(),
1170 }
1171 
1173 {
1174  vector<TPolygon3D> polys, polysTMP, polys2;
1175  getSetOfPolygons(polys);
1176  polys2.reserve(polys.size());
1177  for (vector<TPolygon3D>::const_iterator it = polys.begin();
1178  it != polys.end(); ++it)
1179  if (mrpt::math::splitInConvexComponents(*it, polysTMP))
1180  polys2.insert(polys2.end(), polysTMP.begin(), polysTMP.end());
1181  else
1182  polys2.push_back(*it);
1183  mVertices.clear();
1184  mEdges.clear();
1185  mFaces.clear();
1187  for (vector<TPolyhedronFace>::iterator it = mFaces.begin();
1188  it != mFaces.end(); ++it)
1189  {
1190  if (!setNormal(*it, false))
1191  throw std::logic_error("Bad face specification");
1192  addEdges(*it);
1193  }
1194 }
1195 
1197 {
1198  size_t N = mVertices.size();
1199  if (N == 0) throw new std::logic_error("There are no vertices");
1200  center.x = center.y = center.z = 0;
1201  for (vector<TPoint3D>::const_iterator it = mVertices.begin();
1202  it != mVertices.end(); ++it)
1203  {
1204  center.x += it->x;
1205  center.y += it->y;
1206  center.z += it->z;
1207  }
1208  center.x /= N;
1209  center.y /= N;
1210  center.z /= N;
1211 }
1212 
1214 {
1215  switch (mrpt::random::getRandomGenerator().drawUniform32bit() % 34)
1216  {
1217  case 0:
1218  return CreateTetrahedron(radius);
1219  case 1:
1220  return CreateHexahedron(radius);
1221  case 2:
1222  return CreateOctahedron(radius);
1223  case 3:
1224  return CreateDodecahedron(radius);
1225  case 4:
1226  return CreateIcosahedron(radius);
1227  case 5:
1228  return CreateTruncatedTetrahedron(radius);
1229  case 6:
1230  return CreateTruncatedHexahedron(radius);
1231  case 7:
1232  return CreateTruncatedOctahedron(radius);
1233  case 8:
1234  return CreateTruncatedDodecahedron(radius);
1235  case 9:
1236  return CreateTruncatedIcosahedron(radius);
1237  case 10:
1238  return CreateCuboctahedron(radius);
1239  case 11:
1240  return CreateRhombicuboctahedron(radius);
1241  case 12:
1242  return CreateIcosidodecahedron(radius);
1243  case 13:
1244  return CreateRhombicosidodecahedron(radius);
1245  case 14:
1246  return CreateTriakisTetrahedron(radius);
1247  case 15:
1248  return CreateTriakisOctahedron(radius);
1249  case 16:
1250  return CreateTetrakisHexahedron(radius);
1251  case 17:
1252  return CreateTriakisIcosahedron(radius);
1253  case 18:
1254  return CreatePentakisDodecahedron(radius);
1255  case 19:
1256  return CreateRhombicDodecahedron(radius);
1257  case 20:
1258  return CreateDeltoidalIcositetrahedron(radius);
1259  case 21:
1260  return CreateRhombicTriacontahedron(radius);
1261  case 22:
1262  return CreateDeltoidalHexecontahedron(radius);
1263  case 23:
1265  (mrpt::random::getRandomGenerator().drawUniform32bit() % 10) +
1266  3,
1267  radius);
1268  case 24:
1270  (mrpt::random::getRandomGenerator().drawUniform32bit() % 10) +
1271  3,
1272  radius);
1273  case 25:
1275  ((mrpt::random::getRandomGenerator().drawUniform32bit() % 4)
1276  << 1) +
1277  4,
1278  radius, "C+");
1279  case 26:
1281  ((mrpt::random::getRandomGenerator().drawUniform32bit() % 4)
1282  << 1) +
1283  4,
1284  radius, "C-C+");
1285  case 27:
1287  ((mrpt::random::getRandomGenerator().drawUniform32bit() % 4)
1288  << 1) +
1289  4,
1290  radius, "C-PRC+");
1291  case 28:
1293  ((mrpt::random::getRandomGenerator().drawUniform32bit() % 4)
1294  << 1) +
1295  4,
1296  radius, "C-AC+");
1297  case 29:
1298  return CreateJohnsonSolidWithConstantBase(10, radius, "R+");
1299  case 30:
1300  return CreateJohnsonSolidWithConstantBase(10, radius, "R-PRR+");
1301  case 31:
1302  return CreateJohnsonSolidWithConstantBase(10, radius, "R-AR+");
1303  case 32:
1305  (mrpt::random::getRandomGenerator().drawUniform32bit() % 5) + 3,
1306  radius);
1307  case 33:
1309  (mrpt::random::getRandomGenerator().drawUniform32bit() % 5) + 3,
1310  radius);
1311  default:
1312  return CreateEmpty();
1313  }
1314 }
1315 
1317 {
1318  // This methods builds the dual of a given polyhedron, which is assumed to
1319  // be centered in (0,0,0), using polar reciprocation.
1320  // A vertex (x0,y0,z0), which is inside a circunference x^2+y^2+z^2=r^2, its
1321  // dual face will lie on the x0·x+y0·y+z0·z=r^2 plane.
1322  // The new vertices can, then, be calculated as the corresponding
1323  // intersections between three or more planes.
1324  size_t NV = mFaces.size();
1325  size_t NE = mEdges.size();
1326  size_t NF = mVertices.size();
1327  vector<TPlane> planes(NF);
1328  for (size_t i = 0; i < NF; i++)
1329  {
1330  const TPoint3D& p = mVertices[i];
1331  TPlane& pl = planes[i];
1332  pl.coefs[0] = p.x;
1333  pl.coefs[1] = p.y;
1334  pl.coefs[2] = p.z;
1335  pl.coefs[3] = -square(p.x) - square(p.y) - square(p.z);
1336  }
1337  CMatrixTemplate<bool> incidence(NV, NF);
1338  vector<TPoint3D> vertices(NV);
1339  for (size_t i = 0; i < NV; i++)
1340  {
1341  for (size_t j = 0; j < NF; j++) incidence(i, j) = false;
1342  vector<const TPlane*> fPls;
1343  fPls.reserve(mFaces[i].vertices.size());
1344  for (vector<uint32_t>::const_iterator it = mFaces[i].vertices.begin();
1345  it != mFaces[i].vertices.end(); ++it)
1346  {
1347  incidence(i, *it) = true;
1348  fPls.push_back(&planes[*it]);
1349  }
1350  if (!getPlanesIntersection(fPls, vertices[i]))
1351  throw std::logic_error("Dual polyhedron cannot be found");
1352  }
1353  vector<TPolyhedronFace> faces(NF);
1354  for (size_t i = 0; i < NF; i++)
1355  for (size_t j = 0; j < NV; j++)
1356  if (incidence(j, i)) faces[i].vertices.push_back(j);
1357  // The following code ensures that the faces' vertex list is in the adequate
1358  // order.
1359  CMatrixTemplate<bool> arrayEF(NE, NV);
1360  for (size_t i = 0; i < NE; i++)
1361  for (size_t j = 0; j < NV; j++)
1362  arrayEF(i, j) = faceContainsEdge(mFaces[j], mEdges[i]);
1363  for (vector<TPolyhedronFace>::iterator it = faces.begin();
1364  it != faces.end(); ++it)
1365  {
1366  vector<uint32_t>& face = it->vertices;
1367  if (face.size() <= 3) continue;
1368  size_t index = 0;
1369  while (index < face.size() - 1)
1370  {
1371  bool err = true;
1372  while (err)
1373  {
1374  for (size_t i = 0; i < NE; i++)
1375  if (arrayEF(i, face[index]) && arrayEF(i, face[index + 1]))
1376  {
1377  err = false;
1378  break;
1379  }
1380  if (err)
1381  {
1382  size_t val = face[index + 1];
1383  face.erase(face.begin() + index + 1);
1384  face.push_back(val);
1385  }
1386  }
1387  index++;
1388  }
1389  }
1390  return mrpt::make_aligned_shared<CPolyhedron>(vertices, faces);
1391 }
1392 
1394 {
1395  if (factor < 0) return CreateEmpty();
1396  if (factor == 0)
1397  return CreateNoCheck(mVertices, mFaces);
1398  else if (factor < 1)
1399  {
1400  size_t NE = mEdges.size();
1401  size_t NV = mVertices.size();
1402  size_t NF = mFaces.size();
1403  vector<TPoint3D> vertices(NE << 1);
1404  vector<TPolyhedronFace> faces(NV + NF);
1405  for (size_t i = 0; i < NE; i++)
1406  {
1407  const TPoint3D& p1 = mVertices[mEdges[i].v1];
1408  const TPoint3D& p2 = mVertices[mEdges[i].v2];
1409  TPoint3D& v1 = vertices[i + i];
1410  TPoint3D& v2 = vertices[i + i + 1];
1411  for (size_t j = 0; j < 3; j++)
1412  {
1413  double d = (p2[j] - p1[j]) * factor / 2;
1414  v1[j] = p1[j] + d;
1415  v2[j] = p2[j] - d;
1416  }
1417  faces[mEdges[i].v1].vertices.push_back(i + i);
1418  faces[mEdges[i].v2].vertices.push_back(i + i + 1);
1419  }
1420  for (size_t i = 0; i < NV; i++)
1421  {
1422  vector<uint32_t>& f = faces[i].vertices;
1423  size_t sf = f.size();
1424  if (sf == 3) continue;
1425  for (size_t j = 1; j < sf - 1; j++)
1426  {
1427  const TPolyhedronEdge& e1 = mEdges[f[j - 1] / 2];
1428  for (;;)
1429  {
1430  const TPolyhedronEdge& e2 = mEdges[f[j] / 2];
1431  if (!((e1.v1 == i || e1.v2 == i) &&
1432  (e2.v1 == i || e2.v2 == i)))
1433  THROW_EXCEPTION("En algo te has equivocado, chaval.");
1434  if (searchForFace(
1435  mFaces, i, (e1.v1 == i) ? e1.v2 : e1.v1,
1436  (e2.v1 == i) ? e2.v2 : e2.v1))
1437  break;
1438  uint32_t tmpV = f[j];
1439  f.erase(f.begin() + j);
1440  f.push_back(tmpV);
1441  }
1442  }
1443  }
1444  for (size_t i = 0; i < NF; i++)
1445  {
1446  vector<uint32_t>& f = faces[i + NV].vertices;
1447  const vector<uint32_t>& cf = mFaces[i].vertices;
1448  size_t hmV = cf.size();
1449  f.reserve(hmV << 1);
1450  for (size_t j = 0; j < hmV; j++)
1451  {
1452  size_t where;
1453  if (searchForEdge(mEdges, cf[j], cf[(j + 1) % hmV], where))
1454  {
1455  f.push_back(where << 1);
1456  f.push_back((where << 1) + 1);
1457  }
1458  else
1459  {
1460  f.push_back((where << 1) + 1);
1461  f.push_back(where << 1);
1462  }
1463  }
1464  }
1465  return mrpt::make_aligned_shared<CPolyhedron>(vertices, faces);
1466  }
1467  else if (factor == 1)
1468  {
1469  size_t NE = mEdges.size();
1470  size_t NV = mVertices.size();
1471  size_t NF = mFaces.size();
1472  vector<TPoint3D> vertices(NE);
1473  vector<TPolyhedronFace> faces(NV + NF);
1474  for (size_t i = 0; i < NE; i++)
1475  {
1476  const TPoint3D& p1 = mVertices[mEdges[i].v1];
1477  const TPoint3D& p2 = mVertices[mEdges[i].v2];
1478  TPoint3D& dst = vertices[i];
1479  for (size_t j = 0; j < 3; j++) dst[j] = (p1[j] + p2[j]) / 2;
1480  faces[mEdges[i].v1].vertices.push_back(i);
1481  faces[mEdges[i].v2].vertices.push_back(i);
1482  }
1483  for (size_t i = 0; i < NV; i++)
1484  {
1485  vector<uint32_t>& f = faces[i].vertices;
1486  size_t sf = f.size();
1487  if (sf == 3) continue;
1488  for (size_t j = 1; j < sf - 1; j++)
1489  {
1490  const TPolyhedronEdge& e1 = mEdges[f[j - 1]];
1491  for (;;)
1492  {
1493  const TPolyhedronEdge& e2 = mEdges[f[j - 1]];
1494  if (!((e1.v1 == i || e1.v2 == i) &&
1495  (e2.v1 == 1 || e2.v2 == i)))
1496  THROW_EXCEPTION("En algo te has equivocado, chaval.");
1497  if (searchForFace(
1498  mFaces, i, (e1.v1 == i) ? e1.v2 : e1.v1,
1499  (e2.v1 == i) ? e2.v2 : e2.v1))
1500  break;
1501  uint32_t tmpV = f[j];
1502  f.erase(f.begin() + j);
1503  f.push_back(tmpV);
1504  }
1505  }
1506  }
1507  for (size_t i = 0; i < NF; i++)
1508  {
1509  vector<uint32_t>& f = faces[i + NV].vertices;
1510  const vector<uint32_t>& cf = mFaces[i].vertices;
1511  size_t hmV = cf.size();
1512  f.reserve(hmV);
1513  for (size_t j = 0; j < hmV; j++)
1514  {
1515  size_t where;
1516  searchForEdge(mEdges, cf[j], cf[(j + 1) % hmV], where);
1517  f.push_back(where);
1518  }
1519  }
1520  return mrpt::make_aligned_shared<CPolyhedron>(vertices, faces);
1521  }
1522  else
1523  return CreateEmpty();
1524 }
1525 
1527 {
1528  if (factor < 0)
1529  return CreateEmpty();
1530  else if (factor == 0)
1531  return CreateNoCheck(mVertices, mFaces);
1532  size_t NV = mVertices.size();
1533  size_t NE = mEdges.size();
1534  size_t NF = mFaces.size();
1535  vector<TPolygon3D> origFaces(NF);
1536  getSetOfPolygons(origFaces);
1537  TPoint3D cnt;
1538  getCenter(cnt);
1539  vector<TPoint3D> polyCenters(NF);
1540  vector<TPoint3D> polyNewCenters(NF);
1541  size_t NNV = 0;
1542  for (size_t i = 0; i < NF; i++)
1543  {
1544  origFaces[i].getCenter(polyCenters[i]);
1545  polyCenters[i] -= cnt;
1546  polyNewCenters[i] = polyCenters[i];
1547  polyNewCenters[i] *= (1 + factor);
1548  polyNewCenters[i] += cnt;
1549  NNV += origFaces[i].size();
1550  }
1551  vector<TPoint3D> vertices(NNV);
1552  vector<TPolyhedronFace> faces(NF + NV + NE);
1553  size_t ind = 0;
1554  for (size_t i = 0; i < NF; i++)
1555  {
1556  const TPoint3D& oC = polyCenters[i];
1557  const TPoint3D& nC = polyNewCenters[i];
1558  const TPolygon3D& oP = origFaces[i];
1559  vector<uint32_t>& f = faces[i].vertices;
1560  size_t oPS = oP.size();
1561  for (size_t j = 0; j < oPS; j++)
1562  {
1563  vertices[j + ind] = nC + (oP[j] - oC);
1564  f.push_back(j + ind);
1565  size_t curr = mFaces[i].vertices[j];
1566  faces[NF + curr].vertices.push_back(j + ind);
1567  size_t edge;
1568  searchForEdge(
1569  mEdges, curr, mFaces[i].vertices[(j + oPS - 1) % oPS], edge);
1570  faces[NF + NV + edge].vertices.push_back(j + ind);
1571  searchForEdge(
1572  mEdges, curr, mFaces[i].vertices[(j + 1) % oPS], edge);
1573  faces[NF + NV + edge].vertices.push_back(j + ind);
1574  }
1575  ind += oPS;
1576  }
1578  edgeBegin = faces.begin() + NF + NV,
1579  end = faces.end();
1580  for (vector<TPolyhedronFace>::iterator it = faces.begin() + NF;
1581  it != faces.begin() + NF + NV; ++it)
1582  {
1583  vector<uint32_t>& f = it->vertices;
1584  if (f.size() == 3) continue;
1585  for (size_t i = 1; i < f.size() - 1; i++)
1586  for (;;)
1587  if (searchForEdge(edgeBegin, end, f[i - 1], f[i]))
1588  break;
1589  else
1590  {
1591  uint32_t tmp = f[i];
1592  f.erase(f.begin() + i);
1593  f.push_back(tmp);
1594  }
1595  }
1596  for (vector<TPolyhedronFace>::iterator it = faces.begin() + NF + NV;
1597  it != faces.end(); ++it)
1598  {
1599  vector<uint32_t>& f =
1600  it->vertices; // Will always have exactly 4 vertices
1601  for (size_t i = 1; i < 3; i++)
1602  for (;;)
1603  if (searchForEdge(begin, edgeBegin, f[i - 1], f[i]))
1604  break;
1605  else
1606  {
1607  uint32_t tmp = f[i];
1608  f.erase(f.begin() + i);
1609  f.push_back(tmp);
1610  }
1611  }
1612  return mrpt::make_aligned_shared<CPolyhedron>(vertices, faces);
1613 }
1614 
1616 {
1617  size_t NV = mVertices.size();
1618  size_t NF = mFaces.size();
1619  vector<TPoint3D> vertices(NV + NF);
1620  std::copy(mVertices.begin(), mVertices.end(), vertices.begin());
1621  size_t tnf = 0;
1623  it != mFaces.end(); ++it)
1624  tnf += it->vertices.size();
1625  vector<TPolyhedronFace> faces(tnf);
1626  TPolygon3D tmp;
1627  TPlane pTmp;
1628  TPoint3D cTmp;
1629  size_t iF = 0;
1630  TPoint3D phCenter;
1631  getCenter(phCenter);
1632  TPolyhedronFace fTmp;
1633  fTmp.vertices.resize(3);
1634  for (size_t i = 0; i < NF; i++)
1635  {
1636  TPoint3D& vertex = vertices[NV + i];
1637  const vector<uint32_t>& face = mFaces[i].vertices;
1638  size_t N = face.size();
1639  tmp.resize(N);
1640  for (size_t j = 0; j < N; j++) tmp[j] = mVertices[face[j]];
1641  tmp.getBestFittingPlane(pTmp);
1642  pTmp.unitarize();
1643  tmp.getCenter(cTmp);
1644  if (pTmp.evaluatePoint(phCenter) > 0)
1645  for (size_t j = 0; j < 3; j++)
1646  vertex[j] = cTmp[j] - height * pTmp.coefs[j];
1647  else
1648  for (size_t j = 0; j < 3; j++)
1649  vertex[j] = cTmp[j] + height * pTmp.coefs[j];
1650  fTmp.vertices[0] = NV + i;
1651  for (size_t j = 0; j < N; j++)
1652  {
1653  fTmp.vertices[1] = face[j];
1654  fTmp.vertices[2] = face[(j + 1) % N];
1655  faces[iF + j] = fTmp;
1656  }
1657  iF += N;
1658  }
1659  return CreateNoCheck(vertices, faces);
1660 }
1661 
1662 CPolyhedron::Ptr CPolyhedron::augment(double height, size_t numVertices) const
1663 {
1664  size_t NV = mVertices.size();
1665  size_t NF = mFaces.size();
1666  size_t tnf = 0;
1667  size_t tnv = NV;
1669  it != mFaces.end(); ++it)
1670  if (it->vertices.size() == numVertices)
1671  {
1672  tnf += numVertices;
1673  tnv++;
1674  }
1675  else
1676  tnf++;
1677  if (tnv == NV) return CreateNoCheck(mVertices, mFaces);
1678  vector<TPoint3D> vertices(tnv);
1679  std::copy(mVertices.begin(), mVertices.end(), vertices.begin());
1680  vector<TPolyhedronFace> faces(tnf);
1681  TPolygon3D tmp(numVertices);
1682  TPlane pTmp;
1683  TPoint3D cTmp;
1684  size_t iF = 0;
1685  size_t iV = NV;
1686  TPoint3D phCenter;
1687  getCenter(phCenter);
1688  TPolyhedronFace fTmp;
1689  fTmp.vertices.resize(3);
1690  for (size_t i = 0; i < NF; i++)
1691  {
1692  const vector<uint32_t>& face = mFaces[i].vertices;
1693  size_t N = face.size();
1694  if (N != numVertices)
1695  {
1696  faces[iF].vertices = face;
1697  iF++;
1698  continue;
1699  }
1700  TPoint3D& vertex = vertices[iV];
1701  for (size_t j = 0; j < numVertices; j++) tmp[j] = mVertices[face[j]];
1702  tmp.getBestFittingPlane(pTmp);
1703  pTmp.unitarize();
1704  tmp.getCenter(cTmp);
1705  if (pTmp.evaluatePoint(phCenter) > 0)
1706  for (size_t j = 0; j < 3; j++)
1707  vertex[j] = cTmp[j] - height * pTmp.coefs[j];
1708  else
1709  for (size_t j = 0; j < 3; j++)
1710  vertex[j] = cTmp[j] + height * pTmp.coefs[j];
1711  fTmp.vertices[0] = iV;
1712  for (size_t j = 0; j < N; j++)
1713  {
1714  fTmp.vertices[1] = face[j];
1715  fTmp.vertices[2] = face[(j + 1) % N];
1716  faces[iF + j] = fTmp;
1717  }
1718  iF += N;
1719  iV++;
1720  }
1721  return CreateNoCheck(vertices, faces);
1722 }
1723 
1725 {
1726  size_t NV = mVertices.size();
1727  size_t NF = mFaces.size();
1728  vector<TPoint3D> vertices(NV + NF);
1729  std::copy(mVertices.begin(), mVertices.end(), vertices.begin());
1730  size_t tnf = 0;
1732  it != mFaces.end(); ++it)
1733  tnf += it->vertices.size();
1734  vector<TPolyhedronFace> faces(tnf);
1735  TPolygon3D tmp;
1736  TPlane pTmp;
1737  TPoint3D cTmp;
1738  size_t iF = 0;
1739  TPoint3D phCenter;
1740  getCenter(phCenter);
1741  TPolyhedronFace fTmp;
1742  fTmp.vertices.resize(3);
1743  for (size_t i = 0; i < NF; i++)
1744  {
1745  TPoint3D& vertex = vertices[NV + i];
1746  const vector<uint32_t>& face = mFaces[i].vertices;
1747  size_t N = face.size();
1748  tmp.resize(N);
1749  for (size_t j = 0; j < N; j++) tmp[j] = mVertices[face[j]];
1750  tmp.getCenter(cTmp);
1751  double height = getHeight(tmp, cTmp); // throws std::logic_error
1752  tmp.getBestFittingPlane(pTmp);
1753  pTmp.unitarize();
1754  if ((pTmp.evaluatePoint(phCenter) < 0) == direction)
1755  for (size_t j = 0; j < 3; j++)
1756  vertex[j] = cTmp[j] - height * pTmp.coefs[j];
1757  else
1758  for (size_t j = 0; j < 3; j++)
1759  vertex[j] = cTmp[j] + height * pTmp.coefs[j];
1760  fTmp.vertices[0] = NV + i;
1761  for (size_t j = 0; j < N; j++)
1762  {
1763  fTmp.vertices[1] = face[j];
1764  fTmp.vertices[2] = face[(j + 1) % N];
1765  faces[iF + j] = fTmp;
1766  }
1767  iF += N;
1768  }
1769  return CreateNoCheck(vertices, faces);
1770 }
1771 
1772 CPolyhedron::Ptr CPolyhedron::augment(size_t numVertices, bool direction) const
1773 {
1774  size_t NV = mVertices.size();
1775  size_t NF = mFaces.size();
1776  size_t tnf = 0;
1777  size_t tnv = NV;
1779  it != mFaces.end(); ++it)
1780  if (it->vertices.size() == numVertices)
1781  {
1782  tnf += numVertices;
1783  tnv++;
1784  }
1785  else
1786  tnf++;
1787  if (tnv == NV) return CreateNoCheck(mVertices, mFaces);
1788  vector<TPoint3D> vertices(tnv);
1789  std::copy(mVertices.begin(), mVertices.end(), vertices.begin());
1790  vector<TPolyhedronFace> faces(tnf);
1791  TPolygon3D tmp(numVertices);
1792  TPlane pTmp;
1793  TPoint3D cTmp;
1794  size_t iF = 0;
1795  size_t iV = NV;
1796  TPoint3D phCenter;
1797  getCenter(phCenter);
1798  TPolyhedronFace fTmp;
1799  fTmp.vertices.resize(3);
1800  for (size_t i = 0; i < NF; i++)
1801  {
1802  const vector<uint32_t>& face = mFaces[i].vertices;
1803  size_t N = face.size();
1804  if (N != numVertices)
1805  {
1806  faces[iF].vertices = face;
1807  iF++;
1808  continue;
1809  }
1810  TPoint3D& vertex = vertices[iV];
1811  for (size_t j = 0; j < numVertices; j++) tmp[j] = mVertices[face[j]];
1812  tmp.getBestFittingPlane(pTmp);
1813  pTmp.unitarize();
1814  tmp.getCenter(cTmp);
1815  double height = getHeight(tmp, cTmp); // throws std::logic_error
1816  if ((pTmp.evaluatePoint(phCenter) < 0) == direction)
1817  for (size_t j = 0; j < 3; j++)
1818  vertex[j] = cTmp[j] - height * pTmp.coefs[j];
1819  else
1820  for (size_t j = 0; j < 3; j++)
1821  vertex[j] = cTmp[j] + height * pTmp.coefs[j];
1822  fTmp.vertices[0] = iV;
1823  for (size_t j = 0; j < N; j++)
1824  {
1825  fTmp.vertices[1] = face[j];
1826  fTmp.vertices[2] = face[(j + 1) % N];
1827  faces[iF + j] = fTmp;
1828  }
1829  iF += N;
1830  iV++;
1831  }
1832  return CreateNoCheck(vertices, faces);
1833 }
1834 
1836 {
1837  vector<TPoint3D> vertices(mVertices);
1838  double c = cos(angle), s = sin(angle);
1839  for (vector<TPoint3D>::iterator it = vertices.begin(); it != vertices.end();
1840  ++it)
1841  {
1842  double A = it->x;
1843  double B = it->y;
1844  it->x = A * c - B * s;
1845  it->y = B * c + A * s;
1846  }
1847  return CreateNoCheck(vertices, mFaces);
1848 }
1849 
1851 {
1852  vector<TPoint3D> vertices(mVertices);
1853  if (factor <= 0)
1854  throw std::logic_error("Factor must be a strictly positive number");
1855  for (vector<TPoint3D>::iterator it = vertices.begin(); it != vertices.end();
1856  ++it)
1857  {
1858  it->x *= factor;
1859  it->y *= factor;
1860  }
1861  return CreateNoCheck(vertices, mFaces);
1862 }
1863 
1864 vector<TPoint2D> CPolyhedron::generateBase(
1865  uint32_t numBaseEdges, double baseRadius)
1866 {
1867  vector<TPoint2D> base(numBaseEdges);
1868  for (size_t i = 0; i < numBaseEdges; i++)
1869  {
1870  double ang = 2 * M_PI * i / numBaseEdges;
1871  base[i].x = baseRadius * cos(ang);
1872  base[i].y = baseRadius * sin(ang);
1873  }
1874  return base;
1875 }
1876 
1878  uint32_t numBaseEdges, double baseRadius)
1879 {
1880  vector<TPoint2D> base(numBaseEdges);
1881  double shift = M_PI / numBaseEdges;
1882  for (size_t i = 0; i < numBaseEdges; i++)
1883  {
1884  double ang = shift + 2 * M_PI * i / numBaseEdges;
1885  base[i].x = baseRadius * cos(ang);
1886  base[i].y = baseRadius * sin(ang);
1887  }
1888  return base;
1889 }
1890 
1892  uint32_t numBaseEdges, double baseRadius, double height,
1893  vector<TPoint3D>& vec)
1894 {
1895  for (size_t i = 0; i < numBaseEdges; i++)
1896  {
1897  double ang = 2 * M_PI * i / numBaseEdges;
1898  vec.push_back(
1899  TPoint3D(baseRadius * cos(ang), baseRadius * sin(ang), height));
1900  }
1901 }
1902 
1904  uint32_t numBaseEdges, double baseRadius, double height, double shift,
1905  vector<TPoint3D>& vec)
1906 {
1907  for (size_t i = 0; i < numBaseEdges; i++)
1908  {
1909  double ang = 2 * M_PI * i / numBaseEdges + shift;
1910  vec.push_back(
1911  TPoint3D(baseRadius * cos(ang), baseRadius * sin(ang), height));
1912  }
1913 }
1914 
1916 {
1917  tempPolygons.resize(mFaces.size());
1918  transform(
1919  mFaces.begin(), mFaces.end(), tempPolygons.begin(),
1921  polygonsUpToDate = true;
1922 }
1923 
1925 {
1926  size_t N = doCheck ? f.vertices.size() : 3;
1927  TPolygon3D poly(N);
1928  for (size_t i = 0; i < N; i++) poly[i] = mVertices[f.vertices[i]];
1929  TPlane tmp;
1930  if (!poly.getPlane(tmp)) return false;
1931  tmp.getNormalVector(f.normal);
1932  TPoint3D c;
1933  getCenter(c);
1934  if (tmp.evaluatePoint(c) > 0)
1935  for (size_t i = 0; i < 3; i++) f.normal[i] = -f.normal[i];
1936  return true;
1937 }
1938 
1940 {
1941  TPolyhedronEdge e;
1943  e.v1 = *it;
1944  ++it;
1945  while (it != f.vertices.end())
1946  {
1947  e.v2 = *it;
1948  if (find(mEdges.begin(), mEdges.end(), e) == mEdges.end())
1949  mEdges.push_back(e);
1950  e.v1 = e.v2;
1951  ++it;
1952  }
1953  e.v2 = *(f.vertices.begin());
1954  if (find(mEdges.begin(), mEdges.end(), e) == mEdges.end())
1955  mEdges.push_back(e);
1956 }
1957 
1959  const vector<TPoint3D>& vertices, const vector<TPolyhedronFace>& faces)
1960 {
1961  size_t N = vertices.size();
1962  if (vertices.size() > 0)
1963  for (vector<TPoint3D>::const_iterator it = vertices.begin();
1964  it != vertices.end() - 1; ++it)
1965  for (vector<TPoint3D>::const_iterator it2 = it + 1;
1966  it2 != vertices.end(); ++it2)
1967  if (*it == *it2) return false;
1968  for (vector<TPolyhedronFace>::const_iterator it = faces.begin();
1969  it != faces.end(); ++it)
1970  {
1971  const vector<uint32_t>& e = it->vertices;
1972  for (vector<uint32_t>::const_iterator it2 = e.begin(); it2 != e.end();
1973  ++it2)
1974  if (*it2 >= N) return false;
1975  }
1976  return true;
1977 }
1978 
1979 size_t CPolyhedron::edgesInVertex(size_t vertex) const
1980 {
1981  size_t res = 0;
1983  it != mEdges.end(); ++it)
1984  if (it->v1 == vertex || it->v2 == vertex) res++;
1985  return res;
1986 }
1987 
1988 size_t CPolyhedron::facesInVertex(size_t vertex) const
1989 {
1990  size_t res = 0;
1992  it != mFaces.end(); ++it)
1993  if (find(it->vertices.begin(), it->vertices.end(), vertex) !=
1994  it->vertices.end())
1995  res++;
1996  return res;
1997 }
1998 
2001 {
2002  in >> o.v1 >> o.v2;
2003  return in;
2004 }
2005 
2007  CArchive& out, const CPolyhedron::TPolyhedronEdge& o)
2008 {
2009  out << o.v1 << o.v2;
2010  return out;
2011 }
2012 
2015 {
2016  in >> o.vertices >> o.normal[0] >> o.normal[1] >> o.normal[2];
2017  return in;
2018 }
2019 
2021  CArchive& out, const CPolyhedron::TPolyhedronFace& o)
2022 {
2023  out << o.vertices << o.normal[0] << o.normal[1] << o.normal[2];
2024  return out;
2025 }
2026 
2029 {
2030  writeToStreamRender(out);
2031  // version 0
2032  out << mVertices << mFaces << mWireframe << mLineWidth;
2033 }
2034 
2037 {
2038  switch (version)
2039  {
2040  case 0:
2042  in >> mVertices >> mFaces >> mWireframe >> mLineWidth;
2044  throw std::logic_error("Inconsistent data read from stream");
2045  for (vector<TPolyhedronFace>::iterator it = mFaces.begin();
2046  it != mFaces.end(); ++it)
2047  {
2048  if (!setNormal(*it))
2049  throw std::logic_error("Bad face specification");
2050  addEdges(*it);
2051  }
2052  break;
2053  default:
2055  };
2057 }
2058 
2060  mrpt::math::TPoint3D& bb_min, mrpt::math::TPoint3D& bb_max) const
2061 {
2062  bb_min.x = 0;
2063  bb_min.y = 0;
2064  bb_min.z = 0;
2065 
2066  bb_max.x = 0;
2067  bb_max.y = 0;
2068  bb_max.z = 0;
2069 
2070  // Convert to coordinates of my parent:
2071  m_pose.composePoint(bb_min, bb_min);
2072  m_pose.composePoint(bb_max, bb_max);
2073 }
2074 
2075 /*CPolyhedron::Ptr CPolyhedron::CreateCuboctahedron(double radius) {
2076  if (radius==0) return CreateEmpty();
2077  vector<TPoint3D> verts;
2078  vector<TPolyhedronFace> faces;
2079  double d=radius/sqrt(2.0);
2080  verts.push_back(TPoint3D(d,0,d));
2081  verts.push_back(TPoint3D(0,d,d));
2082  verts.push_back(TPoint3D(0,-d,d));
2083  verts.push_back(TPoint3D(-d,0,d));
2084  verts.push_back(TPoint3D(d,d,0));
2085  verts.push_back(TPoint3D(d,-d,0));
2086  verts.push_back(TPoint3D(-d,d,0));
2087  verts.push_back(TPoint3D(-d,-d,0));
2088  verts.push_back(TPoint3D(d,0,-d));
2089  verts.push_back(TPoint3D(0,d,-d));
2090  verts.push_back(TPoint3D(0,-d,-d));
2091  verts.push_back(TPoint3D(-d,0,-d));
2092  TPolyhedronFace f;
2093  static uint32_t faces3[]={0,1,4, 0,2,5, 1,3,6, 2,3,7, 8,9,4, 8,10,5, 9,11,6,
2094 10,11,7};
2095  static uint32_t faces4[]={0,1,3,2, 8,9,11,10, 0,4,8,5, 1,4,9,6, 2,5,10,7,
2096 3,6,11,7};
2097  for (uint32_t *p=reinterpret_cast<uint32_t
2098 *>(&faces3);p<24+reinterpret_cast<uint32_t *>(&faces3);p+=3) {
2099  f.vertices.insert(f.vertices.begin(),p,p+3);
2100  faces.push_back(f);
2101  f.vertices.clear();
2102  }
2103  for (uint32_t *p=reinterpret_cast<uint32_t
2104 *>(&faces4);p<24+reinterpret_cast<uint32_t *>(&faces4);p+=4) {
2105  f.vertices.insert(f.vertices.begin(),p,p+4);
2106  faces.push_back(f);
2107  f.vertices.clear();
2108  }
2109  return CreateNoCheck(verts,faces);
2110 }*/
2111 
2113 {
2114  CPolyhedron::Ptr tetra =
2115  CreateJohnsonSolidWithConstantBase(3, radius * sqrt(8.0) / 3.0, "P+");
2116  for (vector<TPoint3D>::iterator it = tetra->mVertices.begin();
2117  it != tetra->mVertices.end(); ++it)
2118  it->z -= radius / 3;
2119  return tetra;
2120 }
2122 {
2123  if (radius == 0.0) return CreateEmpty();
2124  double r = radius / sqrt(3.0);
2125  return CreateCubicPrism(-r, r, -r, r, -r, r);
2126 }
2128 {
2129  return CreateJohnsonSolidWithConstantBase(4, radius, "P-P+");
2130 }
2132 {
2133  return CreateIcosahedron(radius / sqrt(15 - 6 * sqrt(5.0)))->getDual();
2134 }
2136 {
2137  double ang = M_PI / 5;
2138  double s2 = 4 * square(sin(ang));
2139  double prop = sqrt(s2 - 1) + sqrt(s2 - 2 + 2 * cos(ang)) / 2;
2140  return CreateJohnsonSolidWithConstantBase(5, radius / prop, "P-AP+", 1);
2141 }
2143 {
2144  return CreateTetrahedron(radius * sqrt(27.0 / 11.0))->truncate(2.0 / 3.0);
2145 }
2147 {
2148  return CreateHexahedron(radius * sqrt(1.5))->truncate(1.0);
2149 }
2151 {
2152  return CreateHexahedron(radius * sqrt(3.0 / (5 - sqrt(8.0))))
2153  ->truncate(2 - sqrt(2.0));
2154 }
2156 {
2157  return CreateOctahedron(radius * 3 / sqrt(5.0))->truncate(2.0 / 3.0);
2158 }
2160  double radius, bool type)
2161 {
2163  8, radius / sqrt(1 + square(sin(M_PI / 8))),
2164  type ? "C-PRC+" : "GC-PRC+", 3);
2165 }
2167 {
2169  10, radius, type ? "GR-R+" : "R-R+", 1);
2170 }
2172 {
2173  return CreateDodecahedron(radius * sqrt(45.0) / sqrt(27 + 6 * sqrt(5.0)))
2174  ->truncate(1 - sqrt(0.2));
2175 }
2177 {
2178  return CreateIcosahedron(radius * sqrt(45.0) / sqrt(25 + 4 * sqrt(5.0)))
2179  ->truncate(2.0 / 3.0);
2180 }
2182 {
2183  return CreateIcosahedron(radius * sqrt(10.0 / (35.0 + 9.0 * sqrt(5.0))))
2184  ->cantellate(1.5 * (sqrt(5.0) - 1));
2185 }
2187 {
2188  return CreateJohnsonSolidWithConstantBase(10, radius, "R+");
2189 }
2191 {
2192  return CreateTruncatedTetrahedron(radius * 3 / sqrt(33.0))->getDual();
2193 }
2195 {
2196  return CreateCuboctahedron(radius / sqrt(2.0))->getDual();
2197 }
2199 {
2200  return CreateTruncatedHexahedron(radius / sqrt((5 - sqrt(8.0))))->getDual();
2201 }
2203 {
2204  return CreateTruncatedOctahedron(radius * sqrt(0.6))->getDual();
2205 }
2207 {
2208  return CreateRhombicuboctahedron(radius / sqrt(7 - sqrt(32.0)), true)
2209  ->getDual();
2210 }
2212 {
2213  return CreateIcosidodecahedron(radius * sqrt(2 / (5 - sqrt(5.0))), true)
2214  ->getDual();
2215 }
2217 {
2218  return CreateTruncatedDodecahedron(radius * sqrt(5 / (25 - 8 * sqrt(5.0))))
2219  ->getDual();
2220 }
2222 {
2223  return CreateTruncatedIcosahedron(radius * sqrt(3 / (17 - 6 * sqrt(5.0))))
2224  ->getDual();
2225 }
2227 {
2228  return CreateRhombicosidodecahedron(radius * 3.0 / sqrt(15 - 2 * sqrt(5.0)))
2229  ->getDual();
2230 }
2232  const TPoint3D& p1, const TPoint3D& p2)
2233 {
2234  return CreateCubicPrism(p1.x, p2.x, p1.y, p2.y, p1.z, p2.z);
2235 }
2237  const vector<TPoint2D>& baseVertices, double height, double ratio)
2238 {
2239  return CreateTruncatedPyramid(baseVertices, height, ratio);
2240 }
2242  const vector<TPoint2D>& baseVertices, double height)
2243 {
2244  return CreateTruncatedPyramid(baseVertices, height, 1.0);
2245 }
2247  uint32_t numBaseEdges, double baseRadius, double height)
2248 {
2249  return CreateCustomAntiprism(
2250  generateBase(numBaseEdges, baseRadius),
2251  generateShiftedBase(numBaseEdges, baseRadius), height);
2252 }
2254  uint32_t numBaseEdges, double baseRadius, double height)
2255 {
2256  return CreateCustomPrism(generateBase(numBaseEdges, baseRadius), height);
2257 }
2259  uint32_t numBaseEdges, double baseRadius, double height)
2260 {
2261  return CreatePyramid(generateBase(numBaseEdges, baseRadius), height);
2262 }
2264  uint32_t numBaseEdges, double baseRadius, double height1, double height2)
2265 {
2266  return CreateDoublePyramid(
2267  generateBase(numBaseEdges, baseRadius), height1, height2);
2268 }
2270  uint32_t numBaseEdges, double baseRadius)
2271 {
2272  return CreateJohnsonSolidWithConstantBase(numBaseEdges, baseRadius, "PR");
2273 }
2275  uint32_t numBaseEdges, double baseRadius)
2276 {
2277  return CreateJohnsonSolidWithConstantBase(numBaseEdges, baseRadius, "A");
2278 }
2280  uint32_t numBaseEdges, double baseRadius, double height, double ratio)
2281 {
2282  return CreateTruncatedPyramid(
2283  generateBase(numBaseEdges, baseRadius), height, ratio);
2284 }
2286  uint32_t numBaseEdges, double baseRadius, double height, double ratio)
2287 {
2289  numBaseEdges, baseRadius, height, ratio);
2290 }
2292  uint32_t numBaseEdges, double baseRadius, double height1, double ratio1,
2293  double height2, double ratio2)
2294 {
2295  return CreateBifrustum(
2296  generateBase(numBaseEdges, baseRadius), height1, ratio1, height2,
2297  ratio2);
2298 }
2300  uint32_t numBaseEdges, double edgeLength)
2301 {
2303  numBaseEdges, edgeLength / (2 * sin(M_PI / numBaseEdges)), "C+");
2304 }
2306  uint32_t numBaseEdges, double height)
2307 {
2308  return CreateArchimedeanRegularAntiprism(numBaseEdges, height)->getDual();
2309 }
2311  uint32_t numBaseEdges, double height)
2312 {
2313  return CreateArchimedeanRegularPrism(numBaseEdges, height)->getDual();
2314 }
2315 
2317  const vector<TPoint3D>& vertices, const vector<TPolyhedronFace>& faces)
2318 {
2319  return CPolyhedron::Ptr(new CPolyhedron(vertices, faces, false));
2320 }
2322 {
2323  return CPolyhedron::Ptr(new CPolyhedron());
2324 }
static CPolyhedron::Ptr CreateEmpty()
Creates an empty Polyhedron.
static CPolyhedron::Ptr CreateRhombicuboctahedron(double radius, bool type=true)
Creates a rhombicuboctahedron, with 18 squares and 8 triangles (see http://en.wikipedia.org/wiki/Rhombicuboctahedron), calculated as an elongated square bicupola.
static CPolyhedron::Ptr CreateBifrustum(const std::vector< mrpt::math::TPoint2D > &baseVertices, double height1, double ratio1, double height2, double ratio2)
Creates a bifrustum, or double truncated pyramid, given a base which will lie on the XY plane...
static CPolyhedron::Ptr CreateTrapezohedron(uint32_t numBaseEdges, double baseRadius, double basesDistance)
Creates a trapezohedron, consisting of 2*N kites, where N is the number of edges in the base...
Scalar * iterator
Definition: eigen_plugins.h:26
bool mWireframe
This flag determines whether the polyhedron will be displayed as a solid object or as a set of edges...
Definition: CPolyhedron.h:124
void insertCupola(size_t numBaseEdges, double angleShift, double baseRadius, double edgeLength, bool isRotated, bool isUpwards, size_t base, vector< TPoint3D > &verts, vector< CPolyhedron::TPolyhedronFace > &faces)
static CPolyhedron::Ptr CreateRhombicTriacontahedron(double radius)
Creates a rhombic triacontahedron, dual to the icosidodecahedron.
static CPolyhedron::Ptr CreateIcosidodecahedron(double radius, bool type=true)
Creates an icosidodecahedron, with 12 pentagons and 20 triangles (see http://en.wikipedia.org/wiki/Icosidodecahedron).
void addEdges(const TPolyhedronFace &e)
Adds, to the existing list of edges, each edge in a given face.
bool splitInConvexComponents(const TPolygon2D &poly, std::vector< TPolygon2D > &components)
Splits a 2D polygon into convex components.
Definition: geometry.cpp:2397
GLdouble GLdouble z
Definition: glext.h:3872
size_t facesInVertex(size_t vertex) const
Returns how many faces converge in a given vertex.
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
const_iterator find(const KEY &key) const
Definition: ts_hash_map.h:217
size_t edgesInVertex(size_t vertex) const
Returns how many edges converge in a given vertex.
GLAPI void GLAPIENTRY glEnable(GLenum cap)
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
JohnsonBodyPart
Definition: CPolyhedron.cpp:79
mrpt::serialization::CArchive & operator<<(mrpt::serialization::CArchive &out, const mrpt::opengl::CLight &o)
Definition: CLight.cpp:128
double getVolume() const
Gets the polyhedron volume.
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
static std::vector< mrpt::math::TPoint2D > generateShiftedBase(uint32_t numBaseEdges, double baseRadius)
Generates a list of 2D vertices constituting a regular polygon, with an angle shift which makes it su...
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
void InitFromVertAndFaces(const std::vector< mrpt::math::TPoint3D > &vertices, const std::vector< TPolyhedronFace > &faces, bool doCheck=true)
Definition: CPolyhedron.h:914
static CPolyhedron::Ptr CreateCuboctahedron(double radius)
Creates a cuboctahedron, consisting of six square faces and eight triangular ones (see http://en...
void makeConvexPolygons()
Recomputes polygons, if necessary, so that each one is convex.
bool getVerticesAndFaces(const vector< math::TPolygon3D > &polys, vector< TPoint3D > &vertices, vector< CPolyhedron::TPolyhedronFace > &faces)
Definition: CPolyhedron.cpp:49
static CPolyhedron::Ptr CreateTriakisIcosahedron(double radius)
Creates a triakis icosahedron, dual to the truncated dodecahedron.
GLAPI void GLAPIENTRY glNormal3f(GLfloat nx, GLfloat ny, GLfloat nz)
GLenum GLsizei n
Definition: glext.h:5074
This file implements several operations that operate element-wise on individual or pairs of container...
CPolyhedron::Ptr getDual() const
Given a polyhedron, creates its dual.
EIGEN_STRONG_INLINE iterator begin()
Definition: eigen_plugins.h:29
std::shared_ptr< CPolyhedron > Ptr
Definition: CPolyhedron.h:46
static CPolyhedron::Ptr CreateCatalanDoublePyramid(uint32_t numBaseEdges, double height)
Creates a double pyramid whose dual is exactly an archimedean prism.
size_t additionalFaces(JohnsonBodyPart j, uint32_t numBaseEdges)
CPolyhedron::Ptr rotate(double angle) const
Rotates a polyhedron around the Z axis a given amount of radians.
Standard object for storing any 3D lightweight object.
EIGEN_STRONG_INLINE void notifyChange() const
Must be called to notify that the object has changed (so, the display list must be updated) ...
static CPolyhedron::Ptr CreateTetrakisHexahedron(double radius)
Creates a tetrakis hexahedron, dual to the truncated octahedron.
static CPolyhedron::Ptr CreateIcosahedron(double radius)
Creates a regular icosahedron (see http://en.wikipedia.org/wiki/Icosahedron).
static CPolyhedron::Ptr CreateRegularPrism(uint32_t numBaseEdges, double baseRadius, double height)
Creates a regular prism whose base is a regular polygon and whose edges are either parallel or perpen...
STL namespace.
uint8_t B
Definition: TColor.h:46
static CPolyhedron::Ptr CreateArchimedeanRegularAntiprism(uint32_t numBaseEdges, double baseRadius)
Creates a regular antiprism whose lateral polygons are equilateral triangles, and so each face of its...
#define GL_ONE_MINUS_SRC_ALPHA
Definition: glew.h:287
uint8_t G
Definition: TColor.h:46
GLenum GLenum GLuint components
Definition: glext.h:7282
bool getPlanesIntersection(const vector< const TPlane *> &planes, TPoint3D &pnt)
static CPolyhedron::Ptr CreateCubicPrism(double x1, double x2, double y1, double y2, double z1, double z2)
Creates a cubic prism, given the coordinates of two opposite vertices.
GLdouble s
Definition: glext.h:3676
#define GL_POLYGON
Definition: glew.h:281
#define GL_LIGHTING
Definition: glew.h:385
GLsizei GLsizei GLuint * obj
Definition: glext.h:4070
GLAPI void GLAPIENTRY glLineWidth(GLfloat width)
mrpt::poses::CPose3D m_pose
6D pose wrt the parent coordinate reference.
Definition: CRenderizable.h:55
static CPolyhedron::Ptr CreateCupola(uint32_t numBaseEdges, double edgeLength)
Creates a cupola.
This class represents arbitrary polyhedra.
Definition: CPolyhedron.h:44
const vector< TPoint3D > & verts
Definition: CPolyhedron.cpp:37
static CPolyhedron::Ptr CreateTriakisTetrahedron(double radius)
Creates a triakis tetrahedron, dual to the truncated tetrahedron.
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
static CPolyhedron::Ptr CreateJohnsonSolidWithConstantBase(uint32_t numBaseEdges, double baseRadius, const std::string &components, size_t shifts=0)
Creates a series of concatenated solids (most of which are prismatoids) whose base is a regular polyg...
CPolyhedron::Ptr cantellate(double factor) const
Cantellates a polyhedron to a given factor.
T square(const T x)
Inline function for the square of a number.
bool traceRay(const mrpt::poses::CPose3D &o, double &dist) const override
Ray trace.
static CPolyhedron::Ptr CreateTetrahedron(double radius)
Creates a regular tetrahedron (see http://en.wikipedia.org/wiki/Tetrahedron).
static CPolyhedron::Ptr CreateCatalanTrapezohedron(uint32_t numBaseEdges, double height)
Creates a trapezohedron whose dual is exactly an archimedean antiprism.
static CPolyhedron::Ptr CreateTruncatedPyramid(const std::vector< mrpt::math::TPoint2D > &baseVertices, double height, double ratio)
Creates a truncated pyramid, using a set of vertices which will lie on the XY plane.
static CPolyhedron::Ptr CreateTruncatedDodecahedron(double radius)
Creates a truncated dodecahedron, consisting of 12 dodecagons and 20 triangles (see http://en...
std::vector< uint32_t > vertices
Vector of indices to the vertex list.
Definition: CPolyhedron.h:92
This base provides a set of functions for maths stuff.
std::vector< mrpt::math::TPolygonWithPlane > tempPolygons
Mutable list of actual polygons, maintained for speed.
Definition: CPolyhedron.h:133
static CPolyhedron::Ptr CreateRandomPolyhedron(double radius)
Creates a random polyhedron from the static methods.
static CPolyhedron::Ptr CreateFrustum(const std::vector< mrpt::math::TPoint2D > &baseVertices, double height, double ratio)
This is a synonym for CreateTruncatedPyramid.
GLuint dst
Definition: glext.h:7135
void writeToStreamRender(mrpt::serialization::CArchive &out) const
static CPolyhedron::Ptr CreateRhombicosidodecahedron(double radius)
Creates a rhombicosidodecahedron, consisting of 30 squares, 12 pentagons and 20 triangles (see http:/...
void insertRotunda(double angleShift, double baseRadius, bool isRotated, bool isUpwards, size_t base, vector< TPoint3D > &verts, vector< CPolyhedron::TPolyhedronFace > &faces)
FCreatePolygonFromFace(const vector< TPoint3D > &v)
Definition: CPolyhedron.cpp:38
static CPolyhedron::Ptr CreatePentagonalRotunda(double radius)
Creates a pentagonal rotunda (half an icosidodecahedron), consisting of six pentagons, ten triangles and a decagon (see http://en.wikipedia.org/wiki/Pentagonal_rotunda).
Struct used to store a polyhedron face.
Definition: CPolyhedron.h:89
void getSetOfPolygons(std::vector< math::TPolygon3D > &vec) const
Gets the polyhedron as a set of polygons.
static CPolyhedron::Ptr CreateTruncatedOctahedron(double radius)
Creates a truncated octahedron, with eight hexagons and eight squares (see http://en.wikipedia.org/wiki/Truncated_octahedron).
GLuint index
Definition: glext.h:4054
const GLubyte * c
Definition: glext.h:6313
static CPolyhedron::Ptr CreatePyramid(const std::vector< mrpt::math::TPoint2D > &baseVertices, double height)
Creates a custom pyramid, using a set of 2D vertices which will lie on the XY plane.
static CPolyhedron::Ptr CreateRegularPyramid(uint32_t numBaseEdges, double baseRadius, double height)
Creates a regular pyramid whose base is a regular polygon.
void unitarize()
Unitarize normal vector.
GLuint GLuint end
Definition: glext.h:3528
CPolyhedron()
Basic empty constructor.
Definition: CPolyhedron.h:889
Struct used to store a polyhedron edge.
Definition: CPolyhedron.h:52
bool searchForEdge(const vector< CPolyhedron::TPolyhedronEdge > &es, uint32_t v1, uint32_t v2, size_t &where)
GLfloat GLfloat GLfloat GLfloat v3
Definition: glext.h:4109
GLubyte g
Definition: glext.h:6279
3D Plane, represented by its equation
static CPolyhedron::Ptr CreateTriakisOctahedron(double radius)
Creates a triakis octahedron, dual to the truncated hexahedron.
int val
Definition: mrpt_jpeglib.h:955
static CPolyhedron::Ptr CreateOctahedron(double radius)
Creates a regular octahedron (see http://en.wikipedia.org/wiki/Octahedron).
bool getPlane(TPlane &p) const
Gets a plane which contains the polygon.
GLubyte GLubyte b
Definition: glext.h:6279
static std::vector< mrpt::math::TPoint2D > generateBase(uint32_t numBaseEdges, double baseRadius)
Generates a list of 2D vertices constituting a regular polygon.
double x
X,Y,Z coordinates.
GLAPI void GLAPIENTRY glBegin(GLenum mode)
#define GL_BLEND
Definition: glew.h:432
GLsizei const GLchar ** string
Definition: glext.h:4101
static CPolyhedron::Ptr CreateParallelepiped(const mrpt::math::TPoint3D &base, const mrpt::math::TPoint3D &v1, const mrpt::math::TPoint3D &v2, const mrpt::math::TPoint3D &v3)
Creates a parallelepiped, given a base point and three vectors represented as points.
std::vector< TPolyhedronEdge > mEdges
List of polyhedron&#39;s edges.
Definition: CPolyhedron.h:115
std::vector< mrpt::math::TPoint3D > mVertices
List of vertices presents in the polyhedron.
Definition: CPolyhedron.h:111
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...
GLAPI void GLAPIENTRY glVertex3f(GLfloat x, GLfloat y, GLfloat z)
uint8_t R
Definition: TColor.h:46
CPolyhedron::Ptr scale(double factor) const
Scales a polyhedron to a given factor.
GLAPI void GLAPIENTRY glColor4ub(GLubyte red, GLubyte green, GLubyte blue, GLubyte alpha)
size_t additionalVertices(JohnsonBodyPart j, uint32_t numBaseEdges)
static CPolyhedron::Ptr CreateCustomAntiprism(const std::vector< mrpt::math::TPoint2D > &bottomBase, const std::vector< mrpt::math::TPoint2D > &topBase, double height)
Creates a custom antiprism, using two custom bases.
mrpt::img::TColor m_color
Color components in the range [0,255].
Definition: CRenderizable.h:52
static CPolyhedron::Ptr CreateTruncatedTetrahedron(double radius)
Creates a truncated tetrahedron, consisting of four triangular faces and for hexagonal ones (see http...
static CPolyhedron::Ptr CreateRegularAntiprism(uint32_t numBaseEdges, double baseRadius, double height)
Creates an antiprism whose base is a regular polygon.
#define GL_SRC_ALPHA
Definition: glew.h:286
static void checkOpenGLError()
Checks glGetError and throws an exception if an error situation is found.
const GLdouble * v
Definition: glext.h:3678
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
bool polygonsUpToDate
Whether the set of actual polygons is up to date or not.
Definition: CPolyhedron.h:137
bool setNormal(TPolyhedronFace &f, bool doCheck=true)
Calculates the normal vector to a face.
bool analyzeJohnsonPartsString(const std::string &components, uint32_t numBaseEdges, vector< JohnsonBodyPart > &parts)
Definition: CPolyhedron.cpp:96
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:52
GLdouble GLdouble GLdouble r
Definition: glext.h:3705
GLfloat GLfloat v1
Definition: glext.h:4105
void updatePolygons() const
Updates the mutable list of polygons used in rendering and ray tracing.
static CPolyhedron::Ptr CreateRhombicDodecahedron(double radius)
Creates a rhombic dodecahedron, dual to the cuboctahedron.
static CPolyhedron::Ptr CreateRegularDoublePyramid(uint32_t numBaseEdges, double baseRadius, double height1, double height2)
Creates a regular double pyramid whose base is a regular polygon.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:86
double & operator[](size_t i)
static CPolyhedron::Ptr CreateRegularBifrustum(uint32_t numBaseEdges, double baseRadius, double height1, double ratio1, double height2, double ratio2)
Creates a bifrustum (double truncated pyramid) whose base is a regular polygon lying in the XY plane...
static CPolyhedron::Ptr CreateNoCheck(const std::vector< mrpt::math::TPoint3D > &vertices, const std::vector< TPolyhedronFace > &faces)
Creates a polyhedron without checking its correctness.
double coefs[4]
Plane coefficients, stored as an array: .
double getEpsilon()
Gets the value of the geometric epsilon (default = 1e-5)
Definition: geometry.cpp:27
void composePoint(double lx, double ly, double lz, double &gx, double &gy, double &gz, mrpt::math::CMatrixFixedNumeric< double, 3, 3 > *out_jacobian_df_dpoint=nullptr, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dpose=nullptr, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dse3=nullptr, 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:379
static CPolyhedron::Ptr CreateTruncatedIcosahedron(double radius)
Creates a truncated icosahedron, consisting of 20 hexagons and 12 pentagons.
GLuint GLsizei GLsizei * length
Definition: glext.h:4064
static CPolyhedron::Ptr CreateDodecahedron(double radius)
Creates a regular dodecahedron (see http://en.wikipedia.org/wiki/Dodecahedron).
GLuint in
Definition: glext.h:7274
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:15
static CPolyhedron::Ptr CreateRegularTruncatedPyramid(uint32_t numBaseEdges, double baseRadius, double height, double ratio)
Creates a regular truncated pyramid whose base is a regular polygon.
double evaluatePoint(const TPoint3D &point) const
Evaluate a point in the plane&#39;s equation.
GLAPI void GLAPIENTRY glEnd(void)
mrpt::serialization::CArchive & operator>>(mrpt::serialization::CArchive &in, mrpt::opengl::CLight &o)
Definition: CLight.cpp:122
CPolyhedron::Ptr truncate(double factor) const
Truncates a polyhedron to a given factor.
static CPolyhedron::Ptr CreateArchimedeanRegularPrism(uint32_t numBaseEdges, double baseRadius)
Creates a regular prism whose lateral area is comprised of squares, and so each face of its is a regu...
GLenum GLint GLint y
Definition: glext.h:3538
static CPolyhedron::Ptr CreateDeltoidalHexecontahedron(double radius)
Creates a deltoidal hexecontahedron, dual to the rhombicosidodecahedron.
static CPolyhedron::Ptr CreateHexahedron(double radius)
Creates a regular cube, also called hexahedron (see http://en.wikipedia.org/wiki/Hexahedron).
void getEdgesLength(std::vector< double > &lengths) const
Gets a vector with each edge&#39;s length.
static CPolyhedron::Ptr CreateRegularFrustum(uint32_t numBaseEdges, double baseRadius, double height, double ratio)
This is a synonym for CreateRegularTruncatedPyramid.
static bool checkConsistence(const std::vector< mrpt::math::TPoint3D > &vertices, const std::vector< TPolyhedronFace > &faces)
Checks whether a set of faces is suitable for a set of vertices.
double mLineWidth
When displaying as wireframe object, this variable stores the width of the edges. ...
Definition: CPolyhedron.h:129
T operator()(const CPolyhedron::TPolyhedronFace &f)
Definition: CPolyhedron.cpp:40
void render_dl() const override
Render.
GLfloat GLfloat GLfloat v2
Definition: glext.h:4107
#define GL_LINES
Definition: glew.h:273
double operator[](size_t i) const
GLuint res
Definition: glext.h:7268
GLenum GLint x
Definition: glext.h:3538
void getCenter(mrpt::math::TPoint3D &center) const
Gets the center of the polyhedron.
std::vector< TPolyhedronFace > mFaces
List of polyhedron&#39;s faces.
Definition: CPolyhedron.h:119
void getFacesArea(std::vector< double > &areas) const
Gets a vector with each face&#39;s area.
Lightweight 3D point.
GLuint GLenum GLenum transform
Definition: glext.h:6975
void getBestFittingPlane(TPlane &p) const
Gets the best fitting plane, disregarding whether the polygon actually fits inside or not...
double getHeight(const TPolygon3D &p, const TPoint3D &c)
GLenum GLsizei GLsizei height
Definition: glext.h:3554
unsigned __int32 uint32_t
Definition: rptypes.h:47
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
GLAPI void GLAPIENTRY glDisable(GLenum cap)
static CPolyhedron::Ptr CreatePentakisDodecahedron(double radius)
Creates a pentakis dodecahedron, dual to the truncated icosahedron.
GLubyte GLubyte GLubyte a
Definition: glext.h:6279
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
GLfloat GLfloat p
Definition: glext.h:6305
bool searchForFace(const vector< CPolyhedron::TPolyhedronFace > &fs, uint32_t v1, uint32_t v2, uint32_t v3)
bool intersect(const TSegment3D &s1, const TSegment3D &s2, TObject3D &obj)
Gets the intersection between two 3D segments.
Definition: geometry.cpp:631
const Scalar * const_iterator
Definition: eigen_plugins.h:27
static CPolyhedron::Ptr CreateDeltoidalIcositetrahedron(double radius)
Creates a deltoidal icositetrahedron, dual to the rhombicuboctahedron.
static CPolyhedron::Ptr CreateCustomPrism(const std::vector< mrpt::math::TPoint2D > &baseVertices, double height)
Creates a custom prism with vertical edges, given any base which will lie on the XY plane...
GLuint GLuint GLsizei GLenum type
Definition: glext.h:3528
void readFromStreamRender(mrpt::serialization::CArchive &in)
3D polygon, inheriting from std::vector<TPoint3D>
double distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.
Definition: geometry.cpp:1891
void getCenter(TPoint3D &p) const
Get polygon&#39;s central point.
static CPolyhedron::Ptr CreateDoublePyramid(const std::vector< mrpt::math::TPoint2D > &baseVertices, double height1, double height2)
Creates a double pyramid, using a set of 2D vertices which will lie on the XY plane.
GLenum GLuint GLint GLenum face
Definition: glext.h:8194
uint8_t A
Definition: TColor.h:46
CPolyhedron::Ptr augment(double height) const
Augments a polyhedron to a given height.
bool faceContainsEdge(const CPolyhedron::TPolyhedronFace &f, const CPolyhedron::TPolyhedronEdge &e)
static CPolyhedron::Ptr CreateTruncatedHexahedron(double radius)
Creates a truncated hexahedron, with six octogonal faces and eight triangular ones (see http://en...
void getSetOfPolygonsAbsolute(std::vector< math::TPolygon3D > &vec) const
Gets the polyhedron as a set of polygons, with the pose transformation already applied.
void getNormalVector(double(&vec)[3]) const
Get plane&#39;s normal vector.
3D line, represented by a base point and a director vector.



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