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

#include "opengl-precomp.h"	 // Precompiled header
//
#include <mrpt/math/CMatrixDynamic.h>
#include <mrpt/math/CMatrixF.h>
#include <mrpt/math/TLine3D.h>
#include <mrpt/math/TObject3D.h>
#include <mrpt/math/geometry.h>
#include <mrpt/math/ops_containers.h>  // dotProduct()
#include <mrpt/opengl/CPolyhedron.h>
#include <mrpt/poses/CPose3D.h>
#include <mrpt/random.h>
#include <mrpt/serialization/CArchive.h>
#include <mrpt/serialization/stl_serialization.h>

using namespace mrpt;
using namespace mrpt::math;
using namespace mrpt::opengl;
using namespace mrpt::poses;
using namespace std;
using mrpt::serialization::CArchive;

IMPLEMENTS_SERIALIZABLE(CPolyhedron, CRenderizable, 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 (const auto& poly : polys)
	{
		size_t N = poly.size();
		if (N < 3) return false;
		CPolyhedron::TPolyhedronFace f;
		f.vertices.resize(N);
		for (size_t i = 0; i < N; i++)
		{
			auto it2 = find(vertices.begin(), vertices.end(), poly[i]);
			if (it2 == vertices.end())
			{
				f.vertices[i] = vertices.size();
				vertices.push_back(poly[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.emplace_back(
			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 (const auto& i : p1)
		verts.push_back(i);
	for (const auto& i : p2)
		verts.push_back(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 (unsigned int vertice : f.vertices)
		if (vertice == e.v1 || vertice == 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 (const auto& it : fs)
	{
		const vector<uint32_t>& f = it.vertices;
		size_t hmf = 0;
		for (unsigned int it2 : f)
		{
			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 (unsigned int it2 : f)
			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 (std::abs(mrpt::math::distance(p[i], c) - r) >=
			mrpt::math::getEpsilon())
			throw std::logic_error("There is a non-regular polygon.");
		else if (
			std::abs(mrpt::math::distance(p[i], p[(i + 1) % N]) - l) >=
			mrpt::math::getEpsilon())
			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 (unsigned int vertice : vertices)
	{
		p.x += vrts[vertice].x;
		p.y += vrts[vertice].y;
		p.z += vrts[vertice].z;
	}
	size_t N = vertices.size();
	p.x /= N;
	p.y /= N;
	p.z /= N;
}

CPolyhedron::CPolyhedron(const std::vector<math::TPolygon3D>& polys)
	: m_Edges(), m_Wireframe(false), polygonsUpToDate(false)
{
	std::vector<TPoint3D> vertices(0);
	std::vector<TPolyhedronFace> faces;
	if (!getVerticesAndFaces(polys, vertices, faces))
		throw std::logic_error("Can't create CPolygon");
	m_Vertices = std::move(vertices);
	m_Faces = std::move(faces);

	InitFromVertAndFaces(m_Vertices, m_Faces);
}

CPolyhedron::CPolyhedron(
	const vector<TPoint3D>& vertices, const vector<vector<uint32_t>>& faces)
{
	vector<TPolyhedronFace> aux;
	for (const auto& face : faces)
	{
		TPolyhedronFace f;
		f.vertices = face;
		aux.push_back(f);
	}
	InitFromVertAndFaces(vertices, aux);
}

CPolyhedron::Ptr 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.emplace_back(
			(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 (auto* 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);
}

CPolyhedron::Ptr CPolyhedron::CreatePyramid(
	const vector<TPoint2D>& baseVertices, double height)
{
	uint32_t n = baseVertices.size();
	if (baseVertices.size() < 3) throw std::logic_error("Not enough vertices");
	vector<TPoint3D> verts;
	vector<TPolyhedronFace> faces;
	verts.emplace_back(0, 0, height);
	for (auto baseVertice : baseVertices)
		verts.emplace_back(baseVertice.x, baseVertice.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);
}

CPolyhedron::Ptr CPolyhedron::CreateDoublePyramid(
	const vector<TPoint2D>& baseVertices, double height1, double height2)
{
	uint32_t N = baseVertices.size();
	if (N < 3) throw std::logic_error("Not enough vertices");
	vector<TPoint3D> verts;
	verts.reserve(N + 2);
	vector<TPolyhedronFace> faces;
	faces.reserve(N << 1);
	verts.emplace_back(0, 0, height1);
	for (auto baseVertice : baseVertices)
		verts.emplace_back(baseVertice.x, baseVertice.y, 0);
	verts.emplace_back(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);
}

CPolyhedron::Ptr 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);
}

CPolyhedron::Ptr 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);
}

CPolyhedron::Ptr 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);
}

CPolyhedron::Ptr 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);
}

CPolyhedron::Ptr 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);
}

CPolyhedron::Ptr 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 antiPrism_height = 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, m_height = 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 = antiPrism_height;
			shifts++;
		}
		else
			h = 0;
		basePositionInfo[i] = make_pair(m_height += h, shifts);
	}
	m_height /= 2;
	double semi = M_PI / numBaseEdges;
	// All the bases are generated and inserted into the vertices' vector.
	for (auto it = basePositionInfo.begin(); it != basePositionInfo.end(); ++it)
		generateShiftedBase(
			numBaseEdges, baseRadius, it->first - m_height, semi * it->second,
			verts);
	size_t initialBase = 0, endBase = 0;
	TPolyhedronFace face;
	face.vertices.reserve(numBaseEdges);
	// Each body is inserted.
	for (size_t p = 0; p < nParts; p++)
	{
		switch (parts[p])
		{
			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.emplace_back(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.emplace_back(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);
}

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

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

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

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();
	auto itP = tempPolygons.begin();
	vector<double> areas(m_Faces.size());
	getFacesArea(areas);
	auto itA = areas.begin();
	for (auto it = m_Faces.begin(); it != m_Faces.end(); ++it, ++itP, ++itA)
		res += std::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(m_Faces.size());
	size_t N = m_Vertices.size();
	vector<TPoint3D> nVerts;
	nVerts.resize(N);
	CPose3D pose = this->m_pose;
	for (size_t i = 0; i < N; i++)
		pose.composePoint(m_Vertices[i], nVerts[i]);
	transform(
		m_Faces.begin(), m_Faces.end(), vec.begin(),
		FCreatePolygonFromFace<TPolygon3D>(nVerts));
}

void CPolyhedron::makeConvexPolygons()
{
	vector<TPolygon3D> polys, polysTMP, polys2;
	getSetOfPolygons(polys);
	polys2.reserve(polys.size());
	for (auto 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);
	m_Vertices.clear();
	m_Edges.clear();
	m_Faces.clear();
	getVerticesAndFaces(polys2, m_Vertices, m_Faces);
	for (auto& mFace : m_Faces)
	{
		if (!setNormal(mFace, false))
			throw std::logic_error("Bad face specification");
		addEdges(mFace);
	}
}

void CPolyhedron::getCenter(TPoint3D& center) const
{
	size_t N = m_Vertices.size();
	if (N == 0) throw new std::logic_error("There are no vertices");
	center.x = center.y = center.z = 0;
	for (const auto& mVertice : m_Vertices)
	{
		center.x += mVertice.x;
		center.y += mVertice.y;
		center.z += mVertice.z;
	}
	center.x /= N;
	center.y /= N;
	center.z /= N;
}

CPolyhedron::Ptr CPolyhedron::CreateRandomPolyhedron(double radius)
{
	switch (mrpt::random::getRandomGenerator().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::getRandomGenerator().drawUniform32bit() % 10) +
					3,
				radius);
		case 24:
			return CreateArchimedeanRegularAntiprism(
				(mrpt::random::getRandomGenerator().drawUniform32bit() % 10) +
					3,
				radius);
		case 25:
			return CreateJohnsonSolidWithConstantBase(
				((mrpt::random::getRandomGenerator().drawUniform32bit() % 4)
				 << 1) +
					4,
				radius, "C+");
		case 26:
			return CreateJohnsonSolidWithConstantBase(
				((mrpt::random::getRandomGenerator().drawUniform32bit() % 4)
				 << 1) +
					4,
				radius, "C-C+");
		case 27:
			return CreateJohnsonSolidWithConstantBase(
				((mrpt::random::getRandomGenerator().drawUniform32bit() % 4)
				 << 1) +
					4,
				radius, "C-PRC+");
		case 28:
			return CreateJohnsonSolidWithConstantBase(
				((mrpt::random::getRandomGenerator().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::getRandomGenerator().drawUniform32bit() % 5) + 3,
				radius);
		case 33:
			return CreateCatalanDoublePyramid(
				(mrpt::random::getRandomGenerator().drawUniform32bit() % 5) + 3,
				radius);
		default: return CreateEmpty();
	}
}

CPolyhedron::Ptr 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 = m_Faces.size();
	size_t NE = m_Edges.size();
	size_t NF = m_Vertices.size();
	vector<TPlane> planes(NF);
	for (size_t i = 0; i < NF; i++)
	{
		const TPoint3D& p = m_Vertices[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);
	}
	CMatrixDynamic<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(m_Faces[i].vertices.size());
		for (auto it = m_Faces[i].vertices.begin();
			 it != m_Faces[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.
	CMatrixDynamic<bool> arrayEF(NE, NV);
	for (size_t i = 0; i < NE; i++)
		for (size_t j = 0; j < NV; j++)
			arrayEF(i, j) = faceContainsEdge(m_Faces[j], m_Edges[i]);
	for (auto& it : faces)
	{
		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 std::make_shared<CPolyhedron>(vertices, faces);
}

CPolyhedron::Ptr CPolyhedron::truncate(double factor) const
{
	if (factor < 0) return CreateEmpty();
	if (factor == 0) return CreateNoCheck(m_Vertices, m_Faces);
	else if (factor < 1)
	{
		size_t NE = m_Edges.size();
		size_t NV = m_Vertices.size();
		size_t NF = m_Faces.size();
		vector<TPoint3D> vertices(NE << 1);
		vector<TPolyhedronFace> faces(NV + NF);
		for (size_t i = 0; i < NE; i++)
		{
			const TPoint3D& p1 = m_Vertices[m_Edges[i].v1];
			const TPoint3D& p2 = m_Vertices[m_Edges[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[m_Edges[i].v1].vertices.push_back(i + i);
			faces[m_Edges[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 = m_Edges[f[j - 1] / 2];
				for (;;)
				{
					const TPolyhedronEdge& e2 = m_Edges[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(
							m_Faces, 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 = m_Faces[i].vertices;
			size_t hmV = cf.size();
			f.reserve(hmV << 1);
			for (size_t j = 0; j < hmV; j++)
			{
				size_t where;
				if (searchForEdge(m_Edges, 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 std::make_shared<CPolyhedron>(vertices, faces);
	}
	else if (factor == 1)
	{
		size_t NE = m_Edges.size();
		size_t NV = m_Vertices.size();
		size_t NF = m_Faces.size();
		vector<TPoint3D> vertices(NE);
		vector<TPolyhedronFace> faces(NV + NF);
		for (size_t i = 0; i < NE; i++)
		{
			const TPoint3D& p1 = m_Vertices[m_Edges[i].v1];
			const TPoint3D& p2 = m_Vertices[m_Edges[i].v2];
			TPoint3D& dst = vertices[i];
			for (size_t j = 0; j < 3; j++)
				dst[j] = (p1[j] + p2[j]) / 2;
			faces[m_Edges[i].v1].vertices.push_back(i);
			faces[m_Edges[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 = m_Edges[f[j - 1]];
				for (;;)
				{
					const TPolyhedronEdge& e2 = m_Edges[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(
							m_Faces, 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 = m_Faces[i].vertices;
			size_t hmV = cf.size();
			f.reserve(hmV);
			for (size_t j = 0; j < hmV; j++)
			{
				size_t where;
				searchForEdge(m_Edges, cf[j], cf[(j + 1) % hmV], where);
				f.push_back(where);
			}
		}
		return std::make_shared<CPolyhedron>(vertices, faces);
	}
	else
		return CreateEmpty();
}

CPolyhedron::Ptr CPolyhedron::cantellate(double factor) const
{
	if (factor < 0) return CreateEmpty();
	else if (factor == 0)
		return CreateNoCheck(m_Vertices, m_Faces);
	size_t NV = m_Vertices.size();
	size_t NE = m_Edges.size();
	size_t NF = m_Faces.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 = m_Faces[i].vertices[j];
			faces[NF + curr].vertices.push_back(j + ind);
			size_t edge;
			searchForEdge(
				m_Edges, curr, m_Faces[i].vertices[(j + oPS - 1) % oPS], edge);
			faces[NF + NV + edge].vertices.push_back(j + ind);
			searchForEdge(
				m_Edges, curr, m_Faces[i].vertices[(j + 1) % oPS], edge);
			faces[NF + NV + edge].vertices.push_back(j + ind);
		}
		ind += oPS;
	}
	auto begin = faces.begin(), edgeBegin = faces.begin() + NF + NV,
		 end = faces.end();
	for (auto 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 (auto 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 std::make_shared<CPolyhedron>(vertices, faces);
}

CPolyhedron::Ptr CPolyhedron::augment(double height) const
{
	size_t NV = m_Vertices.size();
	size_t NF = m_Faces.size();
	vector<TPoint3D> vertices(NV + NF);
	std::copy(m_Vertices.begin(), m_Vertices.end(), vertices.begin());
	size_t tnf = 0;
	for (const auto& mFace : m_Faces)
		tnf += mFace.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 = m_Faces[i].vertices;
		size_t N = face.size();
		tmp.resize(N);
		for (size_t j = 0; j < N; j++)
			tmp[j] = m_Vertices[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);
}

CPolyhedron::Ptr CPolyhedron::augment(double height, size_t numVertices) const
{
	size_t NV = m_Vertices.size();
	size_t NF = m_Faces.size();
	size_t tnf = 0;
	size_t tnv = NV;
	for (const auto& mFace : m_Faces)
		if (mFace.vertices.size() == numVertices)
		{
			tnf += numVertices;
			tnv++;
		}
		else
			tnf++;
	if (tnv == NV) return CreateNoCheck(m_Vertices, m_Faces);
	vector<TPoint3D> vertices(tnv);
	std::copy(m_Vertices.begin(), m_Vertices.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 = m_Faces[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] = m_Vertices[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);
}

CPolyhedron::Ptr CPolyhedron::augment(bool direction) const
{
	size_t NV = m_Vertices.size();
	size_t NF = m_Faces.size();
	vector<TPoint3D> vertices(NV + NF);
	std::copy(m_Vertices.begin(), m_Vertices.end(), vertices.begin());
	size_t tnf = 0;
	for (const auto& mFace : m_Faces)
		tnf += mFace.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 = m_Faces[i].vertices;
		size_t N = face.size();
		tmp.resize(N);
		for (size_t j = 0; j < N; j++)
			tmp[j] = m_Vertices[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);
}

CPolyhedron::Ptr CPolyhedron::augment(size_t numVertices, bool direction) const
{
	size_t NV = m_Vertices.size();
	size_t NF = m_Faces.size();
	size_t tnf = 0;
	size_t tnv = NV;
	for (const auto& mFace : m_Faces)
		if (mFace.vertices.size() == numVertices)
		{
			tnf += numVertices;
			tnv++;
		}
		else
			tnf++;
	if (tnv == NV) return CreateNoCheck(m_Vertices, m_Faces);
	vector<TPoint3D> vertices(tnv);
	std::copy(m_Vertices.begin(), m_Vertices.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 = m_Faces[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] = m_Vertices[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);
}

CPolyhedron::Ptr CPolyhedron::rotate(double angle) const
{
	vector<TPoint3D> vertices(m_Vertices);
	double c = cos(angle), s = sin(angle);
	for (auto& vertice : vertices)
	{
		double A = vertice.x;
		double B = vertice.y;
		vertice.x = A * c - B * s;
		vertice.y = B * c + A * s;
	}
	return CreateNoCheck(vertices, m_Faces);
}

CPolyhedron::Ptr CPolyhedron::scale(double factor) const
{
	vector<TPoint3D> vertices(m_Vertices);
	if (factor <= 0)
		throw std::logic_error("Factor must be a strictly positive number");
	for (auto& vertice : vertices)
	{
		vertice.x *= factor;
		vertice.y *= factor;
	}
	return CreateNoCheck(vertices, m_Faces);
}

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.emplace_back(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.emplace_back(baseRadius * cos(ang), baseRadius * sin(ang), height);
	}
}

void CPolyhedron::updatePolygons() const
{
	tempPolygons.resize(m_Faces.size());
	transform(
		m_Faces.begin(), m_Faces.end(), tempPolygons.begin(),
		FCreatePolygonFromFace<TPolygonWithPlane>(m_Vertices));
	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] = m_Vertices[f.vertices[i]];
	TPlane tmp;
	if (!poly.getPlane(tmp)) return false;
	f.normal = tmp.getNormalVector();
	TPoint3D c;
	getCenter(c);
	if (tmp.evaluatePoint(c) > 0) f.normal *= -1;
	return true;
}

void CPolyhedron::addEdges(const TPolyhedronFace& f)
{
	TPolyhedronEdge e;
	auto it = f.vertices.begin();
	e.v1 = *it;
	++it;
	while (it != f.vertices.end())
	{
		e.v2 = *it;
		if (find(m_Edges.begin(), m_Edges.end(), e) == m_Edges.end())
			m_Edges.push_back(e);
		e.v1 = e.v2;
		++it;
	}
	e.v2 = *(f.vertices.begin());
	if (find(m_Edges.begin(), m_Edges.end(), e) == m_Edges.end())
		m_Edges.push_back(e);
}

bool CPolyhedron::checkConsistence(
	const vector<TPoint3D>& vertices, const vector<TPolyhedronFace>& faces)
{
	size_t N = vertices.size();
	if (vertices.size() > 0)
		for (auto it = vertices.begin(); it != vertices.end() - 1; ++it)
			for (auto it2 = it + 1; it2 != vertices.end(); ++it2)
				if (*it == *it2) return false;
	for (const auto& face : faces)
	{
		const vector<uint32_t>& e = face.vertices;
		for (unsigned int it2 : e)
			if (it2 >= N) return false;
	}
	return true;
}

size_t CPolyhedron::edgesInVertex(size_t vertex) const
{
	size_t res = 0;
	for (const auto& mEdge : m_Edges)
		if (mEdge.v1 == vertex || mEdge.v2 == vertex) res++;
	return res;
}

size_t CPolyhedron::facesInVertex(size_t vertex) const
{
	size_t res = 0;
	for (const auto& mFace : m_Faces)
		if (find(mFace.vertices.begin(), mFace.vertices.end(), vertex) !=
			mFace.vertices.end())
			res++;
	return res;
}

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

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

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

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

uint8_t CPolyhedron::serializeGetVersion() const { return 1; }
void CPolyhedron::serializeTo(mrpt::serialization::CArchive& out) const
{
	writeToStreamRender(out);
	// version 0
	out << m_Vertices << m_Faces << m_Wireframe << m_lineWidth;
	CRenderizableShaderTriangles::params_serialize(out);  // v1
}

void CPolyhedron::serializeFrom(
	mrpt::serialization::CArchive& in, uint8_t version)
{
	switch (version)
	{
		case 0:
		case 1:
			readFromStreamRender(in);
			in >> m_Vertices >> m_Faces >> m_Wireframe >> m_lineWidth;
			if (!checkConsistence(m_Vertices, m_Faces))
				throw std::logic_error("Inconsistent data read from stream");
			for (auto& mFace : m_Faces)
			{
				if (!setNormal(mFace))
					throw std::logic_error("Bad face specification");
				addEdges(mFace);
			}

			if (version >= 1)
				CRenderizableShaderTriangles::params_deserialize(in);

			break;
		default: MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
	};
	CRenderizable::notifyChange();
}

auto CPolyhedron::getBoundingBox() const -> mrpt::math::TBoundingBox
{
	return mrpt::math::TBoundingBox({0, 0, 0}, {0, 0, 0}).compose(m_pose);
}

/*CPolyhedron::Ptr 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);
}*/

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

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

void CPolyhedron::render(const RenderContext& rc) const
{
	switch (rc.shader_id)
	{
		case DefaultShaderID::TRIANGLES:
			if (!m_Wireframe) CRenderizableShaderTriangles::render(rc);
			break;
		case DefaultShaderID::WIREFRAME:
			if (m_Wireframe) CRenderizableShaderWireFrame::render(rc);
			break;
	};
}
void CPolyhedron::renderUpdateBuffers() const
{
	CRenderizableShaderTriangles::renderUpdateBuffers();
	CRenderizableShaderWireFrame::renderUpdateBuffers();
}

void CPolyhedron::onUpdateBuffers_Wireframe()
{
	auto& vbd = CRenderizableShaderWireFrame::m_vertex_buffer_data;
	auto& cbd = CRenderizableShaderWireFrame::m_color_buffer_data;
	vbd.clear();

	for (const auto& edge : m_Edges)
	{
		vbd.emplace_back(m_Vertices[edge.v1]);
		vbd.emplace_back(m_Vertices[edge.v2]);
	}

	cbd.assign(vbd.size(), m_color);
}

void CPolyhedron::onUpdateBuffers_Triangles()
{
	auto& tris = CRenderizableShaderTriangles::m_triangles;
	tris.clear();

	for (const auto& face : m_Faces)
	{
		const size_t N = face.vertices.size();
		if (N < 3) continue;

		// convert polygon -> triangle fan:
		for (size_t i = 0; i < N - 2; i++)
		{
			const size_t ip0 = 0;
			const size_t ip1 = (i + 1) % N;
			const size_t ip2 = (i + 2) % N;

			const TPoint3D& p0 = m_Vertices[face.vertices[ip0]];
			TPoint3D p1 = m_Vertices[face.vertices[ip1]];
			TPoint3D p2 = m_Vertices[face.vertices[ip2]];

			// Ensure the normal points outwards:
			const auto normal = mrpt::math::crossProduct3D(p1 - p0, p2 - p0);
			if (mrpt::math::dotProduct<3, double, TPoint3D, TPoint3D>(
					normal, p0) < 0)
				std::swap(p1, p2);

			tris.emplace_back(p0, p1, p2);
		}
	}

	// All faces, all vertices, same color:
	for (auto& t : tris)
		t.setColor(m_color);
}
