convexHull.hpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2011-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @package
 * @author
 * @date
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
#pragma once
 
#include <cassert>
#include <iostream>
#include <memory>
#include <vector>
 
#include "point.hpp"
 
template <class T>
class DoublyLinkedEdgeList
{
public:
    struct Face;
    struct HalfEdge;
    using FacePtr = std::shared_ptr<Face>;
    using HalfEdgePtr = std::shared_ptr<HalfEdge>;
 
    struct HalfEdge
    {
        const T* origin;
        HalfEdgePtr twin;
        FacePtr incidentFace;
        HalfEdgePtr next;
        HalfEdgePtr prev;
    };
 
    struct Face
    {
        Face() : outerComponent(NULL), lastVisitedBy(NULL), toDelete(false)
        {
        }
 
        HalfEdgePtr outerComponent; //The face lies on the left of the edge
        std::vector<HalfEdgePtr> innerComponents;
        std::vector<const T*> visiblePoints;
        const T* lastVisitedBy;
        bool toDelete;
    };
 
    // ~DoublyLinkedEdgeList() {
    //  for(HalfEdgePtr e : mHalfEdges) {
    //      delete e;
    //  }
    //  for(FacePtr f : mFaces) {
    //      delete f;
    //  }
    // }
 
    std::vector<FacePtr>
    faces()
    {
        return mFaces;
    }
 
    /*
     * Check if the face f is visible from point p
     */
    static bool
    visible(const FacePtr& f, const T& p)
    {
        HalfEdgePtr a = f->outerComponent;
        HalfEdgePtr b = a->next;
        HalfEdgePtr c = b->next;
 
        T x = sub(*b->origin, *a->origin);
        T y = sub(*c->origin, *a->origin);
        T n = cross(x, y);
 
        //The triangle is only visible if the product of the
        //normal and P - A is positive, i.e. smaller than 90 degs.
        return dot(n, sub(p, *(a->origin))) >= 0;
    }
 
    /*
     * (Positive) distance from a point to a face
     */
    static double
    distance(const FacePtr& f, const T& p)
    {
        auto e = f->outerComponent;
        const T* a = e->origin;
        const T* b = e->next->origin;
        const T* c = e->next->next->origin;
 
        return distanceToTriangle(*a, *b, *c, p);
    }
 
    /*
    * Add a CCW triangular face, where e is the base and p the
    * opposite point.
    */
    FacePtr
    addFace(HalfEdgePtr e, const T* p)
    {
        FacePtr f(new Face());
        f->outerComponent = e;
        HalfEdgePtr ep(new HalfEdge());
        HalfEdgePtr pe(new HalfEdge());
        ep->origin = e->twin->origin;
        pe->origin = p;
        ep->incidentFace = pe->incidentFace = e->incidentFace = f;
        e->next = ep;
        ep->next = pe;
        pe->next = e;
        e->prev = pe;
        pe->prev = ep;
        ep->prev = e;
 
        //Note that here we don't use the `twin->origin` tests
        assert(e->prev->next == e);
        assert(e->next->prev == e);
        assert(e->twin->twin == e);
        assert(e->prev->incidentFace == e->incidentFace);
        assert(e->next->incidentFace == e->incidentFace);
 
        assert(ep->prev->next == ep);
        assert(ep->next->prev == ep);
        assert(ep->prev->incidentFace == ep->incidentFace);
        assert(ep->next->incidentFace == ep->incidentFace);
 
        assert(pe->prev->next == pe);
        assert(pe->next->prev == pe);
        assert(pe->prev->incidentFace == pe->incidentFace);
        assert(pe->next->incidentFace == pe->incidentFace);
 
        assert(f->outerComponent->incidentFace == f);
        mFaces.push_back(f);
        return f;
    }
 
    double
    area()
    {
        return m_area;
    }
 
    void
    calculateArea()
    {
        double area = 0.0;
 
        //Heron's formula
        for (const auto& f : mFaces)
        {
            HalfEdgePtr e = f->outerComponent;
            const T* x = e->origin;
            const T* y = e->next->origin;
            const T* z = e->next->next->origin;
 
            double a = norm(sub(*y, *x));
            double b = norm(sub(*z, *x));
            double c = norm(sub(*y, *z));
            double p = (a + b + c) / 2;
 
            area += sqrt(abs(p * (p - a) * (p - b) * (p - c)));
        }
 
        m_area = area;
    }
 
    double
    volume()
    {
        return m_volume;
    }
 
    void
    calculateVolume()
    {
        double volume = 0.0;
        //Use the origin as the center of the volume calculation
        //TODO make this more robust (centroid?)
        T center(0, 0, 0);
 
        for (const auto& f : mFaces)
        {
            HalfEdgePtr e = f->outerComponent;
            const T* x = e->origin;
            const T* y = e->next->origin;
            const T* z = e->next->next->origin;
 
            double a = norm(sub(*y, *x));
            double b = norm(sub(*z, *x));
            double c = norm(sub(*y, *z));
            double p = (a + b + c) / 2;
 
            double area = sqrt(abs(p * (p - a) * (p - b) * (p - c)));
            double height = distance(f, center);
            volume += area * height / 3.0;
        }
 
        m_volume = volume;
    }
 
    /*
     * Assume the first three vertices are already in CCW fashion
     * when looking at the base from the bottom, i.e. the border
     * of the base is a->b->c->a.
     */
    void
    addTetrahedron(const T& a, const T& b, const T& c, const T& d)
    {
        //This code is going to be sooo buggy.....
 
        //Create the base
        FacePtr base(new Face());
        HalfEdgePtr ab(new HalfEdge()); //Base edge from a and twin
        HalfEdgePtr ba(new HalfEdge());
        ab->twin = ba;
        ba->twin = ab;
        ab->origin = &a;
        ba->origin = &b;
        ab->incidentFace = base;
        mHalfEdges.push_back(ab);
        mHalfEdges.push_back(ba);
 
        HalfEdgePtr bc(new HalfEdge()); //Base edge from b and twin
        HalfEdgePtr cb(new HalfEdge());
        bc->twin = cb;
        cb->twin = bc;
        bc->origin = &b;
        cb->origin = &c;
        bc->incidentFace = base;
        mHalfEdges.push_back(bc);
        mHalfEdges.push_back(cb);
 
        HalfEdgePtr ca(new HalfEdge()); //Base edge from c and twin
        HalfEdgePtr ac(new HalfEdge());
        ca->twin = ac;
        ac->twin = ca;
        ca->origin = &c;
        ac->origin = &a;
        ca->incidentFace = base;
        mHalfEdges.push_back(ca);
        mHalfEdges.push_back(ac);
 
        base->outerComponent = ab;
 
        //Link the edges
        ab->next = bc;
        bc->next = ca;
        ca->next = ab;
        ab->prev = ca;
        bc->prev = ab;
        ca->prev = bc;
 
        mFaces.push_back(base);
 
        assert(!DoublyLinkedEdgeList<T>::visible(base, d));
 
        //Create the edges from the point
        HalfEdgePtr bd(new HalfEdge());
        HalfEdgePtr db(new HalfEdge());
        bd->twin = db;
        db->twin = bd;
        bd->origin = &b;
        db->origin = &d;
        mHalfEdges.push_back(bd);
        mHalfEdges.push_back(db);
 
        HalfEdgePtr da(new HalfEdge());
        HalfEdgePtr ad(new HalfEdge());
        da->twin = ad;
        ad->twin = da;
        da->origin = &d;
        ad->origin = &a;
        mHalfEdges.push_back(da);
        mHalfEdges.push_back(ad);
 
        HalfEdgePtr dc(new HalfEdge());
        HalfEdgePtr cd(new HalfEdge());
        dc->twin = cd;
        cd->twin = dc;
        dc->origin = &d;
        cd->origin = &c;
        mHalfEdges.push_back(dc);
        mHalfEdges.push_back(cd);
 
        //Now the other faces
        FacePtr bad(new Face());
        ba->incidentFace = db->incidentFace = ad->incidentFace = bad;
        ba->next = ad;
        ad->next = db;
        db->next = ba;
        ba->prev = db;
        db->prev = ad;
        ad->prev = ba;
        bad->outerComponent = ba;
        mFaces.push_back(bad);
 
        FacePtr acd(new Face());
        ac->incidentFace = cd->incidentFace = da->incidentFace = acd;
        ac->next = cd;
        cd->next = da;
        da->next = ac;
        ac->prev = da;
        da->prev = cd;
        cd->prev = ac;
        acd->outerComponent = ac;
        mFaces.push_back(acd);
 
        FacePtr cbd(new Face());
        cb->incidentFace = bd->incidentFace = dc->incidentFace = cbd;
        cb->next = bd;
        bd->next = dc;
        dc->next = cb;
        cb->prev = dc;
        dc->prev = bd;
        bd->prev = cb;
        cbd->outerComponent = cb;
        mFaces.push_back(cbd);
 
        //Now try to make sense of this mess
        for (FacePtr f : mFaces)
        {
            assert(f->outerComponent->incidentFace == f);
        }
 
        for (HalfEdgePtr e : mHalfEdges)
        {
            assert(e->prev->next == e);
            assert(e->next->prev == e);
            assert(e->twin->twin == e);
            assert(e->twin->next->origin == e->origin);
            assert(e->prev->twin->origin == e->origin);
            assert(e->prev->incidentFace == e->incidentFace);
            assert(e->next->incidentFace == e->incidentFace);
        }
    }
 
    void
    cleanup()
    {
        mFaces.erase(remove_if(mFaces.begin(), mFaces.end(), [](FacePtr f) { return f->toDelete; }),
                     mFaces.end());
    }
 
private:
    std::vector<HalfEdgePtr> mHalfEdges;
    std::vector<FacePtr> mFaces;
 
    double m_volume;
    double m_area;
};
 
/*
 * Weird pre-C++11 deficiency,
 * you have to use ::type because we cannot use
 * `using ConvexHull = DoublyLinkedEdgeList<T>`
 */
template <typename T>
struct ConvexHull
{
    using type = DoublyLinkedEdgeList<T>;
};
 
namespace Convex
{
    template <class Point>
    std::vector<const Point*>
    extremePoints(const std::vector<Point>& points)
    {
        std::vector<const Point*> tetraPoints;
        const Point *a, *b, *c, *d;
        double dmax, dcur;
        dmax = dcur = 0.0;
        const Point* tmp[6] = {&points[0],
                               &points[0], //x min, max
                               &points[0],
                               &points[0], //y min, max
                               &points[0],
                               &points[0]}; //z min, max
 
        for (unsigned int p = 0; p < points.size(); p++)
        {
            if (points.at(p)[0] < (*tmp[0])[0])
            {
                tmp[0] = &points.at(p);
            }
 
            if (points.at(p)[0] > (*tmp[1])[0])
            {
                tmp[1] = &points.at(p);
            }
 
            if (points.at(p)[1] < (*tmp[2])[1])
            {
                tmp[2] = &points.at(p);
            }
 
            if (points.at(p)[1] > (*tmp[3])[1])
            {
                tmp[3] = &points.at(p);
            }
 
            if (points.at(p)[2] < (*tmp[4])[2])
            {
                tmp[4] = &points.at(p);
            }
 
            if (points.at(p)[2] > (*tmp[5])[2])
            {
                tmp[5] = &points.at(p);
            }
        }
 
        //Find the two most distant points
        for (int i = 0; i < 6; i++)
        {
            for (int j = i + 1; j < 6; j++)
            {
                dcur = distance(*tmp[i], *tmp[j]);
 
                if (dmax < dcur)
                {
                    dmax = dcur;
                    a = tmp[i];
                    b = tmp[j];
                }
            }
        }
 
        //Find the most distant point to the line
        dmax = 0.0;
 
        for (int i = 0; i < 6; i++)
        {
            dcur = distanceToLine(*a, *b, *tmp[i]);
 
            if (dmax < dcur)
            {
                dmax = dcur;
                c = tmp[i];
            }
        }
 
        //Find the most distant point to the plane (from the whole point list)
        dmax = 0.0;
 
        for (unsigned int i = 0; i < points.size(); i++)
        {
            dcur = distanceToTriangle(*a, *b, *c, points.at(i));
 
            if (dmax < dcur)
            {
                dmax = dcur;
                d = &points.at(i);
            }
        }
 
        if (inFront(*a, *b, *c, *d))
        {
            tetraPoints.push_back(b);
            tetraPoints.push_back(a);
        }
        else
        {
            tetraPoints.push_back(a);
            tetraPoints.push_back(b);
        }
 
        tetraPoints.push_back(c);
        tetraPoints.push_back(d);
 
        return tetraPoints;
    }
 
    /*
     * Implementation based on the QuickHull algorithm. The idea is to assign to each face of
     * the CH the points from which it is visible. If this list is non-empty, this face should
     * not be on the CH, and has to be processed. The faces are put on a stack. For each face on
     * the stack, a cone is built from the furthest point and its horizon edges. The points
     * visible from the old faces are reassigned to the new faces.
     */
    template <class Point>
    typename ConvexHull<Point>::type
    convexHull(const std::vector<Point>& points)
    {
        std::cout << "CH -----------------------------------------------------------------"
                  << std::endl;
        const double eps = 0.001;
 
        //Try to find four non-coplanar points
        auto tetraPoints = extremePoints<Point>(points);
 
        assert(tetraPoints.size() == 4);
 
        typename ConvexHull<Point>::type CH;
 
        CH.addTetrahedron(*tetraPoints[0], *tetraPoints[1], *tetraPoints[2], *tetraPoints[3]);
 
        auto facesStack = CH.faces();
        assert(facesStack.size() == 4);
 
        //Assign points to the face that is closest
        for (unsigned int p = 0; p < points.size(); p++)
        {
            bool visible = false;
            double d = std::numeric_limits<double>::max();
            typename ConvexHull<Point>::type::FacePtr closestFace = NULL;
 
            //Find closest face
            for (const auto& f : facesStack)
            {
                double distance = ConvexHull<Point>::type::distance(f, points.at(p));
 
                if (ConvexHull<Point>::type::visible(f, points.at(p)) && distance > eps)
                {
                    if (distance < d)
                    {
                        d = distance;
                        visible = true;
                        closestFace = f;
                    }
                }
            }
 
            if (visible)
            {
                closestFace->visiblePoints.push_back(&(points.at(p)));
            }
        }
 
        //Process the stack of facets
        while (!facesStack.empty())
        {
            auto currentFace = facesStack.back();
            facesStack.pop_back();
 
            if (currentFace->visiblePoints.size() == 0)
            {
                continue;
            }
 
            //std::cout << *currentFace->outerComponent->origin << "\n" << std::endl;
            //std::cout << *currentFace->outerComponent->next->origin << "\n" << std::endl;
            //std::cout << *currentFace->outerComponent->next->next->origin << "\n" << std::endl;
 
            //Find point farthest away
            const Point* point = NULL;
            double d = std::numeric_limits<double>::min();
 
            for (const Point* p : currentFace->visiblePoints)
            {
                double distance = ConvexHull<Point>::type::distance(currentFace, *p);
 
                if (distance >= d)
                {
                    d = distance;
                    point = p;
                }
            }
 
            //Find the horizon as seen from that point
            typename ConvexHull<Point>::type::HalfEdgePtr horizonStart = NULL;
            //First find all visible faces
            std::vector<typename ConvexHull<Point>::type::FacePtr> visibleFaces;
            std::vector<typename ConvexHull<Point>::type::FacePtr> toVisit;
            currentFace->lastVisitedBy = point;
            visibleFaces.push_back(currentFace);
            toVisit.push_back(currentFace);
 
            //Spread from the current face to the adjacent ones until no new
            //face can be added
            while (!toVisit.empty())
            {
                currentFace = toVisit.back();
                toVisit.pop_back();
                auto e = currentFace->outerComponent;
 
                //Go through the adjacent faces
                do
                {
                    auto adjFace = e->twin->incidentFace;
 
                    if (adjFace->lastVisitedBy != point &&
                        ConvexHull<Point>::type::visible(adjFace, *point))
                    {
                        adjFace->lastVisitedBy = point;
                        visibleFaces.push_back(adjFace);
                        toVisit.push_back(adjFace);
                    }
 
                    //If the adjacent face is not visible, this edge lies on the horizon
                    //TODO pull into the othe if-branch
                    if (horizonStart == NULL && !ConvexHull<Point>::type::visible(adjFace, *point))
                    {
                        horizonStart = e;
                    }
 
                    e = e->next;
                } while (e != currentFace->outerComponent);
            }
 
            assert(horizonStart != NULL);
 
            //Mark visible faces for deletion later on
            for (auto v : visibleFaces)
            {
                v->toDelete = true;
            }
 
            //The horizon should be convex when 2D-projected from the point
            std::vector<typename ConvexHull<Point>::type::HalfEdgePtr> horizon;
            auto currentHorizon = horizonStart;
 
            //Build the horizon step by step until the loop is closed
            do
            {
                horizon.push_back(currentHorizon);
                //Find adjacent edge that is on the horizon
                auto nextEdge = currentHorizon->next;
 
                while (ConvexHull<Point>::type::visible(nextEdge->twin->incidentFace, *point))
                {
                    nextEdge = nextEdge->twin->next;
                }
 
                currentHorizon = nextEdge;
            } while (currentHorizon != horizonStart);
 
 
            //Now iterate over the horizon and build the new faces
            //Save the last one so that we can go around the horizon
            std::vector<typename ConvexHull<Point>::type::FacePtr> newFaces;
            auto prev = horizon.back();
            newFaces.push_back(CH.addFace(prev, point));
 
            for (auto e : horizon)
            {
                if (e != horizon.back())
                {
                    //For each one create the new triangular facet to the point
                    auto f = CH.addFace(e, point);
                    newFaces.push_back(f);
 
                    //Assume you are going in CCW order?
                    assert(prev->twin->origin == e->origin);
 
                    //Link to the prev face
                    prev->next->twin = e->prev;
                    e->prev->twin = prev->next;
 
                    prev = e;
                    assert(e->prev->twin->twin == e->prev);
                    assert(e->prev->twin->origin == e->origin);
                }
                else
                {
                    //Went through the whole horizon, join the start and the end,
                    //but don't create a new face
                    prev->next->twin = e->prev;
                    e->prev->twin = prev->next;
 
                    assert(e->prev->twin->twin == e->prev);
                    assert(e->prev->twin->origin == e->origin);
                }
            }
 
            int visiblePoints = 0;
            int assignedPoints = 0;
 
            //Also reassign the points of the old visible faces to the new faces
            for (auto& v : visibleFaces)
            {
                for (auto& p : v->visiblePoints)
                {
                    visiblePoints++;
                    bool visible = false;
                    double d = std::numeric_limits<double>::max();
                    typename ConvexHull<Point>::type::FacePtr closestFace = NULL;
 
                    //Find closest face
                    for (auto& f : newFaces)
                    {
                        double distance = ConvexHull<Point>::type::distance(f, *p);
 
                        if (ConvexHull<Point>::type::visible(f, *p) && distance > eps)
                        {
                            if (distance < d)
                            {
                                d = distance;
                                visible = true;
                                closestFace = f;
                            }
                        }
                    }
 
                    if (visible)
                    {
                        closestFace->visiblePoints.push_back(p);
                        assignedPoints++;
                    }
                }
 
                v->visiblePoints.clear();
            }
 
            //Push the new faces into the faces stack
            for (auto& f : newFaces)
            {
                if (!f->visiblePoints.empty())
                {
                    facesStack.push_back(f);
                }
            }
 
            //Remember to delete the old visible faces (which are no longer in the CH)
        }
 
        CH.cleanup();
 
        CH.calculateArea();
        CH.calculateVolume();
 
        return CH;
    }
} // namespace Convex