kicad/pcbnew/ratsnest/delauney.h

/*
 * delauney.h
 *
 *  Created on: Jun 19, 2020
 *      Author: seth
 */

#ifndef PCBNEW_RATSNEST_DELAUNEY_H_
#define PCBNEW_RATSNEST_DELAUNEY_H_

#include <algorithm>
#include <cmath>
#include <iostream>
#include <limits>
#include <numeric>
#include <stdexcept>
#include <tuple>
#include <vector>

constexpr std::size_t INVALID_INDEX =
    (std::numeric_limits<std::size_t>::max)();

class Point
{
public:
    Point(double x, double y) : m_x(x), m_y(y)
    {}
    Point() : m_x(0), m_y(0)
    {}

    double x() const
    { return m_x; }

    double y() const
    { return m_y; }

    double magnitude2() const
    { return m_x * m_x + m_y * m_y; }

    static double determinant(const Point& p1, const Point& p2)
    {
        return p1.m_x * p2.m_y - p1.m_y * p2.m_x;
    }

    static Point vector(const Point& p1, const Point& p2)
    {
        return Point(p2.m_x - p1.m_x, p2.m_y - p1.m_y);
    }

    static double dist2(const Point& p1, const Point& p2)
    {
        Point vec = vector(p1, p2);
        return vec.m_x * vec.m_x + vec.m_y * vec.m_y;
    }

    static bool equal(const Point& p1, const Point& p2, double span)
    {
        double dist = dist2(p1, p2) / span;

        // ABELL - This number should be examined to figure how how
        // it correlates with the breakdown of calculating determinants.
        return dist < 1e-20;
    }

private:
    double m_x;
    double m_y;
};

inline std::ostream& operator<<(std::ostream& out, const Point& p)
{
    out << p.x() << "/" << p.y();
    return out;
}


class Points
{
public:
    using const_iterator = Point const *;

    Points(const std::vector<double>& coords) : m_coords(coords)
    {}

    const Point& operator[](size_t offset)
    {
        return reinterpret_cast<const Point&>(
            *(m_coords.data() + (offset * 2)));
    };

    Points::const_iterator begin() const
        { return reinterpret_cast<const Point *>(m_coords.data()); }
    Points::const_iterator end() const
        { return reinterpret_cast<const Point *>(
            m_coords.data() + m_coords.size()); }
    size_t size() const
        { return m_coords.size() / 2; }

private:
    const std::vector<double>& m_coords;
};

class Delaunator
{

public:
    std::vector<double> const &coords;
    Points m_points;

    // 'triangles' stores the indices to the 'X's of the input
    // 'coords'.
    std::vector<std::size_t> triangles;

    // 'halfedges' store indices into 'triangles'.  If halfedges[X] = Y,
    // It says that there's an edge from X to Y where a) X and Y are
    // both indices into triangles and b) X and Y are indices into different
    // triangles in the array.  This allows you to get from a triangle to
    // its adjacent triangle.  If the a triangle edge has no adjacent triangle,
    // its half edge will be INVALID_INDEX.
    std::vector<std::size_t> halfedges;

    std::vector<std::size_t> hull_prev;
    std::vector<std::size_t> hull_next;

    // This contains indexes into the triangles array.
    std::vector<std::size_t> hull_tri;
    std::size_t hull_start;

    inline Delaunator( std::vector<double> const &in_coords );
    inline double get_hull_area();
    inline double get_triangle_area();

private:
    std::vector<std::size_t> m_hash;
    Point m_center;
    std::size_t m_hash_size;
    std::vector<std::size_t> m_edge_stack;

    inline std::size_t legalize( std::size_t a );
    inline std::size_t hash_key( double x, double y ) const;
    inline std::size_t add_triangle( std::size_t i0, std::size_t i1, std::size_t i2, std::size_t a,
            std::size_t b, std::size_t c );
    inline void link( std::size_t a, std::size_t b );
};

//@see https://stackoverflow.com/questions/33333363/built-in-mod-vs-custom-mod-function-improve-the-performance-of-modulus-op/33333636#33333636
inline size_t fast_mod( const size_t i, const size_t c )
{
    return i >= c ? i % c : i;
}

// Kahan and Babuska summation, Neumaier variant; accumulates less FP error
inline double sum( const std::vector<double> &x )
{
    double sum = x[0];
    double err = 0.0;

    for( size_t i = 1; i < x.size(); i++ )
    {
        const double k = x[i];
        const double m = sum + k;
        err += std::fabs( sum ) >= std::fabs( k ) ? sum - m + k : k - m + sum;
        sum = m;
    }
    return sum + err;
}

inline double dist( const double ax, const double ay, const double bx, const double by )
{
    const double dx = ax - bx;
    const double dy = ay - by;
    return dx * dx + dy * dy;
}

inline double circumradius( const Point &p1, const Point &p2, const Point &p3 )
{
    Point d = Point::vector( p1, p2 );
    Point e = Point::vector( p1, p3 );

    const double bl = d.magnitude2();
    const double cl = e.magnitude2();
    const double det = Point::determinant( d, e );

    Point radius( ( e.y() * bl - d.y() * cl ) * 0.5 / det,
            ( d.x() * cl - e.x() * bl ) * 0.5 / det );

    if( ( bl > 0.0 || bl < 0.0 ) && ( cl > 0.0 || cl < 0.0 ) && ( det > 0.0 || det < 0.0 ) )
        return radius.magnitude2();
    return ( std::numeric_limits<double>::max )();
}

inline double circumradius( const double ax, const double ay, const double bx, const double by,
        const double cx, const double cy )
{
    const double dx = bx - ax;
    const double dy = by - ay;
    const double ex = cx - ax;
    const double ey = cy - ay;

    const double bl = dx * dx + dy * dy;
    const double cl = ex * ex + ey * ey;
    const double d = dx * ey - dy * ex;

    const double x = ( ey * bl - dy * cl ) * 0.5 / d;
    const double y = ( dx * cl - ex * bl ) * 0.5 / d;

    if( ( bl > 0.0 || bl < 0.0 ) && ( cl > 0.0 || cl < 0.0 ) && ( d > 0.0 || d < 0.0 ) )
    {
        return x * x + y * y;
    }
    else
    {
        return ( std::numeric_limits<double>::max )();
    }
}

inline bool clockwise( const Point &p0, const Point &p1, const Point &p2 )
{
    Point v0 = Point::vector( p0, p1 );
    Point v1 = Point::vector( p0, p2 );
    double det = Point::determinant( v0, v1 );
    double dist = v0.magnitude2() + v1.magnitude2();
    double dist2 = Point::dist2( v0, v1 );
    if( det == 0 )
    {
        return false;
    }
    double reldet = std::abs( dist / det );
    if( reldet > 1e14 )
        return false;
    return det < 0;
}

inline bool clockwise( double px, double py, double qx, double qy, double rx, double ry )
{
    Point p0( px, py );
    Point p1( qx, qy );
    Point p2( rx, ry );
    return clockwise( p0, p1, p2 );
}

inline bool counterclockwise( const Point &p0, const Point &p1, const Point &p2 )
{
    Point v0 = Point::vector( p0, p1 );
    Point v1 = Point::vector( p0, p2 );
    double det = Point::determinant( v0, v1 );
    double dist = v0.magnitude2() + v1.magnitude2();
    double dist2 = Point::dist2( v0, v1 );
    if( det == 0 )
        return false;
    double reldet = std::abs( dist / det );
    if( reldet > 1e14 )
        return false;
    return det > 0;
}

inline bool counterclockwise( double px, double py, double qx, double qy, double rx, double ry )
{
    Point p0( px, py );
    Point p1( qx, qy );
    Point p2( rx, ry );
    return counterclockwise( p0, p1, p2 );
}

inline Point circumcenter( const double ax, const double ay, const double bx, const double by,
        const double cx, const double cy )
{
    const double dx = bx - ax;
    const double dy = by - ay;
    const double ex = cx - ax;
    const double ey = cy - ay;

    const double bl = dx * dx + dy * dy;
    const double cl = ex * ex + ey * ey;
    //ABELL - This is suspect for div-by-0.
    const double d = dx * ey - dy * ex;

    const double x = ax + ( ey * bl - dy * cl ) * 0.5 / d;
    const double y = ay + ( dx * cl - ex * bl ) * 0.5 / d;

    return Point( x, y );
}

inline bool in_circle( const double ax, const double ay, const double bx, const double by,
        const double cx, const double cy, const double px, const double py )
{
    const double dx = ax - px;
    const double dy = ay - py;
    const double ex = bx - px;
    const double ey = by - py;
    const double fx = cx - px;
    const double fy = cy - py;

    const double ap = dx * dx + dy * dy;
    const double bp = ex * ex + ey * ey;
    const double cp = fx * fx + fy * fy;

    return ( dx * ( ey * cp - bp * fy ) - dy * ( ex * cp - bp * fx ) + ap * ( ex * fy - ey * fx ) )
            < 0.0;
}

constexpr double EPSILON = std::numeric_limits<double>::epsilon();

inline bool check_pts_equal( double x1, double y1, double x2, double y2 )
{
    return std::fabs( x1 - x2 ) <= EPSILON && std::fabs( y1 - y2 ) <= EPSILON;
}

// monotonically increases with real angle, but doesn't need expensive trigonometry
inline double pseudo_angle( const double dx, const double dy )
{
    const double p = dx / ( std::abs( dx ) + std::abs( dy ) );
    return ( dy > 0.0 ? 3.0 - p : 1.0 + p ) / 4.0; // [0..1)
}

Delaunator::Delaunator( std::vector<double> const &in_coords ) :
        coords( in_coords ), m_points( in_coords )
{
    std::size_t n = coords.size() >> 1;

    std::vector<std::size_t> ids( n );
    std::iota( ids.begin(), ids.end(), 0 );

    double max_x = std::numeric_limits<double>::lowest();
    double max_y = std::numeric_limits<double>::lowest();
    double min_x = ( std::numeric_limits<double>::max )();
    double min_y = ( std::numeric_limits<double>::max )();
    for( const Point &p : m_points )
    {
        min_x = std::min( p.x(), min_x );
        min_y = std::min( p.y(), min_y );
        max_x = std::max( p.x(), max_x );
        max_y = std::max( p.y(), max_y );
    }
    double width = max_x - min_x;
    double height = max_y - min_y;
    double span = width * width + height * height; // Everything is square dist.

    Point center( ( min_x + max_x ) / 2, ( min_y + max_y ) / 2 );

    std::size_t i0 = INVALID_INDEX;
    std::size_t i1 = INVALID_INDEX;
    std::size_t i2 = INVALID_INDEX;

    // pick a seed point close to the centroid
    double min_dist = ( std::numeric_limits<double>::max )();
    for( size_t i = 0; i < m_points.size(); ++i )
    {
        const Point &p = m_points[i];
        const double d = Point::dist2( center, p );
        if( d < min_dist )
        {
            i0 = i;
            min_dist = d;
        }
    }

    const Point &p0 = m_points[i0];

    min_dist = ( std::numeric_limits<double>::max )();

    // find the point closest to the seed
    for( std::size_t i = 0; i < n; i++ )
    {
        if( i == i0 )
            continue;
        const double d = Point::dist2( p0, m_points[i] );
        if( d < min_dist && d > 0.0 )
        {
            i1 = i;
            min_dist = d;
        }
    }

    const Point &p1 = m_points[i1];

    double min_radius = ( std::numeric_limits<double>::max )();

    // find the third point which forms the smallest circumcircle
    // with the first two
    for( std::size_t i = 0; i < n; i++ )
    {
        if( i == i0 || i == i1 )
            continue;

        const double r = circumradius( p0, p1, m_points[i] );
        if( r < min_radius )
        {
            i2 = i;
            min_radius = r;
        }
    }

    if( !( min_radius < ( std::numeric_limits<double>::max )() ) )
    {
        throw std::runtime_error( "not triangulation" );
    }

    const Point &p2 = m_points[i2];

    if( counterclockwise( p0, p1, p2 ) )
        std::swap( i1, i2 );

    double i0x = p0.x();
    double i0y = p0.y();
    double i1x = m_points[i1].x();
    double i1y = m_points[i1].y();
    double i2x = m_points[i2].x();
    double i2y = m_points[i2].y();

    m_center = circumcenter( i0x, i0y, i1x, i1y, i2x, i2y );

    // Calculate the distances from the center once to avoid having to
    // calculate for each compare.  This used to be done in the comparator,
    // but GCC 7.5+ would copy the comparator to iterators used in the
    // sort, and this was excruciatingly slow when there were many points
    // because you had to copy the vector of distances.
    std::vector<double> dists;
    dists.reserve( m_points.size() );
    for( const Point &p : m_points )
        dists.push_back( dist( p.x(), p.y(), m_center.x(), m_center.y() ) );

    // sort the points by distance from the seed triangle circumcenter
    std::sort( ids.begin(), ids.end(), [ &dists ]( std::size_t i, std::size_t j )
    {   return dists[i] < dists[j];} );

    // initialize a hash table for storing edges of the advancing convex hull
    m_hash_size = static_cast<std::size_t>( std::ceil( std::sqrt( n ) ) );
    m_hash.resize( m_hash_size );
    std::fill( m_hash.begin(), m_hash.end(), INVALID_INDEX );

    // initialize arrays for tracking the edges of the advancing convex hull
    hull_prev.resize( n );
    hull_next.resize( n );
    hull_tri.resize( n );

    hull_start = i0;

    size_t hull_size = 3;

    hull_next[i0] = hull_prev[i2] = i1;
    hull_next[i1] = hull_prev[i0] = i2;
    hull_next[i2] = hull_prev[i1] = i0;

    hull_tri[i0] = 0;
    hull_tri[i1] = 1;
    hull_tri[i2] = 2;

    m_hash[hash_key( i0x, i0y )] = i0;
    m_hash[hash_key( i1x, i1y )] = i1;
    m_hash[hash_key( i2x, i2y )] = i2;

    // ABELL - Why are we doing this is n < 3?  There is no triangulation if
    //  there is no triangle.

    std::size_t max_triangles = n < 3 ? 1 : 2 * n - 5;
    triangles.reserve( max_triangles * 3 );
    halfedges.reserve( max_triangles * 3 );
    add_triangle( i0, i1, i2, INVALID_INDEX, INVALID_INDEX, INVALID_INDEX );
    double xp = std::numeric_limits<double>::quiet_NaN();
    double yp = std::numeric_limits<double>::quiet_NaN();

    // Go through points based on distance from the center.
    for( std::size_t k = 0; k < n; k++ )
    {
        const std::size_t i = ids[k];
        const double x = coords[2 * i];
        const double y = coords[2 * i + 1];

        // skip near-duplicate points
        if( k > 0 && check_pts_equal( x, y, xp, yp ) )
            continue;
        xp = x;
        yp = y;

        //ABELL - This is dumb.  We have the indices.  Use them.
        // skip seed triangle points
        if( check_pts_equal( x, y, i0x, i0y ) || check_pts_equal( x, y, i1x, i1y )
                || check_pts_equal( x, y, i2x, i2y ) )
            continue;

        // find a visible edge on the convex hull using edge hash
        std::size_t start = 0;

        size_t key = hash_key( x, y );
        for( size_t j = 0; j < m_hash_size; j++ )
        {
            start = m_hash[fast_mod( key + j, m_hash_size )];

            // ABELL - Not sure how hull_next[start] could ever equal start
            // I *think* hull_next is just a representation of the hull in one
            // direction.
            if( start != INVALID_INDEX && start != hull_next[start] )
                break;
        }

        //ABELL
        // Make sure what we found is on the hull.
        assert( hull_prev[start] != start );
        assert( hull_prev[start] != INVALID_INDEX );

        start = hull_prev[start];
        size_t e = start;
        size_t q;

        // Advance until we find a place in the hull where our current point
        // can be added.
        while( true )
        {
            q = hull_next[e];
            if( Point::equal( m_points[i], m_points[e], span )
                    || Point::equal( m_points[i], m_points[q], span ) )
            {
                e = INVALID_INDEX;
                break;
            }
            if( counterclockwise( x, y, coords[2 * e], coords[2 * e + 1], coords[2 * q],
                    coords[2 * q + 1] ) )
                break;
            e = q;
            if( e == start )
            {
                e = INVALID_INDEX;
                break;
            }
        }

        // ABELL
        // This seems wrong.  Perhaps we should check what's going on?
        if( e == INVALID_INDEX )     // likely a near-duplicate point; skip it
            continue;

        // add the first triangle from the point
        std::size_t t = add_triangle( e, i, hull_next[e], INVALID_INDEX, INVALID_INDEX,
                hull_tri[e] );

        hull_tri[i] = legalize( t + 2 ); // Legalize the triangle we just added.
        hull_tri[e] = t;
        hull_size++;

        // walk forward through the hull, adding more triangles and
        // flipping recursively
        std::size_t next = hull_next[e];
        while( true )
        {
            q = hull_next[next];
            if( !counterclockwise( x, y, coords[2 * next], coords[2 * next + 1], coords[2 * q],
                    coords[2 * q + 1] ) )
                break;
            t = add_triangle( next, i, q, hull_tri[i], INVALID_INDEX, hull_tri[next] );
            hull_tri[i] = legalize( t + 2 );
            hull_next[next] = next; // mark as removed
            hull_size--;
            next = q;
        }

        // walk backward from the other side, adding more triangles and flipping
        if( e == start )
        {
            while( true )
            {
                q = hull_prev[e];
                if( !counterclockwise( x, y, coords[2 * q], coords[2 * q + 1], coords[2 * e],
                        coords[2 * e + 1] ) )
                    break;
                t = add_triangle( q, i, e, INVALID_INDEX, hull_tri[e], hull_tri[q] );
                legalize( t + 2 );
                hull_tri[q] = t;
                hull_next[e] = e; // mark as removed
                hull_size--;
                e = q;
            }
        }

        // update the hull indices
        hull_prev[i] = e;
        hull_start = e;
        hull_prev[next] = i;
        hull_next[e] = i;
        hull_next[i] = next;

        m_hash[hash_key( x, y )] = i;
        m_hash[hash_key( coords[2 * e], coords[2 * e + 1] )] = e;
    }
}

double Delaunator::get_hull_area()
{
    std::vector<double> hull_area;
    size_t e = hull_start;
    size_t cnt = 1;
    do
    {
        hull_area.push_back(
                ( coords[2 * e] - coords[2 * hull_prev[e]] )
                        * ( coords[2 * e + 1] + coords[2 * hull_prev[e] + 1] ) );
        cnt++;
        e = hull_next[e];
    } while( e != hull_start );
    return sum( hull_area );
}

double Delaunator::get_triangle_area()
{
    std::vector<double> vals;
    for( size_t i = 0; i < triangles.size(); i += 3 )
    {
        const double ax = coords[2 * triangles[i]];
        const double ay = coords[2 * triangles[i] + 1];
        const double bx = coords[2 * triangles[i + 1]];
        const double by = coords[2 * triangles[i + 1] + 1];
        const double cx = coords[2 * triangles[i + 2]];
        const double cy = coords[2 * triangles[i + 2] + 1];
        double val = std::fabs( ( by - ay ) * ( cx - bx ) - ( bx - ax ) * ( cy - by ) );
        vals.push_back( val );
    }
    return sum( vals );
}

std::size_t Delaunator::legalize( std::size_t a )
{
    std::size_t i = 0;
    std::size_t ar = 0;
    m_edge_stack.clear();

    // recursion eliminated with a fixed-size stack
    while( true )
    {
        const size_t b = halfedges[a];

        /* if the pair of triangles doesn't satisfy the Delaunay condition
         * (p1 is inside the circumcircle of [p0, pl, pr]), flip them,
         * then do the same check/flip recursively for the new pair of triangles
         *
         *           pl                    pl
         *          /||\                  /  \
         *       al/ || \bl            al/    \a
         *        /  ||  \              /      \
         *       /  a||b  \    flip    /___ar___\
         *     p0\   ||   /p1   =>   p0\---bl---/p1
         *        \  ||  /              \      /
         *       ar\ || /br             b\    /br
         *          \||/                  \  /
         *           pr                    pr
         */
        const size_t a0 = 3 * ( a / 3 );
        ar = a0 + ( a + 2 ) % 3;

        if( b == INVALID_INDEX )
        {
            if( i > 0 )
            {
                i--;
                a = m_edge_stack[i];
                continue;
            }
            else
            {
                //i = INVALID_INDEX;
                break;
            }
        }

        const size_t b0 = 3 * ( b / 3 );
        const size_t al = a0 + ( a + 1 ) % 3;
        const size_t bl = b0 + ( b + 2 ) % 3;

        const std::size_t p0 = triangles[ar];
        const std::size_t pr = triangles[a];
        const std::size_t pl = triangles[al];
        const std::size_t p1 = triangles[bl];

        const bool illegal = in_circle( coords[2 * p0], coords[2 * p0 + 1], coords[2 * pr],
                coords[2 * pr + 1], coords[2 * pl], coords[2 * pl + 1], coords[2 * p1],
                coords[2 * p1 + 1] );

        if( illegal )
        {
            triangles[a] = p1;
            triangles[b] = p0;

            auto hbl = halfedges[bl];

            // Edge swapped on the other side of the hull (rare).
            // Fix the halfedge reference
            if( hbl == INVALID_INDEX )
            {
                std::size_t e = hull_start;
                do
                {
                    if( hull_tri[e] == bl )
                    {
                        hull_tri[e] = a;
                        break;
                    }
                    e = hull_prev[e];
                } while( e != hull_start );
            }
            link( a, hbl );
            link( b, halfedges[ar] );
            link( ar, bl );
            std::size_t br = b0 + ( b + 1 ) % 3;

            if( i < m_edge_stack.size() )
            {
                m_edge_stack[i] = br;
            }
            else
            {
                m_edge_stack.push_back( br );
            }
            i++;

        }
        else
        {
            if( i > 0 )
            {
                i--;
                a = m_edge_stack[i];
                continue;
            }
            else
            {
                break;
            }
        }
    }
    return ar;
}

std::size_t Delaunator::hash_key( const double x, const double y ) const
{
    const double dx = x - m_center.x();
    const double dy = y - m_center.y();
    return fast_mod(
            static_cast<std::size_t>( std::llround(
                    std::floor( pseudo_angle( dx, dy ) * static_cast<double>( m_hash_size ) ) ) ),
            m_hash_size );
}

std::size_t Delaunator::add_triangle( std::size_t i0, std::size_t i1, std::size_t i2,
        std::size_t a, std::size_t b, std::size_t c )
{
    std::size_t t = triangles.size();
    triangles.push_back( i0 );
    triangles.push_back( i1 );
    triangles.push_back( i2 );
    link( t, a );
    link( t + 1, b );
    link( t + 2, c );
    return t;
}

void Delaunator::link( const std::size_t a, const std::size_t b )
{
    std::size_t s = halfedges.size();
    if( a == s )
    {
        halfedges.push_back( b );
    }
    else if( a < s )
    {
        halfedges[a] = b;
    }
    else
    {
        throw std::runtime_error( "Cannot link edge" );
    }
    if( b != INVALID_INDEX )
    {
        std::size_t s2 = halfedges.size();
        if( b == s2 )
        {
            halfedges.push_back( a );
        }
        else if( b < s2 )
        {
            halfedges[b] = a;
        }
        else
        {
            throw std::runtime_error( "Cannot link edge" );
        }
    }
}


#endif /* PCBNEW_RATSNEST_DELAUNEY_H_ */