Main MRPT website > C++ reference for MRPT 1.5.7
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-2017, 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>
18 #include <mrpt/utils/CStream.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::utils;
27 using namespace mrpt::poses;
28 using namespace std;
29 
31 
32 //Auxiliary data and code
33 template<class T> class FCreatePolygonFromFace {
34 public:
35  const vector<TPoint3D> &verts;
36  FCreatePolygonFromFace(const vector<TPoint3D> &v):verts(v) {}
39  p=TPolygon3D(f.vertices.size());
40  for (size_t i=0;i<f.vertices.size();i++) p[i]=verts[f.vertices[i]];
41  return p;
42  }
43 };
44 
45 bool getVerticesAndFaces(const vector<math::TPolygon3D> &polys,vector<TPoint3D> &vertices,vector<CPolyhedron::TPolyhedronFace> &faces) {
46  vertices.reserve(4*polys.size());
47  faces.reserve(polys.size());
48  for (std::vector<math::TPolygon3D>::const_iterator it=polys.begin();it!=polys.end();++it) {
49  size_t N=it->size();
50  if (N<3) return false;
52  f.vertices.resize(N);
53  for (size_t i=0;i<N;i++) {
54  vector<TPoint3D>::iterator it2=find(vertices.begin(),vertices.end(),(*it)[i]);
55  if (it2==vertices.end()) {
56  f.vertices[i]=vertices.size();
57  vertices.push_back((*it)[i]);
58  } else f.vertices[i]=it2-vertices.begin();
59  }
60  faces.push_back(f);
61  }
62  return true;
63 }
64 
67 };
68 bool analyzeJohnsonPartsString(const std::string &components,uint32_t numBaseEdges,vector<JohnsonBodyPart> &parts) {
69  size_t N=components.length();
70  size_t i=0;
71  bool rot=false;
72  while (i<N) {
73  switch (components[i]) {
74  case 'A':case 'a':
75  if (parts.size()==0) parts.push_back(INF_NO_BODY);
76  parts.push_back(ANTIPRISM);
77  break;
78  case 'C':case 'c':
79  if (numBaseEdges&1) return false;
80  if (i==N-1) return false;
81  i++;
82  if (components[i]=='+') {
83  if (i!=N-1) return false;
84  if (parts.size()==0) parts.push_back(INF_NO_BODY);
85  parts.push_back(rot?ROTATED_UPWARDS_CUPOLA:UPWARDS_CUPOLA);
86  rot=false;
87  } else if (components[i]=='-') {
88  if (parts.size()>0) return false;
89  parts.push_back(rot?ROTATED_DOWNWARDS_CUPOLA:DOWNWARDS_CUPOLA);
90  rot=false;
91  } else return false;
92  break;
93  case 'R':case 'r':
94  if (numBaseEdges!=10) return false;
95  if (i==N-1) return false;
96  i++;
97  if (components[i]=='+') {
98  if (i!=N-1) return false;
99  if (parts.size()==0) parts.push_back(INF_NO_BODY);
100  parts.push_back(rot?ROTATED_UPWARDS_ROTUNDA:UPWARDS_ROTUNDA);
101  rot=false;
102  } else if (components[i]=='-') {
103  if (parts.size()>0) return false;
104  parts.push_back(rot?ROTATED_DOWNWARDS_ROTUNDA:DOWNWARDS_ROTUNDA);
105  rot=false;
106  } else return false;
107  break;
108  case 'G':case 'g':
109  if (i==N-1) return false;
110  i++;
111  if (components[i]=='C'||components[i]=='R') {
112  rot=true;
113  continue;
114  } else return false;
115  case 'P':case 'p':
116  if (i==N-1) return false;
117  i++;
118  switch (components[i]) {
119  case '+':
120  if (numBaseEdges>5) return false;
121  if (i!=N-1) return false;
122  if (parts.size()==0) parts.push_back(INF_NO_BODY);
123  parts.push_back(UPWARDS_PYRAMID);
124  break;
125  case '-':
126  if (numBaseEdges>5) return false;
127  if (i!=1) return false;
128  parts.push_back(DOWNWARDS_PYRAMID);
129  break;
130  case 'R':case 'r':
131  if (parts.size()>0&&(*parts.rbegin()==PRISM)) return false;
132  if (parts.size()==0) parts.push_back(INF_NO_BODY);
133  parts.push_back(PRISM);
134  break;
135  default:
136  return false;
137  }
138  break;
139  default:
140  return false;
141  }
142  i++;
143  }
144  if (parts.size()==0) return false;
145  JohnsonBodyPart p=*parts.rbegin();
147  return true;
148 }
149 inline size_t additionalVertices(JohnsonBodyPart j,uint32_t numBaseEdges) {
150  if (j==INF_NO_BODY||j==SUP_NO_BODY) return 0;
151  else if (j<UPWARDS_CUPOLA) return 1; //j is a pyramid
152  else if (j<PRISM) return numBaseEdges>>1; //j is a cupola
153  else if (j<UPWARDS_ROTUNDA) return numBaseEdges; //j is a prism or antiprism
154  else return 10; //j is a rotunda
155 }
156 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) {
157  size_t edges2=numBaseEdges>>1;
158  double minorRadius=baseRadius*sin(M_PI/numBaseEdges)/sin(M_PI/edges2);
159  //"Proper base"'s apothem=base radius*cos(2*pi/(2*numBaseEdges))
160  //"Small base"'s apothem=small radius*cos(2*pi/2*(numBaseEdges/2))
161  //Cupola's height is so that (Da^2+Height^2=Edge length^2, where Da is the difference between both apothems.
162  double h=sqrt(square(edgeLength)-square(baseRadius*cos(M_PI/numBaseEdges)-minorRadius*cos(M_PI/edges2)));
163  double height=verts[base].z+(isUpwards?h:-h);
164  angleShift+=M_PI/edges2+(isRotated?-M_PI/numBaseEdges:M_PI/numBaseEdges);
165  size_t minorBase=verts.size();
166  for (size_t i=0;i<edges2;i++) {
167  double ang=angleShift+2*M_PI*i/edges2;
168  verts.push_back(TPoint3D(minorRadius*cos(ang),minorRadius*sin(ang),height));
169  }
170  CPolyhedron::TPolyhedronFace tri,quad,cBase;
171  tri.vertices.resize(3);
172  quad.vertices.resize(4);
173  cBase.vertices.resize(edges2);
174  size_t iq=isRotated?1:2,it=0;
175  for (size_t i=0;i<edges2;i++) {
176  cBase.vertices[i]=it+minorBase;
177  size_t iiq=(iq+1)%numBaseEdges+base;
178  size_t iiiq=(iiq+1)%numBaseEdges+base;
179  size_t iit=(it+1)%edges2+minorBase;
180  quad.vertices[0]=it+minorBase;
181  quad.vertices[1]=iit;
182  quad.vertices[2]=iiq;
183  quad.vertices[3]=iq+base;
184  tri.vertices[0]=iit;
185  tri.vertices[1]=iiq;
186  tri.vertices[2]=iiiq;
187  iq=(iq+2)%numBaseEdges;
188  it=(it+1)%edges2;
189  faces.push_back(tri);
190  faces.push_back(quad);
191  }
192  if (edges2>=3) faces.push_back(cBase);
193 }
194 void insertRotunda(double angleShift,double baseRadius,bool isRotated,bool isUpwards,size_t base,vector<TPoint3D> &verts,vector<CPolyhedron::TPolyhedronFace> &faces) {
195  double R1=baseRadius*sqrt((5.0-sqrt(5.0))/10.0);
196  double R2=baseRadius*sqrt((5.0+sqrt(5.0))/10.0);
197  double baseHeight=verts[base].z;
198  TPoint3D p1[5],p2[5];
199  angleShift+=M_PI/10;
200  if (isRotated) angleShift+=M_PI/5;
201  for (size_t i=0;i<5;i++) {
202  double a=(i+i+1)*M_PI/5+angleShift;
203  double b=(i+i)*M_PI/5+angleShift;
204  double ca=cos(a),sa=sin(a),cb=cos(b),sb=sin(b);
205  p1[i].x=R1*ca;
206  p1[i].y=R1*sa;
207  p1[i].z=baseHeight+(isUpwards?R2:-R2);
208  p2[i].x=R2*cb;
209  p2[i].y=R2*sb;
210  p2[i].z=baseHeight+(isUpwards?R1:-R1);
211  }
212  size_t newBase=verts.size();
213  for (size_t i=0;i<5;i++) verts.push_back(p1[i]);
214  for (size_t i=0;i<5;i++) verts.push_back(p2[i]);
216  f.vertices.resize(3);
217  g.vertices.resize(5);
218  size_t baseStart=isRotated?2:1;
219  for (size_t i=0;i<5;i++) {
220  size_t ii=(i+1)%5;
221  f.vertices[0]=newBase+i;
222  f.vertices[1]=newBase+ii;
223  f.vertices[2]=newBase+ii+5;
224  faces.push_back(f);
225  f.vertices[0]=newBase+i+5;
226  f.vertices[1]=((i+i+baseStart)%10)+base;
227  f.vertices[2]=((i+i+9+baseStart)%10)+base;
228  faces.push_back(f);
229  g.vertices[0]=newBase+(ii%5)+5;
230  g.vertices[1]=newBase+i;
231  g.vertices[2]=newBase+i+5;
232  g.vertices[3]=(i+i+baseStart)%10+base;
233  g.vertices[4]=(i+i+baseStart+1)%10+base;
234  faces.push_back(g);
235  }
236  for (size_t i=0;i<5;i++) g.vertices[i]=i+newBase;
237  faces.push_back(g);
238  return;
239 }
240 inline size_t additionalFaces(JohnsonBodyPart j,uint32_t numBaseEdges) {
241  if (j==INF_NO_BODY||j==SUP_NO_BODY) return 1; //j is a base
242  else if (j<UPWARDS_CUPOLA) return numBaseEdges; //j is a pyramid
243  else if (j<PRISM) return numBaseEdges+ ((numBaseEdges>=6)?1:0); //j is a cupola
244  else if (j==PRISM) return numBaseEdges;
245  else if (j==ANTIPRISM) return numBaseEdges<<1;
246  else return 16; //j is a rotunda
247 }
248 
250  char hm=0;
251  for (vector<uint32_t>::const_iterator it=f.vertices.begin();it!=f.vertices.end();++it) if (*it==e.v1||*it==e.v2) hm++;
252  return hm==2;
253 }
254 
255 bool getPlanesIntersection(const vector<const TPlane *> &planes,TPoint3D &pnt) {
256  if (planes.size()<3) return false;
257  char o=0;
258  TPlane pl=*planes[0];
259  TLine3D l;
260  TObject3D obj;
261  for (size_t i=1;i<planes.size();i++) switch (o) {
262  case 0:
263  if (!intersect(pl,*planes[i],obj)) return false;
264  if (obj.getPlane(pl)) o=0;
265  else if (obj.getLine(l)) o=1;
266  else if (obj.getPoint(pnt)) o=2;
267  else return false;
268  break;
269  case 1:
270  if (!intersect(l,*planes[i],obj)) return false;
271  if (obj.getLine(l)) o=1;
272  else if (obj.getPoint(pnt)) o=2;
273  else return false;
274  break;
275  case 2:
276  if (!planes[i]->contains(pnt)) return false;
277  break;
278  default:
279  return false;
280  }
281  return o==2;
282 }
283 
284 bool searchForFace(const vector<CPolyhedron::TPolyhedronFace> &fs,uint32_t v1,uint32_t v2,uint32_t v3) {
285  for (vector<CPolyhedron::TPolyhedronFace>::const_iterator it=fs.begin();it!=fs.end();++it) {
286  const vector<uint32_t> &f=it->vertices;
287  size_t hmf=0;
288  for (vector<uint32_t>::const_iterator it2=f.begin();it2!=f.end();++it2) {
289  if (*it2==v1) hmf|=1;
290  else if (*it2==v2) hmf|=2;
291  else if (*it2==v3) hmf|=4;
292  }
293  if (hmf==7) return true;
294  }
295  return false;
296 }
297 bool searchForEdge(const vector<CPolyhedron::TPolyhedronEdge> &es,uint32_t v1,uint32_t v2,size_t &where) {
298  for (where=0;where<es.size();where++) {
299  const CPolyhedron::TPolyhedronEdge &e=es[where];
300  if (e.v1==v1&&e.v2==v2) return true;
301  else if (e.v1==v2&&e.v2==v1) return false;
302  }
303  throw std::logic_error("Internal error. Edge not found");
304 }
307  const vector<uint32_t> &f=it->vertices;
308  char res=0;
309  for (vector<uint32_t>::const_iterator it2=f.begin();it2!=f.end();++it2) if (*it2==v1) res|=1;
310  else if (*it2==v2) res|=2;
311  if (res==3) return true;
312  }
313  return false;
314 }
315 double getHeight(const TPolygon3D &p,const TPoint3D &c) {
316  size_t N=p.size();
317  if (N>5||N<3) throw std::logic_error("Faces must have exactly 3, 4 or 5 vertices.");
318  double r=mrpt::math::distance(p[0],c);
319  double l=mrpt::math::distance(p[0],p[1]);
320  for (size_t i=1;i<N;i++) if (abs(mrpt::math::distance(p[i],c)-r)>=mrpt::math::geometryEpsilon) throw std::logic_error("There is a non-regular polygon.");
321  else if (abs(mrpt::math::distance(p[i],p[(i+1)%N])-l)>=mrpt::math::geometryEpsilon) throw std::logic_error("There is a non-regular polygon.");
322  return sqrt(square(l)-square(r));
323 }
325  double d[3];
326  double mod;
327  inline double &operator[](size_t i) {
328  return d[i];
329  }
330  inline double operator[](size_t i) const {
331  return d[i];
332  }
333 };
334 //End of auxiliary data and code
335 
336 double CPolyhedron::TPolyhedronEdge::length(const vector<TPoint3D> &vertices) const {
337  return mrpt::math::distance(vertices[v1],vertices[v2]);
338 }
339 
340 double CPolyhedron::TPolyhedronFace::area(const vector<TPoint3D> &vs) const {
341  //Calculate as fan of triangles.
342  size_t N=vertices.size();
343  vector<SegmentVector> d(N-1);
344  for (size_t i=1;i<N;i++) {
345  d[i-1].mod=0;
346  for (size_t j=0;j<3;j++) {
347  d[i-1][j]=vs[vertices[i]][j]-vs[vertices[0]][j];
348  d[i-1].mod+=square(d[i-1][j]);
349  }
350  d[i-1].mod=sqrt(d[i-1].mod);
351  }
352  double res=0;
353  for (size_t i=1;i<N-1;i++) res+=sqrt(square(d[i-1].mod*d[i].mod)-square(dotProduct<3,double>(d[i-1],d[i])));
354  return res/2;
355 }
356 
357 void CPolyhedron::TPolyhedronFace::getCenter(const vector<TPoint3D> &vrts,TPoint3D &p) const {
358  p.x=p.y=p.z=0.0;
359  for (vector<uint32_t>::const_iterator it=vertices.begin();it!=vertices.end();++it) {
360  p.x+=vrts[*it].x;
361  p.y+=vrts[*it].y;
362  p.z+=vrts[*it].z;
363  }
364  size_t N=vertices.size();
365  p.x/=N;
366  p.y/=N;
367  p.z/=N;
368 }
369 
370 CPolyhedronPtr CPolyhedron::Create(const std::vector<math::TPolygon3D> &polys) {
371  vector<TPoint3D> vertices(0);
372  vector<TPolyhedronFace> faces;
373  if (getVerticesAndFaces(polys,vertices,faces)) return Create(vertices,faces);
374  else return CreateEmpty();
375 }
376 
377 CPolyhedronPtr CPolyhedron::CreateCubicPrism(double x1,double x2,double y1,double y2,double z1,double z2) {
378  vector<TPoint3D> verts;
379  vector<TPolyhedronFace> faces;
380  for (int i=0;i<8;i++) verts.push_back(TPoint3D((i&1)?x2:x1,(i&2)?y2:y1,(i&4)?z2:z1));
381  static uint32_t faceVertices[]={0,1,5,4, 2,3,7,6, 0,2,6,4, 1,3,7,5, 0,1,3,2, 4,5,7,6};
382  TPolyhedronFace f;
383  for (uint32_t *p=reinterpret_cast<uint32_t *>(&faceVertices);p<24+reinterpret_cast<uint32_t *>(&faceVertices);p+=4) {
384  f.vertices.insert(f.vertices.begin(),p,p+4);
385  faces.push_back(f);
386  f.vertices.clear();
387  }
388  return CreateNoCheck(verts,faces);
389 }
390 
391 CPolyhedronPtr CPolyhedron::CreatePyramid(const vector<TPoint2D> &baseVertices,double height) {
392  uint32_t n=baseVertices.size();
393  if (baseVertices.size()<3) throw std::logic_error("Not enought vertices");
394  vector<TPoint3D> verts;
395  vector<TPolyhedronFace> faces;
396  verts.push_back(TPoint3D(0,0,height));
397  for (vector<TPoint2D>::const_iterator it=baseVertices.begin();it!=baseVertices.end();++it) verts.push_back(TPoint3D(it->x,it->y,0));
398  TPolyhedronFace f,g;
399  f.vertices.push_back(0);
400  f.vertices.push_back(n);
401  f.vertices.push_back(1);
402  g.vertices.push_back(1);
403  faces.push_back(f);
404  for (uint32_t i=2;i<=n;i++) {
405  f.vertices.erase(f.vertices.begin()+1);
406  f.vertices.push_back(i);
407  faces.push_back(f);
408  g.vertices.push_back(i);
409  }
410  faces.push_back(g);
411  return CreateNoCheck(verts,faces);
412 }
413 
414 CPolyhedronPtr CPolyhedron::CreateDoublePyramid(const vector<TPoint2D> &baseVertices,double height1,double height2) {
415  uint32_t N=baseVertices.size();
416  if (N<3) throw std::logic_error("Not enought vertices");
417  vector<TPoint3D> verts;
418  verts.reserve(N+2);
419  vector<TPolyhedronFace> faces;
420  faces.reserve(N<<1);
421  verts.push_back(TPoint3D(0,0,height1));
422  for (vector<TPoint2D>::const_iterator it=baseVertices.begin();it!=baseVertices.end();++it) verts.push_back(TPoint3D(it->x,it->y,0));
423  verts.push_back(TPoint3D(0,0,-height2));
424  TPolyhedronFace f,g;
425  f.vertices.resize(3);
426  g.vertices.resize(3);
427  f.vertices[0]=0;
428  g.vertices[0]=N+1;
429  for (uint32_t i=1;i<N;i++) {
430  f.vertices[1]=g.vertices[1]=i;
431  f.vertices[2]=g.vertices[2]=i+1;
432  faces.push_back(f);
433  faces.push_back(g);
434  }
435  f.vertices[1]=g.vertices[1]=1;
436  faces.push_back(f);
437  faces.push_back(g);
438  return CreateNoCheck(verts,faces);
439 }
440 
441 CPolyhedronPtr CPolyhedron::CreateTruncatedPyramid(const vector<TPoint2D> &baseVertices,double height,double ratio) {
442  uint32_t n=baseVertices.size();
443  if (n<3) throw std::logic_error("Not enough vertices");
444  vector<TPoint3D> verts(n+n);
445  vector<TPolyhedronFace> faces(n+2);
446  TPolyhedronFace f,g,h;
447  f.vertices.resize(4);
448  g.vertices.resize(n);
449  h.vertices.resize(n);
450  for (uint32_t i=0;i<n;i++) {
451  verts[i]=TPoint3D(baseVertices[i].x,baseVertices[i].y,0);
452  verts[i+n]=TPoint3D(baseVertices[i].x*ratio,baseVertices[i].y*ratio,height);
453  uint32_t ii=(i+1)%n;
454  f.vertices[0]=i;
455  f.vertices[1]=ii;
456  f.vertices[2]=ii+n;
457  f.vertices[3]=i+n;
458  faces[i]=f;
459  g.vertices[i]=i;
460  h.vertices[i]=i+n;
461  }
462  faces[n]=g;
463  faces[n+1]=h;
464  return CreateNoCheck(verts,faces);
465 }
466 
467 CPolyhedronPtr CPolyhedron::CreateCustomAntiprism(const vector<TPoint2D> &bottomBase,const vector<TPoint2D> &topBase,const double height) {
468  uint32_t n=bottomBase.size();
469  if (n<3) throw std::logic_error("Not enough vertices");
470  if (n!=topBase.size()) throw std::logic_error("Bases' number of vertices do not match");
471  vector<TPoint3D> verts(n+n);
472  vector<TPolyhedronFace> faces(n+n+2);
473  TPolyhedronFace f,g,h;
474  f.vertices.resize(3);
475  g.vertices.resize(n);
476  h.vertices.resize(n);
477  for (uint32_t i=0;i<n;i++) {
478  verts[i]=TPoint3D(bottomBase[i].x,bottomBase[i].y,0);
479  verts[n+i]=TPoint3D(topBase[i].x,topBase[i].y,height);
480  uint32_t ii=(i+1)%n;
481  f.vertices[0]=i;
482  f.vertices[1]=ii;
483  f.vertices[2]=i+n;
484  faces[i]=f;
485  f.vertices[0]=i+n;
486  f.vertices[1]=ii+n;
487  f.vertices[2]=ii;
488  faces[n+i]=f;
489  g.vertices[i]=i;
490  h.vertices[i]=n+i;
491  }
492  faces[n+n]=g;
493  faces[n+n+1]=h;
494  return CreateNoCheck(verts,faces);
495 }
496 
497 CPolyhedronPtr CPolyhedron::CreateParallelepiped(const TPoint3D &base,const TPoint3D &v1,const TPoint3D &v2,const TPoint3D &v3) {
498  vector<TPoint3D> verts(8);
499  vector<TPolyhedronFace> faces(6);
500  for (uint32_t i=0;i<8;i++) {
501  verts[i]=base;
502  if (i&1) verts[i]=verts[i]+v1;
503  if (i&2) verts[i]=verts[i]+v2;
504  if (i&4) verts[i]=verts[i]+v3;
505  }
506  TPolyhedronFace f;
507  f.vertices.resize(4);
508  f.vertices[0]=0;
509  f.vertices[1]=1;
510  f.vertices[2]=3;
511  f.vertices[3]=2;
512  faces[0]=f;
513  //f.vertices[0]=0;
514  //f.vertices[1]=1;
515  f.vertices[2]=5;
516  f.vertices[3]=4;
517  faces[1]=f;
518  //f.vertices[0]=0;
519  f.vertices[1]=2;
520  f.vertices[2]=6;
521  //f.vertices[3]=4;
522  faces[2]=f;
523  for (uint32_t i=0;i<3;i++) {
524  uint32_t valueAdd=4>>i;
525  faces[i+3].vertices.resize(4);
526  for (uint32_t j=0;j<4;j++) faces[i+3].vertices[j]=faces[i].vertices[j]+valueAdd;
527  }
528  return CreateNoCheck(verts,faces);
529 }
530 
531 CPolyhedronPtr CPolyhedron::CreateBifrustum(const vector<TPoint2D> &baseVertices,double height1,double ratio1,double height2,double ratio2) {
532  //TODO: check special case in which ratio=0.
533  size_t N=baseVertices.size();
534  vector<TPoint3D> verts(3*N);
535  size_t N2=N+N;
536  for (size_t i=0;i<N;i++) {
537  double x=baseVertices[i].x;
538  double y=baseVertices[i].y;
539  verts[i].x=x;
540  verts[i].y=y;
541  verts[i].z=0;
542  verts[i+N].x=x*ratio1;
543  verts[i+N].y=y*ratio1;
544  verts[i+N].z=height1;
545  verts[i+N2].x=x*ratio2;
546  verts[i+N2].y=y*ratio2;
547  verts[i+N2].z=-height2; //This is not an error. This way, two positive heights produce an actual bifrustum.
548  }
549  vector<TPolyhedronFace> faces(N2+2);
550  TPolyhedronFace f,g,h;
551  f.vertices.resize(4);
552  g.vertices.resize(N);
553  h.vertices.resize(N);
554  for (size_t i=0;i<N;i++) {
555  size_t i2=(i+1)%N;
556  f.vertices[0]=i;
557  f.vertices[1]=i2;
558  f.vertices[2]=i2+N;
559  f.vertices[3]=i+N;
560  faces[i]=f;
561  f.vertices[2]=i2+N2;
562  f.vertices[3]=i+N2;
563  faces[i+N]=f;
564  g.vertices[i]=i+N;
565  h.vertices[i]=i+N2;
566  }
567  faces[N2]=g;
568  faces[N2+1]=h;
569  return CreateNoCheck(verts,faces);
570 }
571 
572 CPolyhedronPtr CPolyhedron::CreateTrapezohedron(uint32_t numBaseEdges,double baseRadius,double basesDistance) {
573  if (numBaseEdges<3) throw std::logic_error("Not enough vertices");
574  if (basesDistance==0||baseRadius==0) return CreateEmpty();
575  size_t numBaseEdges2=numBaseEdges<<1;
576  vector<TPoint3D> verts(numBaseEdges2+2);
577  double space=2*M_PI/numBaseEdges;
578  double shift=space/2;
579  double height1=basesDistance/2;
580  double cospii=cos(M_PI/numBaseEdges);
581  double height2=height1*(cospii+1)/(1-cospii); //Apex height, calculated so each face conforms a plane
582  for (size_t i=0;i<numBaseEdges;i++) {
583  double ang=space*i;
584  double ang2=ang+shift;
585  size_t ii=i+numBaseEdges;
586  verts[i].x=baseRadius*cos(ang);
587  verts[i].y=baseRadius*sin(ang);
588  verts[i].z=-height1;
589  verts[ii].x=baseRadius*cos(ang2);
590  verts[ii].y=baseRadius*sin(ang2);
591  verts[ii].z=height1;
592  }
593  verts[numBaseEdges2].x=0;
594  verts[numBaseEdges2].y=0;
595  verts[numBaseEdges2].z=-height2;
596  verts[numBaseEdges2+1].x=0;
597  verts[numBaseEdges2+1].y=0;
598  verts[numBaseEdges2+1].z=height2;
599  vector<TPolyhedronFace> faces(numBaseEdges2);
600  TPolyhedronFace f,g;
601  f.vertices.resize(4);
602  g.vertices.resize(4);
603  f.vertices[3]=numBaseEdges2;
604  g.vertices[3]=numBaseEdges2+1;
605  for (size_t i=0;i<numBaseEdges;i++) {
606  size_t ii=(i+1)%numBaseEdges;
607  size_t i2=i+numBaseEdges;
608  f.vertices[0]=i;
609  f.vertices[1]=i2;
610  f.vertices[2]=ii;
611  g.vertices[0]=i2;
612  g.vertices[1]=ii;
613  g.vertices[2]=ii+numBaseEdges;
614  faces[i]=f;
615  faces[i+numBaseEdges]=g;
616  }
617  return CreateNoCheck(verts,faces);
618 }
619 
620 CPolyhedronPtr CPolyhedron::CreateJohnsonSolidWithConstantBase(uint32_t numBaseEdges,double baseRadius,const std::string &components,size_t shifts) {
621  if (baseRadius==0) return CreateEmpty();
622  if (numBaseEdges<3) throw std::logic_error("Not enough vertices");
623  vector<JohnsonBodyPart> parts;
624  if (!analyzeJohnsonPartsString(components,numBaseEdges,parts)) throw std::logic_error("Invalid string");
625  //Some common values are computed
626  size_t nParts=parts.size();
627  double edgeLength=2*baseRadius*sin(M_PI/numBaseEdges);
628  double antiPrismHeight=sqrt(square(edgeLength)-square(baseRadius)*(2-2*cos(M_PI/numBaseEdges)));
629  //Vertices' and faces' vectors are computed
630  size_t nVerts=numBaseEdges*(nParts-1)+additionalVertices(parts[0],numBaseEdges)+additionalVertices(*parts.rbegin(),numBaseEdges);
631  size_t nFaces=0;
632  for (size_t i=0;i<nParts;i++) nFaces+=additionalFaces(parts[i],numBaseEdges);
633  vector<TPoint3D> verts;
634  verts.reserve(nVerts);
635  vector<TPolyhedronFace> faces;
636  faces.reserve(nFaces);
637  //Each base's position is computed. Also, the height is set so that the polyhedron is vertically centered in z=0.
638  double h,mHeight=0;
639  vector<pair<double,size_t> > basePositionInfo(nParts-1);
640  for (size_t i=0;i<nParts-1;i++) {
641  if (parts[i]==PRISM) h=edgeLength;
642  else if (parts[i]==ANTIPRISM) {
643  h=antiPrismHeight;
644  shifts++;
645  } else h=0;
646  basePositionInfo[i]=make_pair(mHeight+=h,shifts);
647  }
648  mHeight/=2;
649  double semi=M_PI/numBaseEdges;
650  //All the bases are generated and inserted into the vertices' vector.
651  for (vector<pair<double,size_t> >::const_iterator it=basePositionInfo.begin();it!=basePositionInfo.end();++it) generateShiftedBase(numBaseEdges,baseRadius,it->first-mHeight,semi*it->second,verts);
652  size_t initialBase=0,endBase=0;
654  face.vertices.reserve(numBaseEdges);
655  //Each body is inserted.
656  for (size_t i=0;i<nParts;i++) {
657  switch (parts[i]) {
658  case INF_NO_BODY:
659  //Inferior base.
660  face.vertices.resize(numBaseEdges);
661  for (size_t i=0;i<numBaseEdges;i++) face.vertices[i]=endBase+i;
662  faces.push_back(face);
663  break;
664  case SUP_NO_BODY:
665  //Superior base.
666  face.vertices.resize(numBaseEdges);
667  for (size_t i=0;i<numBaseEdges;i++) face.vertices[i]=initialBase+i;
668  faces.push_back(face);
669  break;
670  case UPWARDS_PYRAMID: {
671  //Upwards-pointing pyramid. There must be 5 or less vertices.
672  double apexHeight=baseRadius*sqrt(4*square(sin(M_PI/numBaseEdges))-1);
673  face.vertices.resize(3);
674  face.vertices[0]=verts.size();
675  face.vertices[1]=initialBase+numBaseEdges-1;
676  face.vertices[2]=initialBase;
677  do {
678  faces.push_back(face);
679  face.vertices[1]=face.vertices[2];
680  face.vertices[2]++;
681  } while (face.vertices[2]<initialBase+numBaseEdges);
682  verts.push_back(TPoint3D(0,0,verts[initialBase].z+apexHeight));
683  break;
684  } case DOWNWARDS_PYRAMID: {
685  //Downwards-pointing pyramid. There must be 5 or less vertices.
686  double apexHeight=baseRadius*sqrt(4*square(sin(M_PI/numBaseEdges))-1);
687  face.vertices.resize(3);
688  face.vertices[0]=verts.size();
689  face.vertices[1]=endBase+numBaseEdges-1;
690  face.vertices[2]=endBase;
691  do {
692  faces.push_back(face);
693  face.vertices[1]=face.vertices[2];
694  face.vertices[2]++;
695  } while (face.vertices[2]<endBase+numBaseEdges);
696  verts.push_back(TPoint3D(0,0,verts[endBase].z-apexHeight));
697  break;
698  } case UPWARDS_CUPOLA:
699  //Upwards-pointing cupola. There must be an even amount of vertices.
700  insertCupola(numBaseEdges,basePositionInfo.rbegin()->second*semi,baseRadius,edgeLength,false,true,initialBase,verts,faces);
701  break;
702  case DOWNWARDS_CUPOLA:
703  //Downwards-pointing cupola. There must be an even amount of vertices.
704  insertCupola(numBaseEdges,basePositionInfo[0].second*semi,baseRadius,edgeLength,false,false,endBase,verts,faces);
705  break;
707  //Upwards-pointing, slightly rotated, cupola. There must be an even amount of vertices.
708  insertCupola(numBaseEdges,basePositionInfo.rbegin()->second*semi,baseRadius,edgeLength,true,true,initialBase,verts,faces);
709  break;
711  //Downwards-pointing, slightly rotated, cupola. There must be an even amount of vertices.
712  insertCupola(numBaseEdges,basePositionInfo[0].second*semi,baseRadius,edgeLength,true,false,endBase,verts,faces);
713  break;
714  case PRISM:
715  //Archimedean prism.
716  face.vertices.resize(4);
717  for (size_t i=0;i<numBaseEdges;i++) {
718  size_t ii=(i+1)%numBaseEdges;
719  face.vertices[0]=initialBase+i;
720  face.vertices[1]=endBase+i;
721  face.vertices[2]=endBase+ii;
722  face.vertices[3]=initialBase+ii;
723  faces.push_back(face);
724  }
725  break;
726  case ANTIPRISM: {
727  //Archimedean antiprism.
728  face.vertices.resize(3);
729  face.vertices[0]=initialBase;
730  face.vertices[1]=endBase;
731  face.vertices[2]=initialBase+1;
732  bool nextIsEnd=true;
733  size_t nextEnd=1;
734  size_t nextInitial=2;
735  for (size_t i=0;i<numBaseEdges<<1;i++) {
736  faces.push_back(face);
737  face.vertices[0]=face.vertices[1];
738  face.vertices[1]=face.vertices[2];
739  if (nextIsEnd) {
740  face.vertices[2]=endBase+nextEnd;
741  nextEnd=(nextEnd+1)%numBaseEdges;
742  } else {
743  face.vertices[2]=initialBase+nextInitial;
744  nextInitial=(nextInitial+1)%numBaseEdges;
745  }
746  nextIsEnd=!nextIsEnd;
747  }
748  break;
749  } case UPWARDS_ROTUNDA:
750  //Upwards-pointing pentagonal rotunda. Only for bases of exactly 10 vertices.
751  insertRotunda(basePositionInfo.rbegin()->second*semi,baseRadius,false,true,initialBase,verts,faces);
752  break;
753  case DOWNWARDS_ROTUNDA:
754  //Downwards-pointing pentagonal rotunda. Only for bases of exactly 10 vertices.
755  insertRotunda(basePositionInfo[0].second*semi,baseRadius,false,false,endBase,verts,faces);
756  break;
758  //Upwards-pointing, slightly rotated, pentagonal rotunda. Only for bases of exactly 10 vertices.
759  insertRotunda(basePositionInfo.rbegin()->second*semi,baseRadius,true,true,initialBase,verts,faces);
760  break;
762  //Downwards-pointing, slightly rotated, pentagonal rotunda. Only for bases of exactly 10 vertices.
763  insertRotunda(basePositionInfo[0].second*semi,baseRadius,true,false,endBase,verts,faces);
764  break;
765  default:
766  throw std::logic_error("Internal error");
767  }
768  initialBase=endBase;
769  endBase+=numBaseEdges;
770  }
771  return CreateNoCheck(verts,faces);
772 }
773 
774 /*---------------------------------------------------------------
775  render
776  ---------------------------------------------------------------*/
777 
778 void CPolyhedron::render_dl() const {
779 #if MRPT_HAS_OPENGL_GLUT
780  if (mWireframe) {
781  glDisable(GL_LIGHTING); // Disable lights when drawing lines
782 
783  glLineWidth(mLineWidth);
785  glColor4ub(m_color.R,m_color.G,m_color.B,m_color.A);
786  glBegin(GL_LINES);
787  for (vector<TPolyhedronEdge>::const_iterator it=mEdges.begin();it!=mEdges.end();++it) {
788  TPoint3D p=mVertices[it->v1];
789  glVertex3f(p.x,p.y,p.z);
790  p=mVertices[it->v2];
791  glVertex3f(p.x,p.y,p.z);
792  }
793  glEnd();
794  glEnable(GL_LIGHTING); // Disable lights when drawing lines
795  } else {
799 
800  glColor4ub(m_color.R,m_color.G,m_color.B,m_color.A);
801  for (vector<TPolyhedronFace>::const_iterator it=mFaces.begin();it!=mFaces.end();++it) {
803  glNormal3f(it->normal[0],it->normal[1],it->normal[2]);
804  for (vector<uint32_t>::const_iterator it2=it->vertices.begin();it2!=it->vertices.end();++it2) {
805  const TPoint3D &p=mVertices[*it2];
806  glVertex3f(p.x,p.y,p.z);
807  }
808  glEnd();
809  }
811 
812  }
813 #endif
814 }
815 
816 bool CPolyhedron::traceRay(const mrpt::poses::CPose3D &o,double &dist) const {
817  if (!polygonsUpToDate) updatePolygons();
818  return math::traceRay(tempPolygons,o-this->m_pose,dist);
819 }
820 
821 void CPolyhedron::getEdgesLength(std::vector<double> &lengths) const {
822  lengths.resize(mEdges.size());
823  std::vector<double>::iterator it2=lengths.begin();
824  for (std::vector<TPolyhedronEdge>::const_iterator it=mEdges.begin();it!=mEdges.end();++it,++it2) *it2=it->length(mVertices);
825 }
826 
827 void CPolyhedron::getFacesArea(std::vector<double> &areas) const {
828  areas.resize(mFaces.size());
829  std::vector<double>::iterator it2=areas.begin();
830  for (std::vector<TPolyhedronFace>::const_iterator it=mFaces.begin();it!=mFaces.end();++it,++it2) *it2=it->area(mVertices);
831 }
832 
833 double CPolyhedron::getVolume() const {
834  //TODO. Calculate as set of pyramids whose apices are situated in the center of the polyhedron (will work only with convex polyhedrons).
835  //Pyramid volume=V=1/3*base area*height. Height=abs((A-V)·N), where A is the apex, V is any other vertex, N is the base's normal vector and (·) is the scalar product.
836  TPoint3D center;
837  getCenter(center);
838  double res=0;
839  if (!polygonsUpToDate) updatePolygons();
840  vector<TPolygonWithPlane>::const_iterator itP=tempPolygons.begin();
841  vector<double> areas(mFaces.size());
842  getFacesArea(areas);
843  vector<double>::const_iterator itA=areas.begin();
844  for (vector<TPolyhedronFace>::const_iterator it=mFaces.begin();it!=mFaces.end();++it,++itP,++itA) res+=abs(itP->plane.distance(center))*(*itA);
845  return res/3;
846 }
847 
848 void CPolyhedron::getSetOfPolygons(std::vector<math::TPolygon3D> &vec) const {
849  if (!polygonsUpToDate) updatePolygons();
850  size_t N=tempPolygons.size();
851  vec.resize(N);
852  for (size_t i=0;i<N;i++) vec[i]=tempPolygons[i].poly;
853 }
854 
855 void CPolyhedron::getSetOfPolygonsAbsolute(std::vector<math::TPolygon3D> &vec) const {
856  vec.resize(mFaces.size());
857  size_t N=mVertices.size();
858  vector<TPoint3D> nVerts;
859  nVerts.resize(N);
860  CPose3D pose=this->m_pose;
861  for (size_t i=0;i<N;i++) pose.composePoint(mVertices[i],nVerts[i]);
862  transform(mFaces.begin(),mFaces.end(),vec.begin(),FCreatePolygonFromFace<TPolygon3D>(nVerts));
863 }
864 
865 void CPolyhedron::makeConvexPolygons() {
866  vector<TPolygon3D> polys,polysTMP,polys2;
867  getSetOfPolygons(polys);
868  polys2.reserve(polys.size());
869  for (vector<TPolygon3D>::const_iterator it=polys.begin();it!=polys.end();++it) if (mrpt::math::splitInConvexComponents(*it,polysTMP)) polys2.insert(polys2.end(),polysTMP.begin(),polysTMP.end());
870  else polys2.push_back(*it);
871  mVertices.clear();
872  mEdges.clear();
873  mFaces.clear();
874  getVerticesAndFaces(polys2,mVertices,mFaces);
875  for (vector<TPolyhedronFace>::iterator it=mFaces.begin();it!=mFaces.end();++it) {
876  if (!setNormal(*it,false)) throw std::logic_error("Bad face specification");
877  addEdges(*it);
878  }
879 }
880 
881 void CPolyhedron::getCenter(TPoint3D &center) const {
882  size_t N=mVertices.size();
883  if (N==0) throw new std::logic_error("There are no vertices");
884  center.x=center.y=center.z=0;
885  for (vector<TPoint3D>::const_iterator it=mVertices.begin();it!=mVertices.end();++it) {
886  center.x+=it->x;
887  center.y+=it->y;
888  center.z+=it->z;
889  }
890  center.x/=N;
891  center.y/=N;
892  center.z/=N;
893 }
894 
895 CPolyhedronPtr CPolyhedron::CreateRandomPolyhedron(double radius) {
896  switch (mrpt::random::randomGenerator.drawUniform32bit()%34) {
897  case 0:return CreateTetrahedron(radius);
898  case 1:return CreateHexahedron(radius);
899  case 2:return CreateOctahedron(radius);
900  case 3:return CreateDodecahedron(radius);
901  case 4:return CreateIcosahedron(radius);
902  case 5:return CreateTruncatedTetrahedron(radius);
903  case 6:return CreateTruncatedHexahedron(radius);
904  case 7:return CreateTruncatedOctahedron(radius);
905  case 8:return CreateTruncatedDodecahedron(radius);
906  case 9:return CreateTruncatedIcosahedron(radius);
907  case 10:return CreateCuboctahedron(radius);
908  case 11:return CreateRhombicuboctahedron(radius);
909  case 12:return CreateIcosidodecahedron(radius);
910  case 13:return CreateRhombicosidodecahedron(radius);
911  case 14:return CreateTriakisTetrahedron(radius);
912  case 15:return CreateTriakisOctahedron(radius);
913  case 16:return CreateTetrakisHexahedron(radius);
914  case 17:return CreateTriakisIcosahedron(radius);
915  case 18:return CreatePentakisDodecahedron(radius);
916  case 19:return CreateRhombicDodecahedron(radius);
917  case 20:return CreateDeltoidalIcositetrahedron(radius);
918  case 21:return CreateRhombicTriacontahedron(radius);
919  case 22:return CreateDeltoidalHexecontahedron(radius);
920  case 23:return CreateArchimedeanRegularPrism((mrpt::random::randomGenerator.drawUniform32bit()%10)+3,radius);
921  case 24:return CreateArchimedeanRegularAntiprism((mrpt::random::randomGenerator.drawUniform32bit()%10)+3,radius);
922  case 25:return CreateJohnsonSolidWithConstantBase(((mrpt::random::randomGenerator.drawUniform32bit()%4)<<1)+4,radius,"C+");
923  case 26:return CreateJohnsonSolidWithConstantBase(((mrpt::random::randomGenerator.drawUniform32bit()%4)<<1)+4,radius,"C-C+");
924  case 27:return CreateJohnsonSolidWithConstantBase(((mrpt::random::randomGenerator.drawUniform32bit()%4)<<1)+4,radius,"C-PRC+");
925  case 28:return CreateJohnsonSolidWithConstantBase(((mrpt::random::randomGenerator.drawUniform32bit()%4)<<1)+4,radius,"C-AC+");
926  case 29:return CreateJohnsonSolidWithConstantBase(10,radius,"R+");
927  case 30:return CreateJohnsonSolidWithConstantBase(10,radius,"R-PRR+");
928  case 31:return CreateJohnsonSolidWithConstantBase(10,radius,"R-AR+");
929  case 32:return CreateCatalanTrapezohedron((mrpt::random::randomGenerator.drawUniform32bit()%5)+3,radius);
930  case 33:return CreateCatalanDoublePyramid((mrpt::random::randomGenerator.drawUniform32bit()%5)+3,radius);
931  default:return CreateEmpty();
932  }
933 }
934 
935 CPolyhedronPtr CPolyhedron::getDual() const {
936  //This methods builds the dual of a given polyhedron, which is assumed to be centered in (0,0,0), using polar reciprocation.
937  //A vertex (x0,y0,z0), which is inside a circunference x^2+y^2+z^2=r^2, its dual face will lie on the x0·x+y0·y+z0·z=r^2 plane.
938  //The new vertices can, then, be calculated as the corresponding intersections between three or more planes.
939  size_t NV=mFaces.size();
940  size_t NE=mEdges.size();
941  size_t NF=mVertices.size();
942  vector<TPlane> planes(NF);
943  for (size_t i=0;i<NF;i++) {
944  const TPoint3D &p=mVertices[i];
945  TPlane &pl=planes[i];
946  pl.coefs[0]=p.x;
947  pl.coefs[1]=p.y;
948  pl.coefs[2]=p.z;
949  pl.coefs[3]=-square(p.x)-square(p.y)-square(p.z);
950  }
951  CMatrixTemplate<bool> incidence(NV,NF);
952  vector<TPoint3D> vertices(NV);
953  for (size_t i=0;i<NV;i++) {
954  for (size_t j=0;j<NF;j++) incidence(i,j)=false;
955  vector<const TPlane *> fPls;
956  fPls.reserve(mFaces[i].vertices.size());
957  for (vector<uint32_t>::const_iterator it=mFaces[i].vertices.begin();it!=mFaces[i].vertices.end();++it) {
958  incidence(i,*it)=true;
959  fPls.push_back(&planes[*it]);
960  }
961  if (!getPlanesIntersection(fPls,vertices[i])) throw std::logic_error("Dual polyhedron cannot be found");
962  }
963  vector<TPolyhedronFace> faces(NF);
964  for (size_t i=0;i<NF;i++) for (size_t j=0;j<NV;j++) if (incidence(j,i)) faces[i].vertices.push_back(j);
965  //The following code ensures that the faces' vertex list is in the adequate order.
966  CMatrixTemplate<bool> arrayEF(NE,NV);
967  for (size_t i=0;i<NE;i++) for (size_t j=0;j<NV;j++) arrayEF(i,j)=faceContainsEdge(mFaces[j],mEdges[i]);
968  for (vector<TPolyhedronFace>::iterator it=faces.begin();it!=faces.end();++it) {
969  vector<uint32_t> &face=it->vertices;
970  if (face.size()<=3) continue;
971  size_t index=0;
972  while (index<face.size()-1) {
973  bool err=true;
974  while (err) {
975  for (size_t i=0;i<NE;i++) if (arrayEF(i,face[index])&&arrayEF(i,face[index+1])) {
976  err=false;
977  break;
978  }
979  if (err) {
980  size_t val=face[index+1];
981  face.erase(face.begin()+index+1);
982  face.push_back(val);
983  }
984  }
985  index++;
986  }
987  }
988  return Create(vertices,faces);
989 }
990 
991 CPolyhedronPtr CPolyhedron::truncate(double factor) const {
992  if (factor<0) return CreateEmpty();
993  if (factor==0) return CreateNoCheck(mVertices,mFaces);
994  else if (factor<1) {
995  size_t NE=mEdges.size();
996  size_t NV=mVertices.size();
997  size_t NF=mFaces.size();
998  vector<TPoint3D> vertices(NE<<1);
999  vector<TPolyhedronFace> faces(NV+NF);
1000  for (size_t i=0;i<NE;i++) {
1001  const TPoint3D &p1=mVertices[mEdges[i].v1];
1002  const TPoint3D &p2=mVertices[mEdges[i].v2];
1003  TPoint3D &v1=vertices[i+i];
1004  TPoint3D &v2=vertices[i+i+1];
1005  for (size_t j=0;j<3;j++) {
1006  double d=(p2[j]-p1[j])*factor/2;
1007  v1[j]=p1[j]+d;
1008  v2[j]=p2[j]-d;
1009  }
1010  faces[mEdges[i].v1].vertices.push_back(i+i);
1011  faces[mEdges[i].v2].vertices.push_back(i+i+1);
1012  }
1013  for (size_t i=0;i<NV;i++) {
1014  vector<uint32_t> &f=faces[i].vertices;
1015  size_t sf=f.size();
1016  if (sf==3) continue;
1017  for (size_t j=1;j<sf-1;j++) {
1018  const TPolyhedronEdge &e1=mEdges[f[j-1]/2];
1019  for (;;) {
1020  const TPolyhedronEdge &e2=mEdges[f[j]/2];
1021  if (!((e1.v1==i||e1.v2==i)&&(e2.v1==i||e2.v2==i))) THROW_EXCEPTION("En algo te has equivocado, chaval.");
1022  if (searchForFace(mFaces,i,(e1.v1==i)?e1.v2:e1.v1,(e2.v1==i)?e2.v2:e2.v1)) break;
1023  uint32_t tmpV=f[j];
1024  f.erase(f.begin()+j);
1025  f.push_back(tmpV);
1026  }
1027  }
1028  }
1029  for (size_t i=0;i<NF;i++) {
1030  vector<uint32_t> &f=faces[i+NV].vertices;
1031  const vector<uint32_t> &cf=mFaces[i].vertices;
1032  size_t hmV=cf.size();
1033  f.reserve(hmV<<1);
1034  for (size_t j=0;j<hmV;j++) {
1035  size_t where;
1036  if (searchForEdge(mEdges,cf[j],cf[(j+1)%hmV],where)) {
1037  f.push_back(where<<1);
1038  f.push_back((where<<1)+1);
1039  } else {
1040  f.push_back((where<<1)+1);
1041  f.push_back(where<<1);
1042  }
1043  }
1044  }
1045  return Create(vertices,faces);
1046  } else if (factor==1) {
1047  size_t NE=mEdges.size();
1048  size_t NV=mVertices.size();
1049  size_t NF=mFaces.size();
1050  vector<TPoint3D> vertices(NE);
1051  vector<TPolyhedronFace> faces(NV+NF);
1052  for (size_t i=0;i<NE;i++) {
1053  const TPoint3D &p1=mVertices[mEdges[i].v1];
1054  const TPoint3D &p2=mVertices[mEdges[i].v2];
1055  TPoint3D &dst=vertices[i];
1056  for (size_t j=0;j<3;j++) dst[j]=(p1[j]+p2[j])/2;
1057  faces[mEdges[i].v1].vertices.push_back(i);
1058  faces[mEdges[i].v2].vertices.push_back(i);
1059  }
1060  for (size_t i=0;i<NV;i++) {
1061  vector<uint32_t> &f=faces[i].vertices;
1062  size_t sf=f.size();
1063  if (sf==3) continue;
1064  for (size_t j=1;j<sf-1;j++) {
1065  const TPolyhedronEdge &e1=mEdges[f[j-1]];
1066  for (;;) {
1067  const TPolyhedronEdge &e2=mEdges[f[j-1]];
1068  if (!((e1.v1==i||e1.v2==i)&&(e2.v1==1||e2.v2==i))) THROW_EXCEPTION("En algo te has equivocado, chaval.");
1069  if (searchForFace(mFaces,i,(e1.v1==i)?e1.v2:e1.v1,(e2.v1==i)?e2.v2:e2.v1)) break;
1070  uint32_t tmpV=f[j];
1071  f.erase(f.begin()+j);
1072  f.push_back(tmpV);
1073  }
1074  }
1075  }
1076  for (size_t i=0;i<NF;i++) {
1077  vector<uint32_t> &f=faces[i+NV].vertices;
1078  const vector<uint32_t> &cf=mFaces[i].vertices;
1079  size_t hmV=cf.size();
1080  f.reserve(hmV);
1081  for (size_t j=0;j<hmV;j++) {
1082  size_t where;
1083  searchForEdge(mEdges,cf[j],cf[(j+1)%hmV],where);
1084  f.push_back(where);
1085  }
1086  }
1087  return Create(vertices,faces);
1088  } else return CreateEmpty();
1089 }
1090 
1091 CPolyhedronPtr CPolyhedron::cantellate(double factor) const {
1092  if (factor<0) return CreateEmpty();
1093  else if (factor==0) return CreateNoCheck(mVertices,mFaces);
1094  size_t NV=mVertices.size();
1095  size_t NE=mEdges.size();
1096  size_t NF=mFaces.size();
1097  vector<TPolygon3D> origFaces(NF);
1098  getSetOfPolygons(origFaces);
1099  TPoint3D cnt;
1100  getCenter(cnt);
1101  vector<TPoint3D> polyCenters(NF);
1102  vector<TPoint3D> polyNewCenters(NF);
1103  size_t NNV=0;
1104  for (size_t i=0;i<NF;i++) {
1105  origFaces[i].getCenter(polyCenters[i]);
1106  polyCenters[i]-=cnt;
1107  polyNewCenters[i]=polyCenters[i];
1108  polyNewCenters[i]*=(1+factor);
1109  polyNewCenters[i]+=cnt;
1110  NNV+=origFaces[i].size();
1111  }
1112  vector<TPoint3D> vertices(NNV);
1113  vector<TPolyhedronFace> faces(NF+NV+NE);
1114  size_t ind=0;
1115  for (size_t i=0;i<NF;i++) {
1116  const TPoint3D &oC=polyCenters[i];
1117  const TPoint3D &nC=polyNewCenters[i];
1118  const TPolygon3D &oP=origFaces[i];
1119  vector<uint32_t> &f=faces[i].vertices;
1120  size_t oPS=oP.size();
1121  for (size_t j=0;j<oPS;j++) {
1122  vertices[j+ind]=nC+(oP[j]-oC);
1123  f.push_back(j+ind);
1124  size_t curr=mFaces[i].vertices[j];
1125  faces[NF+curr].vertices.push_back(j+ind);
1126  size_t edge;
1127  searchForEdge(mEdges,curr,mFaces[i].vertices[(j+oPS-1)%oPS],edge);
1128  faces[NF+NV+edge].vertices.push_back(j+ind);
1129  searchForEdge(mEdges,curr,mFaces[i].vertices[(j+1)%oPS],edge);
1130  faces[NF+NV+edge].vertices.push_back(j+ind);
1131  }
1132  ind+=oPS;
1133  }
1134  vector<TPolyhedronFace>::const_iterator begin=faces.begin(),edgeBegin=faces.begin()+NF+NV,end=faces.end();
1135  for (vector<TPolyhedronFace>::iterator it=faces.begin()+NF;it!=faces.begin()+NF+NV;++it) {
1136  vector<uint32_t> &f=it->vertices;
1137  if (f.size()==3) continue;
1138  for (size_t i=1;i<f.size()-1;i++) for(;;) if (searchForEdge(edgeBegin,end,f[i-1],f[i])) break;
1139  else {
1140  uint32_t tmp=f[i];
1141  f.erase(f.begin()+i);
1142  f.push_back(tmp);
1143  }
1144  }
1145  for (vector<TPolyhedronFace>::iterator it=faces.begin()+NF+NV;it!=faces.end();++it) {
1146  vector<uint32_t> &f=it->vertices; //Will always have exactly 4 vertices
1147  for (size_t i=1;i<3;i++) for (;;) if (searchForEdge(begin,edgeBegin,f[i-1],f[i])) break;
1148  else {
1149  uint32_t tmp=f[i];
1150  f.erase(f.begin()+i);
1151  f.push_back(tmp);
1152  }
1153  }
1154  return Create(vertices,faces);
1155 }
1156 
1157 CPolyhedronPtr CPolyhedron::augment(double height) const {
1158  size_t NV=mVertices.size();
1159  size_t NF=mFaces.size();
1160  vector<TPoint3D> vertices(NV+NF);
1161  std::copy(mVertices.begin(),mVertices.end(),vertices.begin());
1162  size_t tnf=0;
1163  for (vector<TPolyhedronFace>::const_iterator it=mFaces.begin();it!=mFaces.end();++it) tnf+=it->vertices.size();
1164  vector<TPolyhedronFace> faces(tnf);
1165  TPolygon3D tmp;
1166  TPlane pTmp;
1167  TPoint3D cTmp;
1168  size_t iF=0;
1169  TPoint3D phCenter;
1170  getCenter(phCenter);
1171  TPolyhedronFace fTmp;
1172  fTmp.vertices.resize(3);
1173  for (size_t i=0;i<NF;i++) {
1174  TPoint3D &vertex=vertices[NV+i];
1175  const vector<uint32_t> &face=mFaces[i].vertices;
1176  size_t N=face.size();
1177  tmp.resize(N);
1178  for (size_t j=0;j<N;j++) tmp[j]=mVertices[face[j]];
1179  tmp.getBestFittingPlane(pTmp);
1180  pTmp.unitarize();
1181  tmp.getCenter(cTmp);
1182  if (pTmp.evaluatePoint(phCenter)>0) for (size_t j=0;j<3;j++) vertex[j]=cTmp[j]-height*pTmp.coefs[j];
1183  else for (size_t j=0;j<3;j++) vertex[j]=cTmp[j]+height*pTmp.coefs[j];
1184  fTmp.vertices[0]=NV+i;
1185  for (size_t j=0;j<N;j++) {
1186  fTmp.vertices[1]=face[j];
1187  fTmp.vertices[2]=face[(j+1)%N];
1188  faces[iF+j]=fTmp;
1189  }
1190  iF+=N;
1191  }
1192  return CreateNoCheck(vertices,faces);
1193 }
1194 
1195 CPolyhedronPtr CPolyhedron::augment(double height,size_t numVertices) const {
1196  size_t NV=mVertices.size();
1197  size_t NF=mFaces.size();
1198  size_t tnf=0;
1199  size_t tnv=NV;
1200  for (vector<TPolyhedronFace>::const_iterator it=mFaces.begin();it!=mFaces.end();++it) if (it->vertices.size()==numVertices) {
1201  tnf+=numVertices;
1202  tnv++;
1203  } else tnf++;
1204  if (tnv==NV) return CreateNoCheck(mVertices,mFaces);
1205  vector<TPoint3D> vertices(tnv);
1206  std::copy(mVertices.begin(),mVertices.end(),vertices.begin());
1207  vector<TPolyhedronFace> faces(tnf);
1208  TPolygon3D tmp(numVertices);
1209  TPlane pTmp;
1210  TPoint3D cTmp;
1211  size_t iF=0;
1212  size_t iV=NV;
1213  TPoint3D phCenter;
1214  getCenter(phCenter);
1215  TPolyhedronFace fTmp;
1216  fTmp.vertices.resize(3);
1217  for (size_t i=0;i<NF;i++) {
1218  const vector<uint32_t> &face=mFaces[i].vertices;
1219  size_t N=face.size();
1220  if (N!=numVertices) {
1221  faces[iF].vertices=face;
1222  iF++;
1223  continue;
1224  }
1225  TPoint3D &vertex=vertices[iV];
1226  for (size_t j=0;j<numVertices;j++) tmp[j]=mVertices[face[j]];
1227  tmp.getBestFittingPlane(pTmp);
1228  pTmp.unitarize();
1229  tmp.getCenter(cTmp);
1230  if (pTmp.evaluatePoint(phCenter)>0) for (size_t j=0;j<3;j++) vertex[j]=cTmp[j]-height*pTmp.coefs[j];
1231  else for (size_t j=0;j<3;j++) vertex[j]=cTmp[j]+height*pTmp.coefs[j];
1232  fTmp.vertices[0]=iV;
1233  for (size_t j=0;j<N;j++) {
1234  fTmp.vertices[1]=face[j];
1235  fTmp.vertices[2]=face[(j+1)%N];
1236  faces[iF+j]=fTmp;
1237  }
1238  iF+=N;
1239  iV++;
1240  }
1241  return CreateNoCheck(vertices,faces);
1242 }
1243 
1244 CPolyhedronPtr CPolyhedron::augment(bool direction) const {
1245  size_t NV=mVertices.size();
1246  size_t NF=mFaces.size();
1247  vector<TPoint3D> vertices(NV+NF);
1248  std::copy(mVertices.begin(),mVertices.end(),vertices.begin());
1249  size_t tnf=0;
1250  for (vector<TPolyhedronFace>::const_iterator it=mFaces.begin();it!=mFaces.end();++it) tnf+=it->vertices.size();
1251  vector<TPolyhedronFace> faces(tnf);
1252  TPolygon3D tmp;
1253  TPlane pTmp;
1254  TPoint3D cTmp;
1255  size_t iF=0;
1256  TPoint3D phCenter;
1257  getCenter(phCenter);
1258  TPolyhedronFace fTmp;
1259  fTmp.vertices.resize(3);
1260  for (size_t i=0;i<NF;i++) {
1261  TPoint3D &vertex=vertices[NV+i];
1262  const vector<uint32_t> &face=mFaces[i].vertices;
1263  size_t N=face.size();
1264  tmp.resize(N);
1265  for (size_t j=0;j<N;j++) tmp[j]=mVertices[face[j]];
1266  tmp.getCenter(cTmp);
1267  double height=getHeight(tmp,cTmp); //throws std::logic_error
1268  tmp.getBestFittingPlane(pTmp);
1269  pTmp.unitarize();
1270  if ((pTmp.evaluatePoint(phCenter)<0)==direction) for (size_t j=0;j<3;j++) vertex[j]=cTmp[j]-height*pTmp.coefs[j];
1271  else for (size_t j=0;j<3;j++) vertex[j]=cTmp[j]+height*pTmp.coefs[j];
1272  fTmp.vertices[0]=NV+i;
1273  for (size_t j=0;j<N;j++) {
1274  fTmp.vertices[1]=face[j];
1275  fTmp.vertices[2]=face[(j+1)%N];
1276  faces[iF+j]=fTmp;
1277  }
1278  iF+=N;
1279  }
1280  return CreateNoCheck(vertices,faces);
1281 
1282 }
1283 
1284 CPolyhedronPtr CPolyhedron::augment(size_t numVertices,bool direction) const {
1285  size_t NV=mVertices.size();
1286  size_t NF=mFaces.size();
1287  size_t tnf=0;
1288  size_t tnv=NV;
1289  for (vector<TPolyhedronFace>::const_iterator it=mFaces.begin();it!=mFaces.end();++it) if (it->vertices.size()==numVertices) {
1290  tnf+=numVertices;
1291  tnv++;
1292  } else tnf++;
1293  if (tnv==NV) return CreateNoCheck(mVertices,mFaces);
1294  vector<TPoint3D> vertices(tnv);
1295  std::copy(mVertices.begin(),mVertices.end(),vertices.begin());
1296  vector<TPolyhedronFace> faces(tnf);
1297  TPolygon3D tmp(numVertices);
1298  TPlane pTmp;
1299  TPoint3D cTmp;
1300  size_t iF=0;
1301  size_t iV=NV;
1302  TPoint3D phCenter;
1303  getCenter(phCenter);
1304  TPolyhedronFace fTmp;
1305  fTmp.vertices.resize(3);
1306  for (size_t i=0;i<NF;i++) {
1307  const vector<uint32_t> &face=mFaces[i].vertices;
1308  size_t N=face.size();
1309  if (N!=numVertices) {
1310  faces[iF].vertices=face;
1311  iF++;
1312  continue;
1313  }
1314  TPoint3D &vertex=vertices[iV];
1315  for (size_t j=0;j<numVertices;j++) tmp[j]=mVertices[face[j]];
1316  tmp.getBestFittingPlane(pTmp);
1317  pTmp.unitarize();
1318  tmp.getCenter(cTmp);
1319  double height=getHeight(tmp,cTmp); //throws std::logic_error
1320  if ((pTmp.evaluatePoint(phCenter)<0)==direction) for (size_t j=0;j<3;j++) vertex[j]=cTmp[j]-height*pTmp.coefs[j];
1321  else for (size_t j=0;j<3;j++) vertex[j]=cTmp[j]+height*pTmp.coefs[j];
1322  fTmp.vertices[0]=iV;
1323  for (size_t j=0;j<N;j++) {
1324  fTmp.vertices[1]=face[j];
1325  fTmp.vertices[2]=face[(j+1)%N];
1326  faces[iF+j]=fTmp;
1327  }
1328  iF+=N;
1329  iV++;
1330  }
1331  return CreateNoCheck(vertices,faces);
1332 }
1333 
1334 CPolyhedronPtr CPolyhedron::rotate(double angle) const {
1335  vector<TPoint3D> vertices(mVertices);
1336  double c=cos(angle),s=sin(angle);
1337  for (vector<TPoint3D>::iterator it=vertices.begin();it!=vertices.end();++it) {
1338  double A=it->x;
1339  double B=it->y;
1340  it->x=A*c-B*s;
1341  it->y=B*c+A*s;
1342  }
1343  return CreateNoCheck(vertices,mFaces);
1344 }
1345 
1346 CPolyhedronPtr CPolyhedron::scale(double factor) const {
1347  vector<TPoint3D> vertices(mVertices);
1348  if (factor<=0) throw std::logic_error("Factor must be a strictly positive number");
1349  for (vector<TPoint3D>::iterator it=vertices.begin();it!=vertices.end();++it) {
1350  it->x*=factor;
1351  it->y*=factor;
1352  }
1353  return CreateNoCheck(vertices,mFaces);
1354 }
1355 
1356 vector<TPoint2D> CPolyhedron::generateBase(uint32_t numBaseEdges,double baseRadius) {
1357  vector<TPoint2D> base(numBaseEdges);
1358  for (size_t i=0;i<numBaseEdges;i++) {
1359  double ang=2*M_PI*i/numBaseEdges;
1360  base[i].x=baseRadius*cos(ang);
1361  base[i].y=baseRadius*sin(ang);
1362  }
1363  return base;
1364 }
1365 
1366 vector<TPoint2D> CPolyhedron::generateShiftedBase(uint32_t numBaseEdges,double baseRadius) {
1367  vector<TPoint2D> base(numBaseEdges);
1368  double shift=M_PI/numBaseEdges;
1369  for (size_t i=0;i<numBaseEdges;i++) {
1370  double ang=shift+2*M_PI*i/numBaseEdges;
1371  base[i].x=baseRadius*cos(ang);
1372  base[i].y=baseRadius*sin(ang);
1373  }
1374  return base;
1375 }
1376 
1377 void CPolyhedron::generateBase(uint32_t numBaseEdges,double baseRadius,double height,vector<TPoint3D> &vec) {
1378  for (size_t i=0;i<numBaseEdges;i++) {
1379  double ang=2*M_PI*i/numBaseEdges;
1380  vec.push_back(TPoint3D(baseRadius*cos(ang),baseRadius*sin(ang),height));
1381  }
1382 }
1383 
1384 void CPolyhedron::generateShiftedBase(uint32_t numBaseEdges,double baseRadius,double height,double shift,vector<TPoint3D> &vec) {
1385  for (size_t i=0;i<numBaseEdges;i++) {
1386  double ang=2*M_PI*i/numBaseEdges+shift;
1387  vec.push_back(TPoint3D(baseRadius*cos(ang),baseRadius*sin(ang),height));
1388  }
1389 }
1390 
1391 void CPolyhedron::updatePolygons() const {
1392  tempPolygons.resize(mFaces.size());
1393  transform(mFaces.begin(),mFaces.end(),tempPolygons.begin(),FCreatePolygonFromFace<TPolygonWithPlane>(mVertices));
1394  polygonsUpToDate=true;
1395 }
1396 
1397 bool CPolyhedron::setNormal(TPolyhedronFace &f,bool doCheck) {
1398  size_t N=doCheck?f.vertices.size():3;
1399  TPolygon3D poly(N);
1400  for (size_t i=0;i<N;i++) poly[i]=mVertices[f.vertices[i]];
1401  TPlane tmp;
1402  if (!poly.getPlane(tmp)) return false;
1403  tmp.getNormalVector(f.normal);
1404  TPoint3D c;
1405  getCenter(c);
1406  if (tmp.evaluatePoint(c)>0) for (size_t i=0;i<3;i++) f.normal[i]=-f.normal[i];
1407  return true;
1408 }
1409 
1410 void CPolyhedron::addEdges(const TPolyhedronFace &f) {
1411  TPolyhedronEdge e;
1413  e.v1=*it;
1414  ++it;
1415  while (it!=f.vertices.end()) {
1416  e.v2=*it;
1417  if (find(mEdges.begin(),mEdges.end(),e)==mEdges.end()) mEdges.push_back(e);
1418  e.v1=e.v2;
1419  ++it;
1420  }
1421  e.v2=*(f.vertices.begin());
1422  if (find(mEdges.begin(),mEdges.end(),e)==mEdges.end()) mEdges.push_back(e);
1423 }
1424 
1425 bool CPolyhedron::checkConsistence(const vector<TPoint3D> &vertices,const vector<TPolyhedronFace> &faces) {
1426  size_t N=vertices.size();
1427  if (vertices.size()>0) for (vector<TPoint3D>::const_iterator it=vertices.begin();it!=vertices.end()-1;++it) for (vector<TPoint3D>::const_iterator it2=it+1;it2!=vertices.end();++it2) if (*it==*it2) return false;
1428  for (vector<TPolyhedronFace>::const_iterator it=faces.begin();it!=faces.end();++it) {
1429  const vector<uint32_t> &e=it->vertices;
1430  for (vector<uint32_t>::const_iterator it2=e.begin();it2!=e.end();++it2) if (*it2>=N) return false;
1431  }
1432  return true;
1433 }
1434 
1435 size_t CPolyhedron::edgesInVertex(size_t vertex) const {
1436  size_t res=0;
1437  for (vector<TPolyhedronEdge>::const_iterator it=mEdges.begin();it!=mEdges.end();++it) if (it->v1==vertex||it->v2==vertex) res++;
1438  return res;
1439 }
1440 
1441 size_t CPolyhedron::facesInVertex(size_t vertex) const {
1442  size_t res=0;
1443  for (vector<TPolyhedronFace>::const_iterator it=mFaces.begin();it!=mFaces.end();++it) if (find(it->vertices.begin(),it->vertices.end(),vertex)!=it->vertices.end()) res++;
1444  return res;
1445 }
1446 
1448  in>>o.v1>>o.v2;
1449  return in;
1450 }
1451 
1453  out<<o.v1<<o.v2;
1454  return out;
1455 }
1456 
1458  in>>o.vertices>>o.normal[0]>>o.normal[1]>>o.normal[2];
1459  return in;
1460 }
1461 
1463  out<<o.vertices<<o.normal[0]<<o.normal[1]<<o.normal[2];
1464  return out;
1465 }
1466 
1467 void CPolyhedron::writeToStream(mrpt::utils::CStream &out,int *version) const {
1468  if (version) *version=0;
1469  else {
1470  writeToStreamRender(out);
1471  //version 0
1472  out<<mVertices<<mFaces<<mWireframe<<mLineWidth;
1473  }
1474 }
1475 
1476 void CPolyhedron::readFromStream(mrpt::utils::CStream &in,int version) {
1477  switch (version) {
1478  case 0:
1479  readFromStreamRender(in);
1480  in>>mVertices>>mFaces>>mWireframe>>mLineWidth;
1481  if (!checkConsistence(mVertices,mFaces)) throw std::logic_error("Inconsistent data read from stream");
1482  for (vector<TPolyhedronFace>::iterator it=mFaces.begin();it!=mFaces.end();++it) {
1483  if (!setNormal(*it)) throw std::logic_error("Bad face specification");
1484  addEdges(*it);
1485  }
1486  break;
1487  default:
1489  };
1490  CRenderizableDisplayList::notifyChange();
1491 }
1492 
1493 
1494 void CPolyhedron::getBoundingBox(mrpt::math::TPoint3D &bb_min, mrpt::math::TPoint3D &bb_max) const
1495 {
1496  bb_min.x = 0;
1497  bb_min.y = 0;
1498  bb_min.z = 0;
1499 
1500  bb_max.x = 0;
1501  bb_max.y = 0;
1502  bb_max.z = 0;
1503 
1504  // Convert to coordinates of my parent:
1505  m_pose.composePoint(bb_min, bb_min);
1506  m_pose.composePoint(bb_max, bb_max);
1507 }
1508 
1509 
1510 /*CPolyhedronPtr CPolyhedron::CreateCuboctahedron(double radius) {
1511  if (radius==0) return CreateEmpty();
1512  vector<TPoint3D> verts;
1513  vector<TPolyhedronFace> faces;
1514  double d=radius/sqrt(2.0);
1515  verts.push_back(TPoint3D(d,0,d));
1516  verts.push_back(TPoint3D(0,d,d));
1517  verts.push_back(TPoint3D(0,-d,d));
1518  verts.push_back(TPoint3D(-d,0,d));
1519  verts.push_back(TPoint3D(d,d,0));
1520  verts.push_back(TPoint3D(d,-d,0));
1521  verts.push_back(TPoint3D(-d,d,0));
1522  verts.push_back(TPoint3D(-d,-d,0));
1523  verts.push_back(TPoint3D(d,0,-d));
1524  verts.push_back(TPoint3D(0,d,-d));
1525  verts.push_back(TPoint3D(0,-d,-d));
1526  verts.push_back(TPoint3D(-d,0,-d));
1527  TPolyhedronFace f;
1528  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, 10,11,7};
1529  static uint32_t faces4[]={0,1,3,2, 8,9,11,10, 0,4,8,5, 1,4,9,6, 2,5,10,7, 3,6,11,7};
1530  for (uint32_t *p=reinterpret_cast<uint32_t *>(&faces3);p<24+reinterpret_cast<uint32_t *>(&faces3);p+=3) {
1531  f.vertices.insert(f.vertices.begin(),p,p+3);
1532  faces.push_back(f);
1533  f.vertices.clear();
1534  }
1535  for (uint32_t *p=reinterpret_cast<uint32_t *>(&faces4);p<24+reinterpret_cast<uint32_t *>(&faces4);p+=4) {
1536  f.vertices.insert(f.vertices.begin(),p,p+4);
1537  faces.push_back(f);
1538  f.vertices.clear();
1539  }
1540  return CreateNoCheck(verts,faces);
1541 }*/
1542 
1543 CPolyhedronPtr CPolyhedron::Create(const vector<TPoint3D> &vertices,const vector<vector<uint32_t> > &faces) {
1544  vector<TPolyhedronFace> aux;
1545  for (vector<vector<uint32_t> >::const_iterator it=faces.begin();it!=faces.end();++it) {
1546  TPolyhedronFace f;
1547  f.vertices=*it;
1548  aux.push_back(f);
1549  }
1550  return Create(vertices,aux);
1551 }
1552 CPolyhedronPtr CPolyhedron::Create(const vector<TPoint3D> &vertices,const vector<TPolyhedronFace> &faces) {
1553  return CPolyhedronPtr(new CPolyhedron(vertices,faces,true));
1554 }
1555 CPolyhedronPtr CPolyhedron::CreateTetrahedron(double radius) {
1556  CPolyhedronPtr tetra=CreateJohnsonSolidWithConstantBase(3,radius*sqrt(8.0)/3.0,"P+");
1557  for (vector<TPoint3D>::iterator it=tetra->mVertices.begin();it!=tetra->mVertices.end();++it) it->z-=radius/3;
1558  return tetra;
1559 }
1560 CPolyhedronPtr CPolyhedron::CreateHexahedron(double radius) {
1561  if (radius==0.0) return CreateEmpty();
1562  double r=radius/sqrt(3.0);
1563  return CreateCubicPrism(-r,r,-r,r,-r,r);
1564 }
1565 CPolyhedronPtr CPolyhedron::CreateOctahedron(double radius) {
1566  return CreateJohnsonSolidWithConstantBase(4,radius,"P-P+");
1567 }
1568 CPolyhedronPtr CPolyhedron::CreateDodecahedron(double radius) {
1569  return CreateIcosahedron(radius/sqrt(15-6*sqrt(5.0)))->getDual();
1570 }
1571 CPolyhedronPtr CPolyhedron::CreateIcosahedron(double radius) {
1572  double ang=M_PI/5;
1573  double s2=4*square(sin(ang));
1574  double prop=sqrt(s2-1)+sqrt(s2-2+2*cos(ang))/2;
1575  return CreateJohnsonSolidWithConstantBase(5,radius/prop,"P-AP+",1);
1576 }
1577 CPolyhedronPtr CPolyhedron::CreateTruncatedTetrahedron(double radius) {
1578  return CreateTetrahedron(radius*sqrt(27.0/11.0))->truncate(2.0/3.0);
1579 }
1580 CPolyhedronPtr CPolyhedron::CreateCuboctahedron(double radius) {
1581  return CreateHexahedron(radius*sqrt(1.5))->truncate(1.0);
1582 }
1583 CPolyhedronPtr CPolyhedron::CreateTruncatedHexahedron(double radius) {
1584  return CreateHexahedron(radius*sqrt(3.0/(5-sqrt(8.0))))->truncate(2-sqrt(2.0));
1585 }
1586 CPolyhedronPtr CPolyhedron::CreateTruncatedOctahedron(double radius) {
1587  return CreateOctahedron(radius*3/sqrt(5.0))->truncate(2.0/3.0);
1588 }
1589 CPolyhedronPtr CPolyhedron::CreateRhombicuboctahedron(double radius,bool type) {
1590  return CreateJohnsonSolidWithConstantBase(8,radius/sqrt(1+square(sin(M_PI/8))),type?"C-PRC+":"GC-PRC+",3);
1591 }
1592 CPolyhedronPtr CPolyhedron::CreateIcosidodecahedron(double radius,bool type) {
1593  return CreateJohnsonSolidWithConstantBase(10,radius,type?"GR-R+":"R-R+",1);
1594 }
1595 CPolyhedronPtr CPolyhedron::CreateTruncatedDodecahedron(double radius) {
1596  return CreateDodecahedron(radius*sqrt(45.0)/sqrt(27+6*sqrt(5.0)))->truncate(1-sqrt(0.2));
1597 }
1598 CPolyhedronPtr CPolyhedron::CreateTruncatedIcosahedron(double radius) {
1599  return CreateIcosahedron(radius*sqrt(45.0)/sqrt(25+4*sqrt(5.0)))->truncate(2.0/3.0);
1600 }
1601 CPolyhedronPtr CPolyhedron::CreateRhombicosidodecahedron(double radius) {
1602  return CreateIcosahedron(radius*sqrt(10.0/(35.0+9.0*sqrt(5.0))))->cantellate(1.5*(sqrt(5.0)-1));
1603 }
1604 CPolyhedronPtr CPolyhedron::CreatePentagonalRotunda(double radius) {
1605  return CreateJohnsonSolidWithConstantBase(10,radius,"R+");
1606 }
1607 CPolyhedronPtr CPolyhedron::CreateTriakisTetrahedron(double radius) {
1608  return CreateTruncatedTetrahedron(radius*3/sqrt(33.0))->getDual();
1609 }
1610 CPolyhedronPtr CPolyhedron::CreateRhombicDodecahedron(double radius) {
1611  return CreateCuboctahedron(radius/sqrt(2.0))->getDual();
1612 }
1613 CPolyhedronPtr CPolyhedron::CreateTriakisOctahedron(double radius) {
1614  return CreateTruncatedHexahedron(radius/sqrt((5-sqrt(8.0))))->getDual();
1615 }
1616 CPolyhedronPtr CPolyhedron::CreateTetrakisHexahedron(double radius) {
1617  return CreateTruncatedOctahedron(radius*sqrt(0.6))->getDual();
1618 }
1619 CPolyhedronPtr CPolyhedron::CreateDeltoidalIcositetrahedron(double radius) {
1620  return CreateRhombicuboctahedron(radius/sqrt(7-sqrt(32.0)),true)->getDual();
1621 }
1622 CPolyhedronPtr CPolyhedron::CreateRhombicTriacontahedron(double radius) {
1623  return CreateIcosidodecahedron(radius*sqrt(2/(5-sqrt(5.0))),true)->getDual();
1624 }
1625 CPolyhedronPtr CPolyhedron::CreateTriakisIcosahedron(double radius) {
1626  return CreateTruncatedDodecahedron(radius*sqrt(5/(25-8*sqrt(5.0))))->getDual();
1627 }
1628 CPolyhedronPtr CPolyhedron::CreatePentakisDodecahedron(double radius) {
1629  return CreateTruncatedIcosahedron(radius*sqrt(3/(17-6*sqrt(5.0))))->getDual();
1630 }
1631 CPolyhedronPtr CPolyhedron::CreateDeltoidalHexecontahedron(double radius) {
1632  return CreateRhombicosidodecahedron(radius*3.0/sqrt(15-2*sqrt(5.0)))->getDual();
1633 }
1634 CPolyhedronPtr CPolyhedron::CreateCubicPrism(const TPoint3D &p1,const TPoint3D &p2) {
1635  return CreateCubicPrism(p1.x,p2.x,p1.y,p2.y,p1.z,p2.z);
1636 }
1637 CPolyhedronPtr CPolyhedron::CreateFrustum(const vector<TPoint2D> &baseVertices,double height,double ratio) {
1638  return CreateTruncatedPyramid(baseVertices,height,ratio);
1639 }
1640 CPolyhedronPtr CPolyhedron::CreateCustomPrism(const vector<TPoint2D> &baseVertices,double height) {
1641  return CreateTruncatedPyramid(baseVertices,height,1.0);
1642 }
1643 CPolyhedronPtr CPolyhedron::CreateRegularAntiprism(uint32_t numBaseEdges,double baseRadius,double height) {
1644  return CreateCustomAntiprism(generateBase(numBaseEdges,baseRadius),generateShiftedBase(numBaseEdges,baseRadius),height);
1645 }
1646 CPolyhedronPtr CPolyhedron::CreateRegularPrism(uint32_t numBaseEdges,double baseRadius,double height) {
1647  return CreateCustomPrism(generateBase(numBaseEdges,baseRadius),height);
1648 }
1649 CPolyhedronPtr CPolyhedron::CreateRegularPyramid(uint32_t numBaseEdges,double baseRadius,double height) {
1650  return CreatePyramid(generateBase(numBaseEdges,baseRadius),height);
1651 }
1652 CPolyhedronPtr CPolyhedron::CreateRegularDoublePyramid(uint32_t numBaseEdges,double baseRadius,double height1,double height2) {
1653  return CreateDoublePyramid(generateBase(numBaseEdges,baseRadius),height1,height2);
1654 }
1655 CPolyhedronPtr CPolyhedron::CreateArchimedeanRegularPrism(uint32_t numBaseEdges,double baseRadius) {
1656  return CreateJohnsonSolidWithConstantBase(numBaseEdges,baseRadius,"PR");
1657 }
1658 CPolyhedronPtr CPolyhedron::CreateArchimedeanRegularAntiprism(uint32_t numBaseEdges,double baseRadius) {
1659  return CreateJohnsonSolidWithConstantBase(numBaseEdges,baseRadius,"A");
1660 }
1661 CPolyhedronPtr CPolyhedron::CreateRegularTruncatedPyramid(uint32_t numBaseEdges,double baseRadius,double height,double ratio) {
1662  return CreateTruncatedPyramid(generateBase(numBaseEdges,baseRadius),height,ratio);
1663 }
1664 CPolyhedronPtr CPolyhedron::CreateRegularFrustum(uint32_t numBaseEdges,double baseRadius,double height,double ratio) {
1665  return CreateRegularTruncatedPyramid(numBaseEdges,baseRadius,height,ratio);
1666 }
1667 CPolyhedronPtr CPolyhedron::CreateRegularBifrustum(uint32_t numBaseEdges,double baseRadius,double height1,double ratio1,double height2,double ratio2) {
1668  return CreateBifrustum(generateBase(numBaseEdges,baseRadius),height1,ratio1,height2,ratio2);
1669 }
1670 CPolyhedronPtr CPolyhedron::CreateCupola(uint32_t numBaseEdges,double edgeLength) {
1671  return CreateJohnsonSolidWithConstantBase(numBaseEdges,edgeLength/(2*sin(M_PI/numBaseEdges)),"C+");
1672 }
1673 CPolyhedronPtr CPolyhedron::CreateCatalanTrapezohedron(uint32_t numBaseEdges,double height) {
1674  return CreateArchimedeanRegularAntiprism(numBaseEdges,height)->getDual();
1675 }
1676 CPolyhedronPtr CPolyhedron::CreateCatalanDoublePyramid(uint32_t numBaseEdges,double height) {
1677  return CreateArchimedeanRegularPrism(numBaseEdges,height)->getDual();
1678 }
1679 
1680 CPolyhedronPtr CPolyhedron::CreateNoCheck(const vector<TPoint3D> &vertices,const vector<TPolyhedronFace> &faces)
1681 {
1682  return CPolyhedronPtr(new CPolyhedron(vertices,faces,false));
1683 }
1684 CPolyhedronPtr CPolyhedron::CreateEmpty()
1685 {
1686  return CPolyhedronPtr(new CPolyhedron());
1687 }
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)
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
bool BASE_IMPEXP splitInConvexComponents(const TPolygon2D &poly, std::vector< TPolygon2D > &components)
Splits a 2D polygon into convex components.
Definition: geometry.cpp:1854
GLdouble GLdouble z
Definition: glext.h:3734
OPENGL_IMPEXP mrpt::utils::CStream & operator<<(mrpt::utils::CStream &out, const mrpt::opengl::CLight &o)
Definition: CLight.cpp:132
GLAPI void GLAPIENTRY glEnable(GLenum cap)
JohnsonBodyPart
Definition: CPolyhedron.cpp:65
::mrpt::utils::CStream & operator>>(mrpt::utils::CStream &in, CAngularObservationMeshPtr &pObj)
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
bool BASE_IMPEXP traceRay(const std::vector< TPolygonWithPlane > &vec, const mrpt::poses::CPose3D &pose, double &dist)
Fast ray tracing method using polygons&#39; properties.
Definition: geometry.cpp:1989
GLenum GLenum GLenum GLenum GLenum scale
Definition: glext.h:5717
#define THROW_EXCEPTION(msg)
bool getVerticesAndFaces(const vector< math::TPolygon3D > &polys, vector< TPoint3D > &vertices, vector< CPolyhedron::TPolyhedronFace > &faces)
Definition: CPolyhedron.cpp:45
BASE_IMPEXP CRandomGenerator randomGenerator
A static instance of a CRandomGenerator class, for use in single-thread applications.
GLAPI void GLAPIENTRY glNormal3f(GLfloat nx, GLfloat ny, GLfloat nz)
GLenum GLsizei n
Definition: glext.h:4618
Scalar * iterator
Definition: eigen_plugins.h:23
This file implements several operations that operate element-wise on individual or pairs of container...
EIGEN_STRONG_INLINE iterator begin()
Definition: eigen_plugins.h:26
size_t additionalFaces(JohnsonBodyPart j, uint32_t numBaseEdges)
Standard object for storing any 3D lightweight object.
STL namespace.
#define M_PI
Definition: bits.h:78
const Scalar * const_iterator
Definition: eigen_plugins.h:24
#define GL_ONE_MINUS_SRC_ALPHA
Definition: glew.h:283
const_iterator find(const KEY &key) const
Definition: ts_hash_map.h:139
double z
X,Y,Z coordinates.
GLenum GLenum GLuint components
Definition: glext.h:6305
bool getPlanesIntersection(const vector< const TPlane *> &planes, TPoint3D &pnt)
GLdouble s
Definition: glext.h:3602
#define GL_POLYGON
Definition: glew.h:277
#define GL_LIGHTING
Definition: glew.h:381
GLsizei GLsizei GLuint * obj
Definition: glext.h:3902
GLAPI void GLAPIENTRY glLineWidth(GLfloat width)
This class represents arbitrary polyhedra.
Definition: CPolyhedron.h:34
const vector< TPoint3D > & verts
Definition: CPolyhedron.cpp:35
GLAPI void GLAPIENTRY glBlendFunc(GLenum sfactor, GLenum dfactor)
T square(const T x)
Inline function for the square of a number.
Definition: bits.h:52
A renderizable object suitable for rendering with OpenGL&#39;s display lists.
void composePoint(double lx, double ly, double lz, double &gx, double &gy, double &gz, mrpt::math::CMatrixFixedNumeric< double, 3, 3 > *out_jacobian_df_dpoint=NULL, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dpose=NULL, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dse3=NULL, 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:427
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
std::vector< uint32_t > vertices
Vector of indices to the vertex list.
Definition: CPolyhedron.h:73
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
GLuint dst
Definition: glext.h:6198
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:36
Struct used to store a polyhedron face.
Definition: CPolyhedron.h:72
GLuint index
Definition: glext.h:3891
const GLubyte * c
Definition: glext.h:5590
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
void unitarize()
Unitarize normal vector.
GLuint GLuint end
Definition: glext.h:3512
Struct used to store a polyhedron edge.
Definition: CPolyhedron.h:40
bool searchForEdge(const vector< CPolyhedron::TPolyhedronEdge > &es, uint32_t v1, uint32_t v2, size_t &where)
GLfloat GLfloat GLfloat GLfloat v3
Definition: glext.h:3924
GLubyte g
Definition: glext.h:5575
3D Plane, represented by its equation
int val
Definition: mrpt_jpeglib.h:953
bool getPlane(TPlane &p) const
Gets a plane which contains the polygon. Returns false if the polygon is skew and cannot be fit insid...
GLubyte GLubyte b
Definition: glext.h:5575
class BASE_IMPEXP TPolygon3D
int version
Definition: mrpt_jpeglib.h:898
GLAPI void GLAPIENTRY glBegin(GLenum mode)
#define GL_BLEND
Definition: glew.h:428
GLsizei const GLchar ** string
Definition: glext.h:3919
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
GLAPI void GLAPIENTRY glVertex3f(GLfloat x, GLfloat y, GLfloat z)
GLAPI void GLAPIENTRY glColor4ub(GLubyte red, GLubyte green, GLubyte blue, GLubyte alpha)
size_t additionalVertices(JohnsonBodyPart j, uint32_t numBaseEdges)
#define GL_SRC_ALPHA
Definition: glew.h:282
const GLdouble * v
Definition: glext.h:3603
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
bool analyzeJohnsonPartsString(const std::string &components, uint32_t numBaseEdges, vector< JohnsonBodyPart > &parts)
Definition: CPolyhedron.cpp:68
GLdouble GLdouble GLdouble r
Definition: glext.h:3618
GLfloat GLfloat v1
Definition: glext.h:3922
class OPENGL_IMPEXP CPolyhedron
Definition: CPolyhedron.h:17
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
double & operator[](size_t i)
double coefs[4]
Plane coefficients, stored as an array: .
void OPENGL_IMPEXP checkOpenGLError()
Checks glGetError and throws an exception if an error situation is found.
Definition: gl_utils.cpp:134
GLuint GLsizei GLsizei * length
Definition: glext.h:3900
GLuint in
Definition: glext.h:6301
The namespace for 3D scene representation and rendering.
double evaluatePoint(const TPoint3D &point) const
Evaluate a point in the plane&#39;s equation.
GLAPI void GLAPIENTRY glEnd(void)
GLenum GLint GLint y
Definition: glext.h:3516
T operator()(const CPolyhedron::TPolyhedronFace &f)
Definition: CPolyhedron.cpp:38
GLfloat GLfloat GLfloat v2
Definition: glext.h:3923
#define GL_LINES
Definition: glew.h:269
double operator[](size_t i) const
GLuint res
Definition: glext.h:6298
GLenum GLint x
Definition: glext.h:3516
Lightweight 3D point.
GLuint GLenum GLenum transform
Definition: glext.h:6092
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:3523
unsigned __int32 uint32_t
Definition: rptypes.h:49
GLAPI void GLAPIENTRY glDisable(GLenum cap)
GLubyte GLubyte GLubyte a
Definition: glext.h:5575
GLfloat GLfloat p
Definition: glext.h:5587
bool searchForFace(const vector< CPolyhedron::TPolyhedronFace > &fs, uint32_t v1, uint32_t v2, uint32_t v3)
bool BASE_IMPEXP intersect(const TSegment3D &s1, const TSegment3D &s2, TObject3D &obj)
Gets the intersection between two 3D segments.
Definition: geometry.cpp:568
GLuint GLuint GLsizei GLenum type
Definition: glext.h:3512
3D polygon, inheriting from std::vector<TPoint3D>
double BASE_IMPEXP distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.
Definition: geometry.cpp:1504
void getCenter(TPoint3D &p) const
Get polygon&#39;s central point.
double BASE_IMPEXP geometryEpsilon
Global epsilon to overcome small precision errors (Default=1e-5)
Definition: geometry.cpp:28
GLenum GLuint GLint GLenum face
Definition: glext.h:7014
bool faceContainsEdge(const CPolyhedron::TPolyhedronFace &f, const CPolyhedron::TPolyhedronEdge &e)
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.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019