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



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020