/* +---------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)               |
   |                          http://www.mrpt.org/                             |
   |                                                                           |
   | Copyright (c) 2005-2015, Individual contributors, see AUTHORS file        |
   | See: http://www.mrpt.org/Authors - All rights reserved.                   |
   | Released under BSD License. See details in http://www.mrpt.org/License    |
   +---------------------------------------------------------------------------+ */

#include "opengl-precomp.h"  // Precompiled header
#include <mrpt/opengl/CPolyhedron.h>
#include <mrpt/math/CMatrixTemplate.h>
#include <mrpt/math/CMatrix.h>
#include <mrpt/poses/CPose3D.h>
#include <mrpt/math/geometry.h>
#include <mrpt/math/ops_containers.h> // dotProduct()
#include <mrpt/random.h>
#include <mrpt/utils/CStream.h>
#include <mrpt/utils/stl_serialization.h>

#include "opengl_internals.h"

using namespace mrpt;
using namespace mrpt::math;
using namespace mrpt::opengl;
using namespace mrpt::utils;
using namespace mrpt::poses;
using namespace std;

IMPLEMENTS_SERIALIZABLE(CPolyhedron,CRenderizableDisplayList,mrpt::opengl)

//Auxiliary data and code
template<class T> class FCreatePolygonFromFace	{
public:
	const vector<TPoint3D> &verts;
	FCreatePolygonFromFace(const vector<TPoint3D> &v):verts(v)	{}
	TPolygon3D p;
	T operator()(const CPolyhedron::TPolyhedronFace &f)	{
		p=TPolygon3D(f.vertices.size());
		for (size_t i=0;i<f.vertices.size();i++) p[i]=verts[f.vertices[i]];
		return p;
	}
};

bool getVerticesAndFaces(const vector<math::TPolygon3D> &polys,vector<TPoint3D> &vertices,vector<CPolyhedron::TPolyhedronFace> &faces)	{
	vertices.reserve(4*polys.size());
	faces.reserve(polys.size());
	for (std::vector<math::TPolygon3D>::const_iterator it=polys.begin();it!=polys.end();++it)	{
		size_t N=it->size();
		if (N<3) return false;
		CPolyhedron::TPolyhedronFace f;
		f.vertices.resize(N);
		for (size_t i=0;i<N;i++)	{
			vector<TPoint3D>::iterator it2=find(vertices.begin(),vertices.end(),(*it)[i]);
			if (it2==vertices.end())	{
				f.vertices[i]=vertices.size();
				vertices.push_back((*it)[i]);
			}	else f.vertices[i]=it2-vertices.begin();
		}
		faces.push_back(f);
	}
	return true;
}

enum JohnsonBodyPart	{
	INF_NO_BODY=-2,SUP_NO_BODY=-1,UPWARDS_PYRAMID=0,DOWNWARDS_PYRAMID=1,UPWARDS_CUPOLA=2,DOWNWARDS_CUPOLA=3,ROTATED_UPWARDS_CUPOLA=4,ROTATED_DOWNWARDS_CUPOLA=5,PRISM=6,ANTIPRISM=7,UPWARDS_ROTUNDA=8,DOWNWARDS_ROTUNDA=9,ROTATED_UPWARDS_ROTUNDA=10,ROTATED_DOWNWARDS_ROTUNDA=11
};
bool analyzeJohnsonPartsString(const std::string &components,uint32_t numBaseEdges,vector<JohnsonBodyPart> &parts)	{
	size_t N=components.length();
	size_t i=0;
	bool rot=false;
	while (i<N)	{
		switch (components[i])	{
			case 'A':case 'a':
				if (parts.size()==0) parts.push_back(INF_NO_BODY);
				parts.push_back(ANTIPRISM);
				break;
			case 'C':case 'c':
				if (numBaseEdges&1) return false;
				if (i==N-1) return false;
				i++;
				if (components[i]=='+')	{
					if (i!=N-1) return false;
					if (parts.size()==0) parts.push_back(INF_NO_BODY);
					parts.push_back(rot?ROTATED_UPWARDS_CUPOLA:UPWARDS_CUPOLA);
					rot=false;
				}	else if (components[i]=='-')	{
					if (parts.size()>0) return false;
					parts.push_back(rot?ROTATED_DOWNWARDS_CUPOLA:DOWNWARDS_CUPOLA);
					rot=false;
				}	else return false;
				break;
			case 'R':case 'r':
				if (numBaseEdges!=10) return false;
				if (i==N-1) return false;
				i++;
				if (components[i]=='+')	{
					if (i!=N-1) return false;
					if (parts.size()==0) parts.push_back(INF_NO_BODY);
					parts.push_back(rot?ROTATED_UPWARDS_ROTUNDA:UPWARDS_ROTUNDA);
					rot=false;
				}	else if (components[i]=='-')	{
					if (parts.size()>0) return false;
					parts.push_back(rot?ROTATED_DOWNWARDS_ROTUNDA:DOWNWARDS_ROTUNDA);
					rot=false;
				}	else return false;
				break;
			case 'G':case 'g':
				if (i==N-1) return false;
				i++;
				if (components[i]=='C'||components[i]=='R')	{
					rot=true;
					continue;
				}	else return false;
			case 'P':case 'p':
				if (i==N-1) return false;
				i++;
				switch (components[i])	{
					case '+':
						if (numBaseEdges>5) return false;
						if (i!=N-1) return false;
						if (parts.size()==0) parts.push_back(INF_NO_BODY);
						parts.push_back(UPWARDS_PYRAMID);
						break;
					case '-':
						if (numBaseEdges>5) return false;
						if (i!=1) return false;
						parts.push_back(DOWNWARDS_PYRAMID);
						break;
					case 'R':case 'r':
						if (parts.size()>0&&(*parts.rbegin()==PRISM)) return false;
						if (parts.size()==0) parts.push_back(INF_NO_BODY);
						parts.push_back(PRISM);
						break;
					default:
						return false;
				}
				break;
			default:
				return false;
		}
		i++;
	}
	if (parts.size()==0) return false;
	JohnsonBodyPart p=*parts.rbegin();
	if (p!=UPWARDS_PYRAMID&&p!=UPWARDS_CUPOLA&&p!=ROTATED_UPWARDS_CUPOLA&&p!=UPWARDS_ROTUNDA&&p!=ROTATED_UPWARDS_ROTUNDA) parts.push_back(SUP_NO_BODY);
	return true;
}
inline size_t additionalVertices(JohnsonBodyPart j,uint32_t numBaseEdges)	{
	if (j==INF_NO_BODY||j==SUP_NO_BODY) return 0;
	else if (j<UPWARDS_CUPOLA) return 1;	//j is a pyramid
	else if (j<PRISM) return numBaseEdges>>1;	//j is a cupola
	else if (j<UPWARDS_ROTUNDA) return numBaseEdges;	//j is a prism or antiprism
	else return 10;	//j is a rotunda
}
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)	{
	size_t edges2=numBaseEdges>>1;
	double minorRadius=baseRadius*sin(M_PI/numBaseEdges)/sin(M_PI/edges2);
	//"Proper base"'s apothem=base radius*cos(2*pi/(2*numBaseEdges))
	//"Small base"'s apothem=small radius*cos(2*pi/2*(numBaseEdges/2))
	//Cupola's height is so that (Da^2+Height^2=Edge length^2, where Da is the difference between both apothems.
	double h=sqrt(square(edgeLength)-square(baseRadius*cos(M_PI/numBaseEdges)-minorRadius*cos(M_PI/edges2)));
	double height=verts[base].z+(isUpwards?h:-h);
	angleShift+=M_PI/edges2+(isRotated?-M_PI/numBaseEdges:M_PI/numBaseEdges);
	size_t minorBase=verts.size();
	for (size_t i=0;i<edges2;i++)	{
		double ang=angleShift+2*M_PI*i/edges2;
		verts.push_back(TPoint3D(minorRadius*cos(ang),minorRadius*sin(ang),height));
	}
	CPolyhedron::TPolyhedronFace tri,quad,cBase;
	tri.vertices.resize(3);
	quad.vertices.resize(4);
	cBase.vertices.resize(edges2);
	size_t iq=isRotated?1:2,it=0;
	for (size_t i=0;i<edges2;i++)	{
		cBase.vertices[i]=it+minorBase;
		size_t iiq=(iq+1)%numBaseEdges+base;
		size_t iiiq=(iiq+1)%numBaseEdges+base;
		size_t iit=(it+1)%edges2+minorBase;
		quad.vertices[0]=it+minorBase;
		quad.vertices[1]=iit;
		quad.vertices[2]=iiq;
		quad.vertices[3]=iq+base;
		tri.vertices[0]=iit;
		tri.vertices[1]=iiq;
		tri.vertices[2]=iiiq;
		iq=(iq+2)%numBaseEdges;
		it=(it+1)%edges2;
		faces.push_back(tri);
		faces.push_back(quad);
	}
	if (edges2>=3) faces.push_back(cBase);
}
void insertRotunda(double angleShift,double baseRadius,bool isRotated,bool isUpwards,size_t base,vector<TPoint3D> &verts,vector<CPolyhedron::TPolyhedronFace> &faces)	{
	double R1=baseRadius*sqrt((5.0-sqrt(5.0))/10.0);
	double R2=baseRadius*sqrt((5.0+sqrt(5.0))/10.0);
	double baseHeight=verts[base].z;
	TPoint3D p1[5],p2[5];
	angleShift+=M_PI/10;
	if (isRotated) angleShift+=M_PI/5;
	for (size_t i=0;i<5;i++)	{
		double a=(i+i+1)*M_PI/5+angleShift;
		double b=(i+i)*M_PI/5+angleShift;
		double ca=cos(a),sa=sin(a),cb=cos(b),sb=sin(b);
		p1[i].x=R1*ca;
		p1[i].y=R1*sa;
		p1[i].z=baseHeight+(isUpwards?R2:-R2);
		p2[i].x=R2*cb;
		p2[i].y=R2*sb;
		p2[i].z=baseHeight+(isUpwards?R1:-R1);
	}
	size_t newBase=verts.size();
	for (size_t i=0;i<5;i++) verts.push_back(p1[i]);
	for (size_t i=0;i<5;i++) verts.push_back(p2[i]);
	CPolyhedron::TPolyhedronFace f,g;
	f.vertices.resize(3);
	g.vertices.resize(5);
	size_t baseStart=isRotated?2:1;
	for (size_t i=0;i<5;i++)	{
		size_t ii=(i+1)%5;
		f.vertices[0]=newBase+i;
		f.vertices[1]=newBase+ii;
		f.vertices[2]=newBase+ii+5;
		faces.push_back(f);
		f.vertices[0]=newBase+i+5;
		f.vertices[1]=((i+i+baseStart)%10)+base;
		f.vertices[2]=((i+i+9+baseStart)%10)+base;
		faces.push_back(f);
		g.vertices[0]=newBase+(ii%5)+5;
		g.vertices[1]=newBase+i;
		g.vertices[2]=newBase+i+5;
		g.vertices[3]=(i+i+baseStart)%10+base;
		g.vertices[4]=(i+i+baseStart+1)%10+base;
		faces.push_back(g);
	}
	for (size_t i=0;i<5;i++) g.vertices[i]=i+newBase;
	faces.push_back(g);
	return;
}
inline size_t additionalFaces(JohnsonBodyPart j,uint32_t numBaseEdges)	{
	if (j==INF_NO_BODY||j==SUP_NO_BODY) return 1;	//j is a base
	else if (j<UPWARDS_CUPOLA) return numBaseEdges;	//j is a pyramid
	else if (j<PRISM) return numBaseEdges+ ((numBaseEdges>=6)?1:0);	//j is a cupola
	else if (j==PRISM) return numBaseEdges;
	else if (j==ANTIPRISM) return numBaseEdges<<1;
	else return 16;	//j is a rotunda
}

inline bool faceContainsEdge(const CPolyhedron::TPolyhedronFace &f,const CPolyhedron::TPolyhedronEdge &e)	{
	char hm=0;
	for (vector<uint32_t>::const_iterator it=f.vertices.begin();it!=f.vertices.end();++it) if (*it==e.v1||*it==e.v2) hm++;
	return hm==2;
}

bool getPlanesIntersection(const vector<const TPlane *> &planes,TPoint3D &pnt)	{
	if (planes.size()<3) return false;
	char o=0;
	TPlane pl=*planes[0];
	TLine3D l;
	TObject3D obj;
	for (size_t i=1;i<planes.size();i++) switch (o)	{
		case 0:
			if (!intersect(pl,*planes[i],obj)) return false;
			if (obj.getPlane(pl)) o=0;
			else if (obj.getLine(l)) o=1;
			else if (obj.getPoint(pnt)) o=2;
			else return false;
			break;
		case 1:
			if (!intersect(l,*planes[i],obj)) return false;
			if (obj.getLine(l)) o=1;
			else if (obj.getPoint(pnt)) o=2;
			else return false;
			break;
		case 2:
			if (!planes[i]->contains(pnt)) return false;
			break;
		default:
			return false;
	}
	return o==2;
}

bool searchForFace(const vector<CPolyhedron::TPolyhedronFace> &fs,uint32_t v1,uint32_t v2,uint32_t v3)	{
	for (vector<CPolyhedron::TPolyhedronFace>::const_iterator it=fs.begin();it!=fs.end();++it)	{
		const vector<uint32_t> &f=it->vertices;
		size_t hmf=0;
		for (vector<uint32_t>::const_iterator it2=f.begin();it2!=f.end();++it2)	{
			if (*it2==v1) hmf|=1;
			else if (*it2==v2) hmf|=2;
			else if (*it2==v3) hmf|=4;
		}
		if (hmf==7) return true;
	}
	return false;
}
bool searchForEdge(const vector<CPolyhedron::TPolyhedronEdge> &es,uint32_t v1,uint32_t v2,size_t &where)	{
	for (where=0;where<es.size();where++)	{
		const CPolyhedron::TPolyhedronEdge &e=es[where];
		if (e.v1==v1&&e.v2==v2) return true;
		else if (e.v1==v2&&e.v2==v1) return false;
	}
	throw std::logic_error("Internal error. Edge not found");
}
bool searchForEdge(const vector<CPolyhedron::TPolyhedronFace>::const_iterator &begin,const vector<CPolyhedron::TPolyhedronFace>::const_iterator &end,uint32_t v1,uint32_t v2)	{
	for (vector<CPolyhedron::TPolyhedronFace>::const_iterator it=begin;it!=end;++it)	{
		const vector<uint32_t> &f=it->vertices;
		char res=0;
		for (vector<uint32_t>::const_iterator it2=f.begin();it2!=f.end();++it2) if (*it2==v1) res|=1;
		else if (*it2==v2) res|=2;
		if (res==3) return true;
	}
	return false;
}
double getHeight(const TPolygon3D &p,const TPoint3D &c)	{
	size_t N=p.size();
	if (N>5||N<3) throw std::logic_error("Faces must have exactly 3, 4 or 5 vertices.");
	double r=mrpt::math::distance(p[0],c);
	double l=mrpt::math::distance(p[0],p[1]);
	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.");
	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.");
	return sqrt(square(l)-square(r));
}
struct SegmentVector	{
	double d[3];
	double mod;
	inline double &operator[](size_t i) {
		return d[i];
	}
	inline double operator[](size_t i) const	{
		return d[i];
	}
};
//End of auxiliary data and code

double CPolyhedron::TPolyhedronEdge::length(const vector<TPoint3D> &vertices) const	{
	return mrpt::math::distance(vertices[v1],vertices[v2]);
}

double CPolyhedron::TPolyhedronFace::area(const vector<TPoint3D> &vs) const	{
	//Calculate as fan of triangles.
	size_t N=vertices.size();
	vector<SegmentVector> d(N-1);
	for (size_t i=1;i<N;i++)	{
		d[i-1].mod=0;
		for (size_t j=0;j<3;j++)	{
			d[i-1][j]=vs[vertices[i]][j]-vs[vertices[0]][j];
			d[i-1].mod+=square(d[i-1][j]);
		}
		d[i-1].mod=sqrt(d[i-1].mod);
	}
	double res=0;
	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])));
	return res/2;
}

void CPolyhedron::TPolyhedronFace::getCenter(const vector<TPoint3D> &vrts,TPoint3D &p) const	{
	p.x=p.y=p.z=0.0;
	for (vector<uint32_t>::const_iterator it=vertices.begin();it!=vertices.end();++it)	{
		p.x+=vrts[*it].x;
		p.y+=vrts[*it].y;
		p.z+=vrts[*it].z;
	}
	size_t N=vertices.size();
	p.x/=N;
	p.y/=N;
	p.z/=N;
}

CPolyhedronPtr CPolyhedron::Create(const std::vector<math::TPolygon3D> &polys)	{
	vector<TPoint3D> vertices(0);
	vector<TPolyhedronFace> faces;
	if (getVerticesAndFaces(polys,vertices,faces)) return Create(vertices,faces);
	else return CreateEmpty();
}

CPolyhedronPtr CPolyhedron::CreateCubicPrism(double x1,double x2,double y1,double y2,double z1,double z2)	{
	vector<TPoint3D> verts;
	vector<TPolyhedronFace> faces;
	for (int i=0;i<8;i++) verts.push_back(TPoint3D((i&1)?x2:x1,(i&2)?y2:y1,(i&4)?z2:z1));
	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};
	TPolyhedronFace f;
	for (uint32_t *p=reinterpret_cast<uint32_t *>(&faceVertices);p<24+reinterpret_cast<uint32_t *>(&faceVertices);p+=4)	{
		f.vertices.insert(f.vertices.begin(),p,p+4);
		faces.push_back(f);
		f.vertices.clear();
	}
	return CreateNoCheck(verts,faces);
}

CPolyhedronPtr CPolyhedron::CreatePyramid(const vector<TPoint2D> &baseVertices,double height)	{
	uint32_t n=baseVertices.size();
	if (baseVertices.size()<3) throw std::logic_error("Not enought vertices");
	vector<TPoint3D> verts;
	vector<TPolyhedronFace> faces;
	verts.push_back(TPoint3D(0,0,height));
	for (vector<TPoint2D>::const_iterator it=baseVertices.begin();it!=baseVertices.end();++it) verts.push_back(TPoint3D(it->x,it->y,0));
	TPolyhedronFace f,g;
	f.vertices.push_back(0);
	f.vertices.push_back(n);
	f.vertices.push_back(1);
	g.vertices.push_back(1);
	faces.push_back(f);
	for (uint32_t i=2;i<=n;i++)	{
		f.vertices.erase(f.vertices.begin()+1);
		f.vertices.push_back(i);
		faces.push_back(f);
		g.vertices.push_back(i);
	}
	faces.push_back(g);
	return CreateNoCheck(verts,faces);
}

CPolyhedronPtr CPolyhedron::CreateDoublePyramid(const vector<TPoint2D> &baseVertices,double height1,double height2)	{
	uint32_t N=baseVertices.size();
	if (N<3) throw std::logic_error("Not enought vertices");
	vector<TPoint3D> verts;
	verts.reserve(N+2);
	vector<TPolyhedronFace> faces;
	faces.reserve(N<<1);
	verts.push_back(TPoint3D(0,0,height1));
	for (vector<TPoint2D>::const_iterator it=baseVertices.begin();it!=baseVertices.end();++it) verts.push_back(TPoint3D(it->x,it->y,0));
	verts.push_back(TPoint3D(0,0,-height2));
	TPolyhedronFace f,g;
	f.vertices.resize(3);
	g.vertices.resize(3);
	f.vertices[0]=0;
	g.vertices[0]=N+1;
	for (uint32_t i=1;i<N;i++)	{
		f.vertices[1]=g.vertices[1]=i;
		f.vertices[2]=g.vertices[2]=i+1;
		faces.push_back(f);
		faces.push_back(g);
	}
	f.vertices[1]=g.vertices[1]=1;
	faces.push_back(f);
	faces.push_back(g);
	return CreateNoCheck(verts,faces);
}

CPolyhedronPtr CPolyhedron::CreateTruncatedPyramid(const vector<TPoint2D> &baseVertices,double height,double ratio)	{
	uint32_t n=baseVertices.size();
	if (n<3) throw std::logic_error("Not enough vertices");
	vector<TPoint3D> verts(n+n);
	vector<TPolyhedronFace> faces(n+2);
	TPolyhedronFace f,g,h;
	f.vertices.resize(4);
	g.vertices.resize(n);
	h.vertices.resize(n);
	for (uint32_t i=0;i<n;i++)	{
		verts[i]=TPoint3D(baseVertices[i].x,baseVertices[i].y,0);
		verts[i+n]=TPoint3D(baseVertices[i].x*ratio,baseVertices[i].y*ratio,height);
		uint32_t ii=(i+1)%n;
		f.vertices[0]=i;
		f.vertices[1]=ii;
		f.vertices[2]=ii+n;
		f.vertices[3]=i+n;
		faces[i]=f;
		g.vertices[i]=i;
		h.vertices[i]=i+n;
	}
	faces[n]=g;
	faces[n+1]=h;
	return CreateNoCheck(verts,faces);
}

CPolyhedronPtr CPolyhedron::CreateCustomAntiprism(const vector<TPoint2D> &bottomBase,const vector<TPoint2D> &topBase,const double height)	{
	uint32_t n=bottomBase.size();
	if (n<3) throw std::logic_error("Not enough vertices");
	if (n!=topBase.size()) throw std::logic_error("Bases' number of vertices do not match");
	vector<TPoint3D> verts(n+n);
	vector<TPolyhedronFace> faces(n+n+2);
	TPolyhedronFace f,g,h;
	f.vertices.resize(3);
	g.vertices.resize(n);
	h.vertices.resize(n);
	for (uint32_t i=0;i<n;i++)	{
		verts[i]=TPoint3D(bottomBase[i].x,bottomBase[i].y,0);
		verts[n+i]=TPoint3D(topBase[i].x,topBase[i].y,height);
		uint32_t ii=(i+1)%n;
		f.vertices[0]=i;
		f.vertices[1]=ii;
		f.vertices[2]=i+n;
		faces[i]=f;
		f.vertices[0]=i+n;
		f.vertices[1]=ii+n;
		f.vertices[2]=ii;
		faces[n+i]=f;
		g.vertices[i]=i;
		h.vertices[i]=n+i;
	}
	faces[n+n]=g;
	faces[n+n+1]=h;
	return CreateNoCheck(verts,faces);
}

CPolyhedronPtr CPolyhedron::CreateParallelepiped(const TPoint3D &base,const TPoint3D &v1,const TPoint3D &v2,const TPoint3D &v3)	{
	vector<TPoint3D> verts(8);
	vector<TPolyhedronFace> faces(6);
	for (uint32_t i=0;i<8;i++)	{
		verts[i]=base;
		if (i&1) verts[i]=verts[i]+v1;
		if (i&2) verts[i]=verts[i]+v2;
		if (i&4) verts[i]=verts[i]+v3;
	}
	TPolyhedronFace f;
	f.vertices.resize(4);
	f.vertices[0]=0;
	f.vertices[1]=1;
	f.vertices[2]=3;
	f.vertices[3]=2;
	faces[0]=f;
	//f.vertices[0]=0;
	//f.vertices[1]=1;
	f.vertices[2]=5;
	f.vertices[3]=4;
	faces[1]=f;
	//f.vertices[0]=0;
	f.vertices[1]=2;
	f.vertices[2]=6;
	//f.vertices[3]=4;
	faces[2]=f;
	for (uint32_t i=0;i<3;i++)	{
		uint32_t valueAdd=4>>i;
		faces[i+3].vertices.resize(4);
		for (uint32_t j=0;j<4;j++) faces[i+3].vertices[j]=faces[i].vertices[j]+valueAdd;
	}
	return CreateNoCheck(verts,faces);
}

CPolyhedronPtr CPolyhedron::CreateBifrustum(const vector<TPoint2D> &baseVertices,double height1,double ratio1,double height2,double ratio2)	{
	//TODO: check special case in which ratio=0.
	size_t N=baseVertices.size();
	vector<TPoint3D> verts(3*N);
	size_t N2=N+N;
	for (size_t i=0;i<N;i++)	{
		double x=baseVertices[i].x;
		double y=baseVertices[i].y;
		verts[i].x=x;
		verts[i].y=y;
		verts[i].z=0;
		verts[i+N].x=x*ratio1;
		verts[i+N].y=y*ratio1;
		verts[i+N].z=height1;
		verts[i+N2].x=x*ratio2;
		verts[i+N2].y=y*ratio2;
		verts[i+N2].z=-height2;	//This is not an error. This way, two positive heights produce an actual bifrustum.
	}
	vector<TPolyhedronFace> faces(N2+2);
	TPolyhedronFace f,g,h;
	f.vertices.resize(4);
	g.vertices.resize(N);
	h.vertices.resize(N);
	for (size_t i=0;i<N;i++)	{
		size_t i2=(i+1)%N;
		f.vertices[0]=i;
		f.vertices[1]=i2;
		f.vertices[2]=i2+N;
		f.vertices[3]=i+N;
		faces[i]=f;
		f.vertices[2]=i2+N2;
		f.vertices[3]=i+N2;
		faces[i+N]=f;
		g.vertices[i]=i+N;
		h.vertices[i]=i+N2;
	}
	faces[N2]=g;
	faces[N2+1]=h;
	return CreateNoCheck(verts,faces);
}

CPolyhedronPtr CPolyhedron::CreateTrapezohedron(uint32_t numBaseEdges,double baseRadius,double basesDistance)	{
	if (numBaseEdges<3) throw std::logic_error("Not enough vertices");
	if (basesDistance==0||baseRadius==0) return CreateEmpty();
	size_t numBaseEdges2=numBaseEdges<<1;
	vector<TPoint3D> verts(numBaseEdges2+2);
	double space=2*M_PI/numBaseEdges;
	double shift=space/2;
	double height1=basesDistance/2;
	double cospii=cos(M_PI/numBaseEdges);
	double height2=height1*(cospii+1)/(1-cospii);	//Apex height, calculated so each face conforms a plane
	for (size_t i=0;i<numBaseEdges;i++)	{
		double ang=space*i;
		double ang2=ang+shift;
		size_t ii=i+numBaseEdges;
		verts[i].x=baseRadius*cos(ang);
		verts[i].y=baseRadius*sin(ang);
		verts[i].z=-height1;
		verts[ii].x=baseRadius*cos(ang2);
		verts[ii].y=baseRadius*sin(ang2);
		verts[ii].z=height1;
	}
	verts[numBaseEdges2].x=0;
	verts[numBaseEdges2].y=0;
	verts[numBaseEdges2].z=-height2;
	verts[numBaseEdges2+1].x=0;
	verts[numBaseEdges2+1].y=0;
	verts[numBaseEdges2+1].z=height2;
	vector<TPolyhedronFace> faces(numBaseEdges2);
	TPolyhedronFace f,g;
	f.vertices.resize(4);
	g.vertices.resize(4);
	f.vertices[3]=numBaseEdges2;
	g.vertices[3]=numBaseEdges2+1;
	for (size_t i=0;i<numBaseEdges;i++)	{
		size_t ii=(i+1)%numBaseEdges;
		size_t i2=i+numBaseEdges;
		f.vertices[0]=i;
		f.vertices[1]=i2;
		f.vertices[2]=ii;
		g.vertices[0]=i2;
		g.vertices[1]=ii;
		g.vertices[2]=ii+numBaseEdges;
		faces[i]=f;
		faces[i+numBaseEdges]=g;
	}
	return CreateNoCheck(verts,faces);
}

CPolyhedronPtr CPolyhedron::CreateJohnsonSolidWithConstantBase(uint32_t numBaseEdges,double baseRadius,const std::string &components,size_t shifts)	{
	if (baseRadius==0) return CreateEmpty();
	if (numBaseEdges<3) throw std::logic_error("Not enough vertices");
	vector<JohnsonBodyPart> parts;
	if (!analyzeJohnsonPartsString(components,numBaseEdges,parts)) throw std::logic_error("Invalid string");
	//Some common values are computed
	size_t nParts=parts.size();
	double edgeLength=2*baseRadius*sin(M_PI/numBaseEdges);
	double antiPrismHeight=sqrt(square(edgeLength)-square(baseRadius)*(2-2*cos(M_PI/numBaseEdges)));
	//Vertices' and faces' vectors are computed
	size_t nVerts=numBaseEdges*(nParts-1)+additionalVertices(parts[0],numBaseEdges)+additionalVertices(*parts.rbegin(),numBaseEdges);
	size_t nFaces=0;
	for (size_t i=0;i<nParts;i++) nFaces+=additionalFaces(parts[i],numBaseEdges);
	vector<TPoint3D> verts;
	verts.reserve(nVerts);
	vector<TPolyhedronFace> faces;
	faces.reserve(nFaces);
	//Each base's position is computed. Also, the height is set so that the polyhedron is vertically centered in z=0.
	double h,mHeight=0;
	vector<pair<double,size_t> > basePositionInfo(nParts-1);
	for (size_t i=0;i<nParts-1;i++)	{
		if (parts[i]==PRISM) h=edgeLength;
		else if (parts[i]==ANTIPRISM)	{
			h=antiPrismHeight;
			shifts++;
		}	else h=0;
		basePositionInfo[i]=make_pair(mHeight+=h,shifts);
	}
	mHeight/=2;
	double semi=M_PI/numBaseEdges;
	//All the bases are generated and inserted into the vertices' vector.
	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);
	size_t initialBase=0,endBase=0;
	TPolyhedronFace face;
	face.vertices.reserve(numBaseEdges);
	//Each body is inserted.
	for (size_t i=0;i<nParts;i++)	{
		switch (parts[i])	{
			case INF_NO_BODY:
				//Inferior base.
				face.vertices.resize(numBaseEdges);
				for (size_t i=0;i<numBaseEdges;i++) face.vertices[i]=endBase+i;
				faces.push_back(face);
			break;
			case SUP_NO_BODY:
				//Superior base.
				face.vertices.resize(numBaseEdges);
				for (size_t i=0;i<numBaseEdges;i++) face.vertices[i]=initialBase+i;
				faces.push_back(face);
			break;
			case UPWARDS_PYRAMID:	{
				//Upwards-pointing pyramid. There must be 5 or less vertices.
				double apexHeight=baseRadius*sqrt(4*square(sin(M_PI/numBaseEdges))-1);
				face.vertices.resize(3);
				face.vertices[0]=verts.size();
				face.vertices[1]=initialBase+numBaseEdges-1;
				face.vertices[2]=initialBase;
				do	{
					faces.push_back(face);
					face.vertices[1]=face.vertices[2];
					face.vertices[2]++;
				}	while (face.vertices[2]<initialBase+numBaseEdges);
				verts.push_back(TPoint3D(0,0,verts[initialBase].z+apexHeight));
				break;
			}	case DOWNWARDS_PYRAMID:	{
				//Downwards-pointing pyramid. There must be 5 or less vertices.
				double apexHeight=baseRadius*sqrt(4*square(sin(M_PI/numBaseEdges))-1);
				face.vertices.resize(3);
				face.vertices[0]=verts.size();
				face.vertices[1]=endBase+numBaseEdges-1;
				face.vertices[2]=endBase;
				do	{
					faces.push_back(face);
					face.vertices[1]=face.vertices[2];
					face.vertices[2]++;
				}	while (face.vertices[2]<endBase+numBaseEdges);
				verts.push_back(TPoint3D(0,0,verts[endBase].z-apexHeight));
				break;
			}	case UPWARDS_CUPOLA:
				//Upwards-pointing cupola. There must be an even amount of vertices.
				insertCupola(numBaseEdges,basePositionInfo.rbegin()->second*semi,baseRadius,edgeLength,false,true,initialBase,verts,faces);
			break;
			case DOWNWARDS_CUPOLA:
				//Downwards-pointing cupola. There must be an even amount of vertices.
				insertCupola(numBaseEdges,basePositionInfo[0].second*semi,baseRadius,edgeLength,false,false,endBase,verts,faces);
			break;
			case ROTATED_UPWARDS_CUPOLA:
				//Upwards-pointing, slightly rotated, cupola. There must be an even amount of vertices.
				insertCupola(numBaseEdges,basePositionInfo.rbegin()->second*semi,baseRadius,edgeLength,true,true,initialBase,verts,faces);
			break;
			case ROTATED_DOWNWARDS_CUPOLA:
				//Downwards-pointing, slightly rotated, cupola. There must be an even amount of vertices.
				insertCupola(numBaseEdges,basePositionInfo[0].second*semi,baseRadius,edgeLength,true,false,endBase,verts,faces);
			break;
			case PRISM:
				//Archimedean prism.
				face.vertices.resize(4);
				for (size_t i=0;i<numBaseEdges;i++)	{
					size_t ii=(i+1)%numBaseEdges;
					face.vertices[0]=initialBase+i;
					face.vertices[1]=endBase+i;
					face.vertices[2]=endBase+ii;
					face.vertices[3]=initialBase+ii;
					faces.push_back(face);
				}
			break;
			case ANTIPRISM:	{
				//Archimedean antiprism.
				face.vertices.resize(3);
				face.vertices[0]=initialBase;
				face.vertices[1]=endBase;
				face.vertices[2]=initialBase+1;
				bool nextIsEnd=true;
				size_t nextEnd=1;
				size_t nextInitial=2;
				for (size_t i=0;i<numBaseEdges<<1;i++)	{
					faces.push_back(face);
					face.vertices[0]=face.vertices[1];
					face.vertices[1]=face.vertices[2];
					if (nextIsEnd)	{
						face.vertices[2]=endBase+nextEnd;
						nextEnd=(nextEnd+1)%numBaseEdges;
					}	else	{
						face.vertices[2]=initialBase+nextInitial;
						nextInitial=(nextInitial+1)%numBaseEdges;
					}
					nextIsEnd=!nextIsEnd;
				}
				break;
			}	case UPWARDS_ROTUNDA:
				//Upwards-pointing pentagonal rotunda. Only for bases of exactly 10 vertices.
				insertRotunda(basePositionInfo.rbegin()->second*semi,baseRadius,false,true,initialBase,verts,faces);
			break;
			case DOWNWARDS_ROTUNDA:
				//Downwards-pointing pentagonal rotunda. Only for bases of exactly 10 vertices.
				insertRotunda(basePositionInfo[0].second*semi,baseRadius,false,false,endBase,verts,faces);
			break;
			case ROTATED_UPWARDS_ROTUNDA:
				//Upwards-pointing, slightly rotated, pentagonal rotunda. Only for bases of exactly 10 vertices.
				insertRotunda(basePositionInfo.rbegin()->second*semi,baseRadius,true,true,initialBase,verts,faces);
			break;
			case ROTATED_DOWNWARDS_ROTUNDA:
				//Downwards-pointing, slightly rotated, pentagonal rotunda. Only for bases of exactly 10 vertices.
				insertRotunda(basePositionInfo[0].second*semi,baseRadius,true,false,endBase,verts,faces);
			break;
			default:
				throw std::logic_error("Internal error");
		}
		initialBase=endBase;
		endBase+=numBaseEdges;
	}
	return CreateNoCheck(verts,faces);
}

/*---------------------------------------------------------------
							render
  ---------------------------------------------------------------*/

void CPolyhedron::render_dl() const	{
#if MRPT_HAS_OPENGL_GLUT
	if (mWireframe)	{
		glDisable(GL_LIGHTING);  // Disable lights when drawing lines

		glLineWidth(mLineWidth);
		checkOpenGLError();
		glColor4ub(m_color.R,m_color.G,m_color.B,m_color.A);
		glBegin(GL_LINES);
		for (vector<TPolyhedronEdge>::const_iterator it=mEdges.begin();it!=mEdges.end();++it)	{
			TPoint3D p=mVertices[it->v1];
			glVertex3f(p.x,p.y,p.z);
			p=mVertices[it->v2];
			glVertex3f(p.x,p.y,p.z);
		}
		glEnd();
		glEnable(GL_LIGHTING);  // Disable lights when drawing lines
	}	else	{
		checkOpenGLError();
		glEnable(GL_BLEND);
		glBlendFunc(GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA);

		glColor4ub(m_color.R,m_color.G,m_color.B,m_color.A);
		for (vector<TPolyhedronFace>::const_iterator it=mFaces.begin();it!=mFaces.end();++it)	{
			glBegin(GL_POLYGON);
			glNormal3f(it->normal[0],it->normal[1],it->normal[2]);
			for (vector<uint32_t>::const_iterator it2=it->vertices.begin();it2!=it->vertices.end();++it2)	{
				const TPoint3D &p=mVertices[*it2];
				glVertex3f(p.x,p.y,p.z);
			}
			glEnd();
		}
		glDisable(GL_BLEND);

	}
#endif
}

bool CPolyhedron::traceRay(const mrpt::poses::CPose3D &o,double &dist) const	{
	if (!polygonsUpToDate) updatePolygons();
	return math::traceRay(tempPolygons,o-this->m_pose,dist);
}

void CPolyhedron::getEdgesLength(std::vector<double> &lengths) const	{
	lengths.resize(mEdges.size());
	std::vector<double>::iterator it2=lengths.begin();
	for (std::vector<TPolyhedronEdge>::const_iterator it=mEdges.begin();it!=mEdges.end();++it,++it2) *it2=it->length(mVertices);
}

void CPolyhedron::getFacesArea(std::vector<double> &areas) const	{
	areas.resize(mFaces.size());
	std::vector<double>::iterator it2=areas.begin();
	for (std::vector<TPolyhedronFace>::const_iterator it=mFaces.begin();it!=mFaces.end();++it,++it2) *it2=it->area(mVertices);
}

double CPolyhedron::getVolume() const	{
	//TODO. Calculate as set of pyramids whose apices are situated in the center of the polyhedron (will work only with convex polyhedrons).
	//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.
	TPoint3D center;
	getCenter(center);
	double res=0;
	if (!polygonsUpToDate) updatePolygons();
	vector<TPolygonWithPlane>::const_iterator itP=tempPolygons.begin();
	vector<double> areas(mFaces.size());
	getFacesArea(areas);
	vector<double>::const_iterator itA=areas.begin();
	for (vector<TPolyhedronFace>::const_iterator it=mFaces.begin();it!=mFaces.end();++it,++itP,++itA) res+=abs(itP->plane.distance(center))*(*itA);
	return res/3;
}

void CPolyhedron::getSetOfPolygons(std::vector<math::TPolygon3D> &vec) const	{
	if (!polygonsUpToDate) updatePolygons();
	size_t N=tempPolygons.size();
	vec.resize(N);
	for (size_t i=0;i<N;i++) vec[i]=tempPolygons[i].poly;
}

void CPolyhedron::getSetOfPolygonsAbsolute(std::vector<math::TPolygon3D> &vec) const	{
	vec.resize(mFaces.size());
	size_t N=mVertices.size();
	vector<TPoint3D> nVerts;
	nVerts.resize(N);
	CPose3D pose=this->m_pose;
	for (size_t i=0;i<N;i++) pose.composePoint(mVertices[i],nVerts[i]);
	transform(mFaces.begin(),mFaces.end(),vec.begin(),FCreatePolygonFromFace<TPolygon3D>(nVerts));
}

void CPolyhedron::makeConvexPolygons()	{
	vector<TPolygon3D> polys,polysTMP,polys2;
	getSetOfPolygons(polys);
	polys2.reserve(polys.size());
	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());
	else polys2.push_back(*it);
	mVertices.clear();
	mEdges.clear();
	mFaces.clear();
	getVerticesAndFaces(polys2,mVertices,mFaces);
	for (vector<TPolyhedronFace>::iterator it=mFaces.begin();it!=mFaces.end();++it)	{
		if (!setNormal(*it,false)) throw std::logic_error("Bad face specification");
		addEdges(*it);
	}
}

void CPolyhedron::getCenter(TPoint3D &center) const	{
	size_t N=mVertices.size();
	if (N==0) throw new std::logic_error("There are no vertices");
	center.x=center.y=center.z=0;
	for (vector<TPoint3D>::const_iterator it=mVertices.begin();it!=mVertices.end();++it)	{
		center.x+=it->x;
		center.y+=it->y;
		center.z+=it->z;
	}
	center.x/=N;
	center.y/=N;
	center.z/=N;
}

CPolyhedronPtr CPolyhedron::CreateRandomPolyhedron(double radius)	{
	switch (mrpt::random::randomGenerator.drawUniform32bit()%34)	{
		case 0:return CreateTetrahedron(radius);
		case 1:return CreateHexahedron(radius);
		case 2:return CreateOctahedron(radius);
		case 3:return CreateDodecahedron(radius);
		case 4:return CreateIcosahedron(radius);
		case 5:return CreateTruncatedTetrahedron(radius);
		case 6:return CreateTruncatedHexahedron(radius);
		case 7:return CreateTruncatedOctahedron(radius);
		case 8:return CreateTruncatedDodecahedron(radius);
		case 9:return CreateTruncatedIcosahedron(radius);
		case 10:return CreateCuboctahedron(radius);
		case 11:return CreateRhombicuboctahedron(radius);
		case 12:return CreateIcosidodecahedron(radius);
		case 13:return CreateRhombicosidodecahedron(radius);
		case 14:return CreateTriakisTetrahedron(radius);
		case 15:return CreateTriakisOctahedron(radius);
		case 16:return CreateTetrakisHexahedron(radius);
		case 17:return CreateTriakisIcosahedron(radius);
		case 18:return CreatePentakisDodecahedron(radius);
		case 19:return CreateRhombicDodecahedron(radius);
		case 20:return CreateDeltoidalIcositetrahedron(radius);
		case 21:return CreateRhombicTriacontahedron(radius);
		case 22:return CreateDeltoidalHexecontahedron(radius);
		case 23:return CreateArchimedeanRegularPrism((mrpt::random::randomGenerator.drawUniform32bit()%10)+3,radius);
		case 24:return CreateArchimedeanRegularAntiprism((mrpt::random::randomGenerator.drawUniform32bit()%10)+3,radius);
		case 25:return CreateJohnsonSolidWithConstantBase(((mrpt::random::randomGenerator.drawUniform32bit()%4)<<1)+4,radius,"C+");
		case 26:return CreateJohnsonSolidWithConstantBase(((mrpt::random::randomGenerator.drawUniform32bit()%4)<<1)+4,radius,"C-C+");
		case 27:return CreateJohnsonSolidWithConstantBase(((mrpt::random::randomGenerator.drawUniform32bit()%4)<<1)+4,radius,"C-PRC+");
		case 28:return CreateJohnsonSolidWithConstantBase(((mrpt::random::randomGenerator.drawUniform32bit()%4)<<1)+4,radius,"C-AC+");
		case 29:return CreateJohnsonSolidWithConstantBase(10,radius,"R+");
		case 30:return CreateJohnsonSolidWithConstantBase(10,radius,"R-PRR+");
		case 31:return CreateJohnsonSolidWithConstantBase(10,radius,"R-AR+");
		case 32:return CreateCatalanTrapezohedron((mrpt::random::randomGenerator.drawUniform32bit()%5)+3,radius);
		case 33:return CreateCatalanDoublePyramid((mrpt::random::randomGenerator.drawUniform32bit()%5)+3,radius);
		default:return CreateEmpty();
	}
}

CPolyhedronPtr CPolyhedron::getDual() const	{
	//This methods builds the dual of a given polyhedron, which is assumed to be centered in (0,0,0), using polar reciprocation.
	//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.
	//The new vertices can, then, be calculated as the corresponding intersections between three or more planes.
	size_t NV=mFaces.size();
	size_t NE=mEdges.size();
	size_t NF=mVertices.size();
	vector<TPlane> planes(NF);
	for (size_t i=0;i<NF;i++)	{
		const TPoint3D &p=mVertices[i];
		TPlane &pl=planes[i];
		pl.coefs[0]=p.x;
		pl.coefs[1]=p.y;
		pl.coefs[2]=p.z;
		pl.coefs[3]=-square(p.x)-square(p.y)-square(p.z);
	}
	CMatrixTemplate<bool> incidence(NV,NF);
	vector<TPoint3D> vertices(NV);
	for (size_t i=0;i<NV;i++)	{
		for (size_t j=0;j<NF;j++) incidence(i,j)=false;
		vector<const TPlane *> fPls;
		fPls.reserve(mFaces[i].vertices.size());
		for (vector<uint32_t>::const_iterator it=mFaces[i].vertices.begin();it!=mFaces[i].vertices.end();++it)	{
			incidence(i,*it)=true;
			fPls.push_back(&planes[*it]);
		}
		if (!getPlanesIntersection(fPls,vertices[i])) throw std::logic_error("Dual polyhedron cannot be found");
	}
	vector<TPolyhedronFace> faces(NF);
	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);
	//The following code ensures that the faces' vertex list is in the adequate order.
	CMatrixTemplate<bool> arrayEF(NE,NV);
	for (size_t i=0;i<NE;i++) for (size_t j=0;j<NV;j++) arrayEF(i,j)=faceContainsEdge(mFaces[j],mEdges[i]);
	for (vector<TPolyhedronFace>::iterator it=faces.begin();it!=faces.end();++it)	{
		vector<uint32_t> &face=it->vertices;
		if (face.size()<=3) continue;
		size_t index=0;
		while (index<face.size()-1)	{
			bool err=true;
			while (err)	{
				for (size_t i=0;i<NE;i++) if (arrayEF(i,face[index])&&arrayEF(i,face[index+1]))	{
					err=false;
					break;
				}
				if (err)	{
					size_t val=face[index+1];
					face.erase(face.begin()+index+1);
					face.push_back(val);
				}
			}
			index++;
		}
	}
	return Create(vertices,faces);
}

CPolyhedronPtr CPolyhedron::truncate(double factor) const	{
	if (factor<0) return CreateEmpty();
	if (factor==0) return CreateNoCheck(mVertices,mFaces);
	else if (factor<1)	{
		size_t NE=mEdges.size();
		size_t NV=mVertices.size();
		size_t NF=mFaces.size();
		vector<TPoint3D> vertices(NE<<1);
		vector<TPolyhedronFace> faces(NV+NF);
		for (size_t i=0;i<NE;i++)	{
			const TPoint3D &p1=mVertices[mEdges[i].v1];
			const TPoint3D &p2=mVertices[mEdges[i].v2];
			TPoint3D &v1=vertices[i+i];
			TPoint3D &v2=vertices[i+i+1];
			for (size_t j=0;j<3;j++)	{
				double d=(p2[j]-p1[j])*factor/2;
				v1[j]=p1[j]+d;
				v2[j]=p2[j]-d;
			}
			faces[mEdges[i].v1].vertices.push_back(i+i);
			faces[mEdges[i].v2].vertices.push_back(i+i+1);
		}
		for (size_t i=0;i<NV;i++)	{
			vector<uint32_t> &f=faces[i].vertices;
			size_t sf=f.size();
			if (sf==3) continue;
			for (size_t j=1;j<sf-1;j++)	{
				const TPolyhedronEdge &e1=mEdges[f[j-1]/2];
				for (;;)	{
					const TPolyhedronEdge &e2=mEdges[f[j]/2];
					if (!((e1.v1==i||e1.v2==i)&&(e2.v1==i||e2.v2==i))) THROW_EXCEPTION("En algo te has equivocado, chaval.");
					if (searchForFace(mFaces,i,(e1.v1==i)?e1.v2:e1.v1,(e2.v1==i)?e2.v2:e2.v1)) break;
					uint32_t tmpV=f[j];
					f.erase(f.begin()+j);
					f.push_back(tmpV);
				}
			}
		}
		for (size_t i=0;i<NF;i++)	{
			vector<uint32_t> &f=faces[i+NV].vertices;
			const vector<uint32_t> &cf=mFaces[i].vertices;
			size_t hmV=cf.size();
			f.reserve(hmV<<1);
			for (size_t j=0;j<hmV;j++)	{
				size_t where;
				if (searchForEdge(mEdges,cf[j],cf[(j+1)%hmV],where))	{
					f.push_back(where<<1);
					f.push_back((where<<1)+1);
				}	else	{
					f.push_back((where<<1)+1);
					f.push_back(where<<1);
				}
			}
		}
		return Create(vertices,faces);
	}	else if (factor==1)	{
		size_t NE=mEdges.size();
		size_t NV=mVertices.size();
		size_t NF=mFaces.size();
		vector<TPoint3D> vertices(NE);
		vector<TPolyhedronFace> faces(NV+NF);
		for (size_t i=0;i<NE;i++)	{
			const TPoint3D &p1=mVertices[mEdges[i].v1];
			const TPoint3D &p2=mVertices[mEdges[i].v2];
			TPoint3D &dst=vertices[i];
			for (size_t j=0;j<3;j++) dst[j]=(p1[j]+p2[j])/2;
			faces[mEdges[i].v1].vertices.push_back(i);
			faces[mEdges[i].v2].vertices.push_back(i);
		}
		for (size_t i=0;i<NV;i++)	{
			vector<uint32_t> &f=faces[i].vertices;
			size_t sf=f.size();
			if (sf==3) continue;
			for (size_t j=1;j<sf-1;j++)	{
				const TPolyhedronEdge &e1=mEdges[f[j-1]];
				for (;;)	{
					const TPolyhedronEdge &e2=mEdges[f[j-1]];
					if (!((e1.v1==i||e1.v2==i)&&(e2.v1==1||e2.v2==i))) THROW_EXCEPTION("En algo te has equivocado, chaval.");
					if (searchForFace(mFaces,i,(e1.v1==i)?e1.v2:e1.v1,(e2.v1==i)?e2.v2:e2.v1)) break;
					uint32_t tmpV=f[j];
					f.erase(f.begin()+j);
					f.push_back(tmpV);
				}
			}
		}
		for (size_t i=0;i<NF;i++)	{
			vector<uint32_t> &f=faces[i+NV].vertices;
			const vector<uint32_t> &cf=mFaces[i].vertices;
			size_t hmV=cf.size();
			f.reserve(hmV);
			for (size_t j=0;j<hmV;j++)	{
				size_t where;
				searchForEdge(mEdges,cf[j],cf[(j+1)%hmV],where);
				f.push_back(where);
			}
		}
		return Create(vertices,faces);
	}	else return CreateEmpty();
}

CPolyhedronPtr CPolyhedron::cantellate(double factor) const	{
	if (factor<0) return CreateEmpty();
	else if (factor==0) return CreateNoCheck(mVertices,mFaces);
	size_t NV=mVertices.size();
	size_t NE=mEdges.size();
	size_t NF=mFaces.size();
	vector<TPolygon3D> origFaces(NF);
	getSetOfPolygons(origFaces);
	TPoint3D cnt;
	getCenter(cnt);
	vector<TPoint3D> polyCenters(NF);
	vector<TPoint3D> polyNewCenters(NF);
	size_t NNV=0;
	for (size_t i=0;i<NF;i++)	{
		origFaces[i].getCenter(polyCenters[i]);
		polyCenters[i]-=cnt;
		polyNewCenters[i]=polyCenters[i];
		polyNewCenters[i]*=(1+factor);
		polyNewCenters[i]+=cnt;
		NNV+=origFaces[i].size();
	}
	vector<TPoint3D> vertices(NNV);
	vector<TPolyhedronFace> faces(NF+NV+NE);
	size_t ind=0;
	for (size_t i=0;i<NF;i++)	{
		const TPoint3D &oC=polyCenters[i];
		const TPoint3D &nC=polyNewCenters[i];
		const TPolygon3D &oP=origFaces[i];
		vector<uint32_t> &f=faces[i].vertices;
		size_t oPS=oP.size();
		for (size_t j=0;j<oPS;j++)	{
			vertices[j+ind]=nC+(oP[j]-oC);
			f.push_back(j+ind);
			size_t curr=mFaces[i].vertices[j];
			faces[NF+curr].vertices.push_back(j+ind);
			size_t edge;
			searchForEdge(mEdges,curr,mFaces[i].vertices[(j+oPS-1)%oPS],edge);
			faces[NF+NV+edge].vertices.push_back(j+ind);
			searchForEdge(mEdges,curr,mFaces[i].vertices[(j+1)%oPS],edge);
			faces[NF+NV+edge].vertices.push_back(j+ind);
		}
		ind+=oPS;
	}
	vector<TPolyhedronFace>::const_iterator begin=faces.begin(),edgeBegin=faces.begin()+NF+NV,end=faces.end();
	for (vector<TPolyhedronFace>::iterator it=faces.begin()+NF;it!=faces.begin()+NF+NV;++it)	{
		vector<uint32_t> &f=it->vertices;
		if (f.size()==3) continue;
		for (size_t i=1;i<f.size()-1;i++) for(;;) if (searchForEdge(edgeBegin,end,f[i-1],f[i])) break;
		else	{
			uint32_t tmp=f[i];
			f.erase(f.begin()+i);
			f.push_back(tmp);
		}
	}
	for (vector<TPolyhedronFace>::iterator it=faces.begin()+NF+NV;it!=faces.end();++it)	{
		vector<uint32_t> &f=it->vertices;	//Will always have exactly 4 vertices
		for (size_t i=1;i<3;i++) for (;;) if (searchForEdge(begin,edgeBegin,f[i-1],f[i])) break;
		else	{
			uint32_t tmp=f[i];
			f.erase(f.begin()+i);
			f.push_back(tmp);
		}
	}
	return Create(vertices,faces);
}

CPolyhedronPtr CPolyhedron::augment(double height) const	{
	size_t NV=mVertices.size();
	size_t NF=mFaces.size();
	vector<TPoint3D> vertices(NV+NF);
	std::copy(mVertices.begin(),mVertices.end(),vertices.begin());
	size_t tnf=0;
	for (vector<TPolyhedronFace>::const_iterator it=mFaces.begin();it!=mFaces.end();++it) tnf+=it->vertices.size();
	vector<TPolyhedronFace> faces(tnf);
	TPolygon3D tmp;
	TPlane pTmp;
	TPoint3D cTmp;
	size_t iF=0;
	TPoint3D phCenter;
	getCenter(phCenter);
	TPolyhedronFace fTmp;
	fTmp.vertices.resize(3);
	for (size_t i=0;i<NF;i++)	{
		TPoint3D &vertex=vertices[NV+i];
		const vector<uint32_t> &face=mFaces[i].vertices;
		size_t N=face.size();
		tmp.resize(N);
		for (size_t j=0;j<N;j++) tmp[j]=mVertices[face[j]];
		tmp.getBestFittingPlane(pTmp);
		pTmp.unitarize();
		tmp.getCenter(cTmp);
		if (pTmp.evaluatePoint(phCenter)>0)	for (size_t j=0;j<3;j++) vertex[j]=cTmp[j]-height*pTmp.coefs[j];
		else for (size_t j=0;j<3;j++) vertex[j]=cTmp[j]+height*pTmp.coefs[j];
		fTmp.vertices[0]=NV+i;
		for (size_t j=0;j<N;j++)	{
			fTmp.vertices[1]=face[j];
			fTmp.vertices[2]=face[(j+1)%N];
			faces[iF+j]=fTmp;
		}
		iF+=N;
	}
	return CreateNoCheck(vertices,faces);
}

CPolyhedronPtr CPolyhedron::augment(double height,size_t numVertices) const	{
	size_t NV=mVertices.size();
	size_t NF=mFaces.size();
	size_t tnf=0;
	size_t tnv=NV;
	for (vector<TPolyhedronFace>::const_iterator it=mFaces.begin();it!=mFaces.end();++it) if (it->vertices.size()==numVertices)	{
		tnf+=numVertices;
		tnv++;
	}	else tnf++;
	if (tnv==NV) return CreateNoCheck(mVertices,mFaces);
	vector<TPoint3D> vertices(tnv);
	std::copy(mVertices.begin(),mVertices.end(),vertices.begin());
	vector<TPolyhedronFace> faces(tnf);
	TPolygon3D tmp(numVertices);
	TPlane pTmp;
	TPoint3D cTmp;
	size_t iF=0;
	size_t iV=NV;
	TPoint3D phCenter;
	getCenter(phCenter);
	TPolyhedronFace fTmp;
	fTmp.vertices.resize(3);
	for (size_t i=0;i<NF;i++)	{
		const vector<uint32_t> &face=mFaces[i].vertices;
		size_t N=face.size();
		if (N!=numVertices)	{
			faces[iF].vertices=face;
			iF++;
			continue;
		}
		TPoint3D &vertex=vertices[iV];
		for (size_t j=0;j<numVertices;j++) tmp[j]=mVertices[face[j]];
		tmp.getBestFittingPlane(pTmp);
		pTmp.unitarize();
		tmp.getCenter(cTmp);
		if (pTmp.evaluatePoint(phCenter)>0) for (size_t j=0;j<3;j++) vertex[j]=cTmp[j]-height*pTmp.coefs[j];
		else for (size_t j=0;j<3;j++) vertex[j]=cTmp[j]+height*pTmp.coefs[j];
		fTmp.vertices[0]=iV;
		for (size_t j=0;j<N;j++)	{
			fTmp.vertices[1]=face[j];
			fTmp.vertices[2]=face[(j+1)%N];
			faces[iF+j]=fTmp;
		}
		iF+=N;
		iV++;
	}
	return CreateNoCheck(vertices,faces);
}

CPolyhedronPtr CPolyhedron::augment(bool direction) const	{
	size_t NV=mVertices.size();
	size_t NF=mFaces.size();
	vector<TPoint3D> vertices(NV+NF);
	std::copy(mVertices.begin(),mVertices.end(),vertices.begin());
	size_t tnf=0;
	for (vector<TPolyhedronFace>::const_iterator it=mFaces.begin();it!=mFaces.end();++it) tnf+=it->vertices.size();
	vector<TPolyhedronFace> faces(tnf);
	TPolygon3D tmp;
	TPlane pTmp;
	TPoint3D cTmp;
	size_t iF=0;
	TPoint3D phCenter;
	getCenter(phCenter);
	TPolyhedronFace fTmp;
	fTmp.vertices.resize(3);
	for (size_t i=0;i<NF;i++)	{
		TPoint3D &vertex=vertices[NV+i];
		const vector<uint32_t> &face=mFaces[i].vertices;
		size_t N=face.size();
		tmp.resize(N);
		for (size_t j=0;j<N;j++) tmp[j]=mVertices[face[j]];
		tmp.getCenter(cTmp);
		double height=getHeight(tmp,cTmp);	//throws std::logic_error
		tmp.getBestFittingPlane(pTmp);
		pTmp.unitarize();
		if ((pTmp.evaluatePoint(phCenter)<0)==direction)	for (size_t j=0;j<3;j++) vertex[j]=cTmp[j]-height*pTmp.coefs[j];
		else for (size_t j=0;j<3;j++) vertex[j]=cTmp[j]+height*pTmp.coefs[j];
		fTmp.vertices[0]=NV+i;
		for (size_t j=0;j<N;j++)	{
			fTmp.vertices[1]=face[j];
			fTmp.vertices[2]=face[(j+1)%N];
			faces[iF+j]=fTmp;
		}
		iF+=N;
	}
	return CreateNoCheck(vertices,faces);

}

CPolyhedronPtr CPolyhedron::augment(size_t numVertices,bool direction) const	{
	size_t NV=mVertices.size();
	size_t NF=mFaces.size();
	size_t tnf=0;
	size_t tnv=NV;
	for (vector<TPolyhedronFace>::const_iterator it=mFaces.begin();it!=mFaces.end();++it) if (it->vertices.size()==numVertices)	{
		tnf+=numVertices;
		tnv++;
	}	else tnf++;
	if (tnv==NV) return CreateNoCheck(mVertices,mFaces);
	vector<TPoint3D> vertices(tnv);
	std::copy(mVertices.begin(),mVertices.end(),vertices.begin());
	vector<TPolyhedronFace> faces(tnf);
	TPolygon3D tmp(numVertices);
	TPlane pTmp;
	TPoint3D cTmp;
	size_t iF=0;
	size_t iV=NV;
	TPoint3D phCenter;
	getCenter(phCenter);
	TPolyhedronFace fTmp;
	fTmp.vertices.resize(3);
	for (size_t i=0;i<NF;i++)	{
		const vector<uint32_t> &face=mFaces[i].vertices;
		size_t N=face.size();
		if (N!=numVertices)	{
			faces[iF].vertices=face;
			iF++;
			continue;
		}
		TPoint3D &vertex=vertices[iV];
		for (size_t j=0;j<numVertices;j++) tmp[j]=mVertices[face[j]];
		tmp.getBestFittingPlane(pTmp);
		pTmp.unitarize();
		tmp.getCenter(cTmp);
		double height=getHeight(tmp,cTmp);	//throws std::logic_error
		if ((pTmp.evaluatePoint(phCenter)<0)==direction) for (size_t j=0;j<3;j++) vertex[j]=cTmp[j]-height*pTmp.coefs[j];
		else for (size_t j=0;j<3;j++) vertex[j]=cTmp[j]+height*pTmp.coefs[j];
		fTmp.vertices[0]=iV;
		for (size_t j=0;j<N;j++)	{
			fTmp.vertices[1]=face[j];
			fTmp.vertices[2]=face[(j+1)%N];
			faces[iF+j]=fTmp;
		}
		iF+=N;
		iV++;
	}
	return CreateNoCheck(vertices,faces);
}

CPolyhedronPtr CPolyhedron::rotate(double angle) const	{
	vector<TPoint3D> vertices(mVertices);
	double c=cos(angle),s=sin(angle);
	for (vector<TPoint3D>::iterator it=vertices.begin();it!=vertices.end();++it)	{
		double A=it->x;
		double B=it->y;
		it->x=A*c-B*s;
		it->y=B*c+A*s;
	}
	return CreateNoCheck(vertices,mFaces);
}

CPolyhedronPtr CPolyhedron::scale(double factor) const	{
	vector<TPoint3D> vertices(mVertices);
	if (factor<=0) throw std::logic_error("Factor must be a strictly positive number");
	for (vector<TPoint3D>::iterator it=vertices.begin();it!=vertices.end();++it)	{
		it->x*=factor;
		it->y*=factor;
	}
	return CreateNoCheck(vertices,mFaces);
}

vector<TPoint2D> CPolyhedron::generateBase(uint32_t numBaseEdges,double baseRadius)	{
	vector<TPoint2D> base(numBaseEdges);
	for (size_t i=0;i<numBaseEdges;i++)	{
		double ang=2*M_PI*i/numBaseEdges;
		base[i].x=baseRadius*cos(ang);
		base[i].y=baseRadius*sin(ang);
	}
	return base;
}

vector<TPoint2D> CPolyhedron::generateShiftedBase(uint32_t numBaseEdges,double baseRadius)	{
	vector<TPoint2D> base(numBaseEdges);
	double shift=M_PI/numBaseEdges;
	for (size_t i=0;i<numBaseEdges;i++)	{
		double ang=shift+2*M_PI*i/numBaseEdges;
		base[i].x=baseRadius*cos(ang);
		base[i].y=baseRadius*sin(ang);
	}
	return base;
}

void CPolyhedron::generateBase(uint32_t numBaseEdges,double baseRadius,double height,vector<TPoint3D> &vec)	{
	for (size_t i=0;i<numBaseEdges;i++)	{
		double ang=2*M_PI*i/numBaseEdges;
		vec.push_back(TPoint3D(baseRadius*cos(ang),baseRadius*sin(ang),height));
	}
}

void CPolyhedron::generateShiftedBase(uint32_t numBaseEdges,double baseRadius,double height,double shift,vector<TPoint3D> &vec)	{
	for (size_t i=0;i<numBaseEdges;i++)	{
		double ang=2*M_PI*i/numBaseEdges+shift;
		vec.push_back(TPoint3D(baseRadius*cos(ang),baseRadius*sin(ang),height));
	}
}

void CPolyhedron::updatePolygons() const	{
	tempPolygons.resize(mFaces.size());
	transform(mFaces.begin(),mFaces.end(),tempPolygons.begin(),FCreatePolygonFromFace<TPolygonWithPlane>(mVertices));
	polygonsUpToDate=true;
}

bool CPolyhedron::setNormal(TPolyhedronFace &f,bool doCheck)	{
	size_t N=doCheck?f.vertices.size():3;
	TPolygon3D poly(N);
	for (size_t i=0;i<N;i++) poly[i]=mVertices[f.vertices[i]];
	TPlane tmp;
	if (!poly.getPlane(tmp)) return false;
	tmp.getNormalVector(f.normal);
	TPoint3D c;
	getCenter(c);
	if (tmp.evaluatePoint(c)>0) for (size_t i=0;i<3;i++) f.normal[i]=-f.normal[i];
	return true;
}

void CPolyhedron::addEdges(const TPolyhedronFace &f)	{
	TPolyhedronEdge e;
	vector<uint32_t>::const_iterator it=f.vertices.begin();
	e.v1=*it;
	++it;
	while (it!=f.vertices.end())	{
		e.v2=*it;
		if (find(mEdges.begin(),mEdges.end(),e)==mEdges.end()) mEdges.push_back(e);
		e.v1=e.v2;
		++it;
	}
	e.v2=*(f.vertices.begin());
	if (find(mEdges.begin(),mEdges.end(),e)==mEdges.end()) mEdges.push_back(e);
}

bool CPolyhedron::checkConsistence(const vector<TPoint3D> &vertices,const vector<TPolyhedronFace> &faces)	{
	size_t N=vertices.size();
	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;
	for (vector<TPolyhedronFace>::const_iterator it=faces.begin();it!=faces.end();++it)	{
		const vector<uint32_t> &e=it->vertices;
		for (vector<uint32_t>::const_iterator it2=e.begin();it2!=e.end();++it2) if (*it2>=N) return false;
	}
	return true;
}

size_t CPolyhedron::edgesInVertex(size_t vertex) const	{
	size_t res=0;
	for (vector<TPolyhedronEdge>::const_iterator it=mEdges.begin();it!=mEdges.end();++it) if (it->v1==vertex||it->v2==vertex) res++;
	return res;
}

size_t CPolyhedron::facesInVertex(size_t vertex) const	{
	size_t res=0;
	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++;
	return res;
}

CStream &mrpt::opengl::operator>>(mrpt::utils::CStream& in,CPolyhedron::TPolyhedronEdge &o)	{
	in>>o.v1>>o.v2;
	return in;
}

CStream &mrpt::opengl::operator<<(mrpt::utils::CStream &out,const CPolyhedron::TPolyhedronEdge &o)	{
	out<<o.v1<<o.v2;
	return out;
}

CStream &mrpt::opengl::operator>>(mrpt::utils::CStream& in,CPolyhedron::TPolyhedronFace &o)	{
	in>>o.vertices>>o.normal[0]>>o.normal[1]>>o.normal[2];
	return in;
}

CStream &mrpt::opengl::operator<<(mrpt::utils::CStream& out,const CPolyhedron::TPolyhedronFace &o)	{
	out<<o.vertices<<o.normal[0]<<o.normal[1]<<o.normal[2];
	return out;
}

void CPolyhedron::writeToStream(mrpt::utils::CStream &out,int *version) const	{
	if (version) *version=0;
	else	{
		writeToStreamRender(out);
		//version 0
		out<<mVertices<<mFaces<<mWireframe<<mLineWidth;
	}
}

void CPolyhedron::readFromStream(mrpt::utils::CStream &in,int version)	{
	switch (version)	{
		case 0:
			readFromStreamRender(in);
			in>>mVertices>>mFaces>>mWireframe>>mLineWidth;
			if (!checkConsistence(mVertices,mFaces)) throw std::logic_error("Inconsistent data read from stream");
			for (vector<TPolyhedronFace>::iterator it=mFaces.begin();it!=mFaces.end();++it)	{
				if (!setNormal(*it)) throw std::logic_error("Bad face specification");
				addEdges(*it);
			}
			break;
		default:
			MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version)
	};
	CRenderizableDisplayList::notifyChange();
}


void CPolyhedron::getBoundingBox(mrpt::math::TPoint3D &bb_min, mrpt::math::TPoint3D &bb_max) const
{
	bb_min.x = 0;
	bb_min.y = 0;
	bb_min.z = 0;

	bb_max.x = 0;
	bb_max.y = 0;
	bb_max.z = 0;

	// Convert to coordinates of my parent:
	m_pose.composePoint(bb_min, bb_min);
	m_pose.composePoint(bb_max, bb_max);
}


/*CPolyhedronPtr CPolyhedron::CreateCuboctahedron(double radius)	{
	if (radius==0) return CreateEmpty();
	vector<TPoint3D> verts;
	vector<TPolyhedronFace> faces;
	double d=radius/sqrt(2.0);
	verts.push_back(TPoint3D(d,0,d));
	verts.push_back(TPoint3D(0,d,d));
	verts.push_back(TPoint3D(0,-d,d));
	verts.push_back(TPoint3D(-d,0,d));
	verts.push_back(TPoint3D(d,d,0));
	verts.push_back(TPoint3D(d,-d,0));
	verts.push_back(TPoint3D(-d,d,0));
	verts.push_back(TPoint3D(-d,-d,0));
	verts.push_back(TPoint3D(d,0,-d));
	verts.push_back(TPoint3D(0,d,-d));
	verts.push_back(TPoint3D(0,-d,-d));
	verts.push_back(TPoint3D(-d,0,-d));
	TPolyhedronFace f;
	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};
	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};
	for (uint32_t *p=reinterpret_cast<uint32_t *>(&faces3);p<24+reinterpret_cast<uint32_t *>(&faces3);p+=3)	{
		f.vertices.insert(f.vertices.begin(),p,p+3);
		faces.push_back(f);
		f.vertices.clear();
	}
	for (uint32_t *p=reinterpret_cast<uint32_t *>(&faces4);p<24+reinterpret_cast<uint32_t *>(&faces4);p+=4)	{
		f.vertices.insert(f.vertices.begin(),p,p+4);
		faces.push_back(f);
		f.vertices.clear();
	}
	return CreateNoCheck(verts,faces);
}*/

CPolyhedronPtr CPolyhedron::Create(const vector<TPoint3D> &vertices,const vector<vector<uint32_t> > &faces)	{
	vector<TPolyhedronFace> aux;
	for (vector<vector<uint32_t> >::const_iterator it=faces.begin();it!=faces.end();++it)	{
		TPolyhedronFace f;
		f.vertices=*it;
		aux.push_back(f);
	}
	return Create(vertices,aux);
}
CPolyhedronPtr CPolyhedron::Create(const vector<TPoint3D> &vertices,const vector<TPolyhedronFace> &faces)	{
	return CPolyhedronPtr(new CPolyhedron(vertices,faces,true));
}
CPolyhedronPtr CPolyhedron::CreateTetrahedron(double radius)	{
	CPolyhedronPtr tetra=CreateJohnsonSolidWithConstantBase(3,radius*sqrt(8.0)/3.0,"P+");
	for (vector<TPoint3D>::iterator it=tetra->mVertices.begin();it!=tetra->mVertices.end();++it) it->z-=radius/3;
	return tetra;
}
CPolyhedronPtr CPolyhedron::CreateHexahedron(double radius)	{
	if (radius==0.0) return CreateEmpty();
	double r=radius/sqrt(3.0);
	return CreateCubicPrism(-r,r,-r,r,-r,r);
}
CPolyhedronPtr CPolyhedron::CreateOctahedron(double radius)	{
	return CreateJohnsonSolidWithConstantBase(4,radius,"P-P+");
}
CPolyhedronPtr CPolyhedron::CreateDodecahedron(double radius)	{
	return CreateIcosahedron(radius/sqrt(15-6*sqrt(5.0)))->getDual();
}
CPolyhedronPtr CPolyhedron::CreateIcosahedron(double radius)	{
	double ang=M_PI/5;
	double s2=4*square(sin(ang));
	double prop=sqrt(s2-1)+sqrt(s2-2+2*cos(ang))/2;
	return CreateJohnsonSolidWithConstantBase(5,radius/prop,"P-AP+",1);
}
CPolyhedronPtr CPolyhedron::CreateTruncatedTetrahedron(double radius)	{
	return CreateTetrahedron(radius*sqrt(27.0/11.0))->truncate(2.0/3.0);
}
CPolyhedronPtr CPolyhedron::CreateCuboctahedron(double radius)	{
	return CreateHexahedron(radius*sqrt(1.5))->truncate(1.0);
}
CPolyhedronPtr CPolyhedron::CreateTruncatedHexahedron(double radius)	{
	return CreateHexahedron(radius*sqrt(3.0/(5-sqrt(8.0))))->truncate(2-sqrt(2.0));
}
CPolyhedronPtr CPolyhedron::CreateTruncatedOctahedron(double radius)	{
	return CreateOctahedron(radius*3/sqrt(5.0))->truncate(2.0/3.0);
}
CPolyhedronPtr CPolyhedron::CreateRhombicuboctahedron(double radius,bool type)	{
	return CreateJohnsonSolidWithConstantBase(8,radius/sqrt(1+square(sin(M_PI/8))),type?"C-PRC+":"GC-PRC+",3);
}
CPolyhedronPtr CPolyhedron::CreateIcosidodecahedron(double radius,bool type)	{
	return CreateJohnsonSolidWithConstantBase(10,radius,type?"GR-R+":"R-R+",1);
}
CPolyhedronPtr CPolyhedron::CreateTruncatedDodecahedron(double radius)	{
	return CreateDodecahedron(radius*sqrt(45.0)/sqrt(27+6*sqrt(5.0)))->truncate(1-sqrt(0.2));
}
CPolyhedronPtr CPolyhedron::CreateTruncatedIcosahedron(double radius)	{
	return CreateIcosahedron(radius*sqrt(45.0)/sqrt(25+4*sqrt(5.0)))->truncate(2.0/3.0);
}
CPolyhedronPtr CPolyhedron::CreateRhombicosidodecahedron(double radius)	{
	return CreateIcosahedron(radius*sqrt(10.0/(35.0+9.0*sqrt(5.0))))->cantellate(1.5*(sqrt(5.0)-1));
}
CPolyhedronPtr CPolyhedron::CreatePentagonalRotunda(double radius)	{
	return CreateJohnsonSolidWithConstantBase(10,radius,"R+");
}
CPolyhedronPtr CPolyhedron::CreateTriakisTetrahedron(double radius)	{
	return CreateTruncatedTetrahedron(radius*3/sqrt(33.0))->getDual();
}
CPolyhedronPtr CPolyhedron::CreateRhombicDodecahedron(double radius)	{
	return CreateCuboctahedron(radius/sqrt(2.0))->getDual();
}
CPolyhedronPtr CPolyhedron::CreateTriakisOctahedron(double radius)	{
	return CreateTruncatedHexahedron(radius/sqrt((5-sqrt(8.0))))->getDual();
}
CPolyhedronPtr CPolyhedron::CreateTetrakisHexahedron(double radius)	{
	return CreateTruncatedOctahedron(radius*sqrt(0.6))->getDual();
}
CPolyhedronPtr CPolyhedron::CreateDeltoidalIcositetrahedron(double radius)	{
	return CreateRhombicuboctahedron(radius/sqrt(7-sqrt(32.0)),true)->getDual();
}
CPolyhedronPtr CPolyhedron::CreateRhombicTriacontahedron(double radius)	{
	return CreateIcosidodecahedron(radius*sqrt(2/(5-sqrt(5.0))),true)->getDual();
}
CPolyhedronPtr CPolyhedron::CreateTriakisIcosahedron(double radius)	{
	return CreateTruncatedDodecahedron(radius*sqrt(5/(25-8*sqrt(5.0))))->getDual();
}
CPolyhedronPtr CPolyhedron::CreatePentakisDodecahedron(double radius)	{
	return CreateTruncatedIcosahedron(radius*sqrt(3/(17-6*sqrt(5.0))))->getDual();
}
CPolyhedronPtr CPolyhedron::CreateDeltoidalHexecontahedron(double radius)	{
	return CreateRhombicosidodecahedron(radius*3.0/sqrt(15-2*sqrt(5.0)))->getDual();
}
CPolyhedronPtr CPolyhedron::CreateCubicPrism(const TPoint3D &p1,const TPoint3D &p2)	{
	return CreateCubicPrism(p1.x,p2.x,p1.y,p2.y,p1.z,p2.z);
}
CPolyhedronPtr CPolyhedron::CreateFrustum(const vector<TPoint2D> &baseVertices,double height,double ratio)	{
	return CreateTruncatedPyramid(baseVertices,height,ratio);
}
CPolyhedronPtr CPolyhedron::CreateCustomPrism(const vector<TPoint2D> &baseVertices,double height)	{
	return CreateTruncatedPyramid(baseVertices,height,1.0);
}
CPolyhedronPtr CPolyhedron::CreateRegularAntiprism(uint32_t numBaseEdges,double baseRadius,double height)	{
	return CreateCustomAntiprism(generateBase(numBaseEdges,baseRadius),generateShiftedBase(numBaseEdges,baseRadius),height);
}
CPolyhedronPtr CPolyhedron::CreateRegularPrism(uint32_t numBaseEdges,double baseRadius,double height)	{
	return CreateCustomPrism(generateBase(numBaseEdges,baseRadius),height);
}
CPolyhedronPtr CPolyhedron::CreateRegularPyramid(uint32_t numBaseEdges,double baseRadius,double height)	{
	return CreatePyramid(generateBase(numBaseEdges,baseRadius),height);
}
CPolyhedronPtr CPolyhedron::CreateRegularDoublePyramid(uint32_t numBaseEdges,double baseRadius,double height1,double height2)	{
	return CreateDoublePyramid(generateBase(numBaseEdges,baseRadius),height1,height2);
}
CPolyhedronPtr CPolyhedron::CreateArchimedeanRegularPrism(uint32_t numBaseEdges,double baseRadius)	{
	return CreateJohnsonSolidWithConstantBase(numBaseEdges,baseRadius,"PR");
}
CPolyhedronPtr CPolyhedron::CreateArchimedeanRegularAntiprism(uint32_t numBaseEdges,double baseRadius)	{
	return CreateJohnsonSolidWithConstantBase(numBaseEdges,baseRadius,"A");
}
CPolyhedronPtr CPolyhedron::CreateRegularTruncatedPyramid(uint32_t numBaseEdges,double baseRadius,double height,double ratio)	{
	return CreateTruncatedPyramid(generateBase(numBaseEdges,baseRadius),height,ratio);
}
CPolyhedronPtr CPolyhedron::CreateRegularFrustum(uint32_t numBaseEdges,double baseRadius,double height,double ratio)	{
	return CreateRegularTruncatedPyramid(numBaseEdges,baseRadius,height,ratio);
}
CPolyhedronPtr CPolyhedron::CreateRegularBifrustum(uint32_t numBaseEdges,double baseRadius,double height1,double ratio1,double height2,double ratio2)	{
	return CreateBifrustum(generateBase(numBaseEdges,baseRadius),height1,ratio1,height2,ratio2);
}
CPolyhedronPtr CPolyhedron::CreateCupola(uint32_t numBaseEdges,double edgeLength)	{
	return CreateJohnsonSolidWithConstantBase(numBaseEdges,edgeLength/(2*sin(M_PI/numBaseEdges)),"C+");
}
CPolyhedronPtr CPolyhedron::CreateCatalanTrapezohedron(uint32_t numBaseEdges,double height)	{
	return CreateArchimedeanRegularAntiprism(numBaseEdges,height)->getDual();
}
CPolyhedronPtr CPolyhedron::CreateCatalanDoublePyramid(uint32_t numBaseEdges,double height)	{
	return CreateArchimedeanRegularPrism(numBaseEdges,height)->getDual();
}

CPolyhedronPtr CPolyhedron::CreateNoCheck(const vector<TPoint3D> &vertices,const vector<TPolyhedronFace> &faces)	
{
	return CPolyhedronPtr(new CPolyhedron(vertices,faces,false));
}
CPolyhedronPtr CPolyhedron::CreateEmpty()	
{
	return CPolyhedronPtr(new CPolyhedron());
}
