CPolyhedron.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2020, 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 0; }
void CPolyhedron::serializeTo(mrpt::serialization::CArchive& out) const
{
    writeToStreamRender(out);
    // version 0
    out << m_Vertices << m_Faces << m_Wireframe << m_lineWidth;
}

void CPolyhedron::serializeFrom(
    mrpt::serialization::CArchive& in, uint8_t version)
{
    switch (version)
    {
        case 0:
            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);
            }
            break;
        default:
            MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
    };
    CRenderizable::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);
}

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