delaunay.c

/*
 *  Copyright 2016 xyzxqs (xyzxqs@gmail.com)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */


//
// Created by xyzxqs (xyzxqs@gmail.com) on 9/22/16.
//

#include <stdint.h>
#include <math.h>
#include <stdlib.h>
#include <stdbool.h>
#include "delaunay.h"
#include "_log.h"
#include "delaunay_internal.h"


/////////////////////

//Intrinsically, this func is sort, but result_size > vertices_size
void triangulate(Point *vertices, int size_vertices, int w, int h, PNode *head) {
    TriNet *triNet = net_init(vertices, size_vertices, w, h);
    for (int i = 0; i < size_vertices; ++i) {
        trinet_point_add(i, triNet);
    }
    Triangle **pTriangle = triNet->triangles_heads;
    for (int i = 0, n = triNet->length; i < n; i++) {
        Triangle *arc_head = pTriangle[i];
        for (Triangle *p = arc_head->next; p != NULL;) {
            int v1 = p->subtense->arrow_vertex;
            int v2 = p->subtense->nock_vertex;
            pnode_add(head, i);
            pnode_add(head, v1);
            pnode_add(head, v2);
            p = p->next;
        }
    }
    net_free(triNet);
}

TriNet *net_init(Point *vertices, int size_vertices, int w, int h) {
    TriNet *triNet = (TriNet *) malloc(sizeof(TriNet));
    triNet->length = size_vertices + 4;
    triNet->arcs_heads = (Arc **) malloc(sizeof(Arc *) * (size_vertices + 4));
    triNet->triangles_heads = (Triangle **) malloc(sizeof(Triangle *) * (size_vertices + 4));
    triNet->vertices = vertices;

    for (int i = 0; i < size_vertices + 4; ++i) {
        (triNet->arcs_heads)[i] = (Arc *) malloc(sizeof(Arc));
        (triNet->arcs_heads)[i]->next = NULL;
        (triNet->arcs_heads)[i]->arrow_vertex = -1;
        (triNet->triangles_heads)[i] = (Triangle *) malloc(sizeof(Triangle));
        (triNet->triangles_heads)[i]->next = NULL;
        (triNet->triangles_heads)[i]->subtense = NULL;
    }

    Point *p = &(vertices[size_vertices]);
    p->x = 0;
    p->y = 0;

    p = &(vertices[size_vertices + 1]);
    p->x = 0;
    p->y = h - 1;

    p = &(vertices[size_vertices + 2]);
    p->x = w - 1;
    p->y = 0;

    p = &(vertices[size_vertices + 3]);
    p->x = w - 1;
    p->y = h - 1;

#if DEBUG_DELAUNAY
    for (int i = 0; i < size_vertices + 4; ++i) {
        _log_i("delaunay", "-->node%d:(%d,%d)", i, vertices[i].x, vertices[i].y);
    }
#endif
    arc_add(triNet, size_vertices + 1, size_vertices);
    arc_add(triNet, size_vertices + 2, size_vertices);
    arc_add(triNet, size_vertices + 2, size_vertices + 1);
    arc_add(triNet, size_vertices + 3, size_vertices + 1);
    arc_add(triNet, size_vertices + 3, size_vertices + 2);

    triangle_add(size_vertices + 2,
                 get_arc(triNet->arcs_heads[size_vertices + 1], size_vertices),
                 triNet);
    triangle_add(size_vertices + 3,
                 get_arc(triNet->arcs_heads[size_vertices + 2], size_vertices + 1),
                 triNet);
    return triNet;
}

void trinet_point_add(const int target_point_index, const TriNet *triNet) {
#if DEBUG_DELAUNAY
    _log_d("delaunay", "start trinet_point_add node %d", target_point_index);
    _print_triNet(triNet);
    _log_d("delaunay", "遍历图中已有三角形,获取影响三角形链表");
#endif
    AngNode *angs_head = (AngNode *) malloc(sizeof(AngNode));
    angs_head->subtense = NULL;
    angs_head->vertex = -1;
    angs_head->next = NULL;
    for (int vertex = 0, n = triNet->length; vertex < n; vertex++) {
        Triangle *head = triNet->triangles_heads[vertex];
        if (head != NULL) {
            for (Triangle *p = head->next; p != NULL;) {
                if (in_circumcircle(target_point_index, vertex, p->subtense, triNet)) {
                    angnode_add(angs_head, vertex, p->subtense);
                }
                p = p->next;
            }

        } else {
            _log_e("delaunay", "point_add: head can not be NULL");
        }
    }
#if DEBUG_DELAUNAY
    _print_angnode(angs_head, triNet);
    _log_d("delaunay", "遍历影响三角形链表，删除影响三角形，获取公共边链表,非公共边链表和待连接点链表");
#endif
    ArcNode *arcs_head = (ArcNode *) malloc(sizeof(ArcNode));
    arcs_head->next = NULL;
    arcs_head->arc = NULL;

    PNode *points_head = (PNode *) malloc(sizeof(PNode));
    points_head->next = NULL;
    points_head->index = -1;

#if DEBUG_DELAUNAY
    _log_d("delaunay", "->1.把三角形的<点边>换成<边边边>,无重复添加待连接点链表，同时删除影响三角形");
#endif
    for (AngNode *p = angs_head->next; p != NULL;) {
        triangle_remove(p->vertex, p->subtense, triNet);
        int nock = p->subtense->nock_vertex;
        int arrow = p->subtense->arrow_vertex;
        arcnode_add(arcs_head, p->subtense);
        Arc *head = (triNet->arcs_heads)[p->vertex];
        pnode_unique_add(points_head, p->vertex);
        for (Arc *pa = head->next; pa != NULL;) {
            if (pa->arrow_vertex == arrow) {
                arcnode_add(arcs_head, pa);
                pnode_unique_add(points_head, pa->arrow_vertex);
            }
            if (pa->arrow_vertex == nock) {
                arcnode_add(arcs_head, pa);
                pnode_unique_add(points_head, pa->arrow_vertex);
            }
            pa = pa->next;
        }
        p = p->next;
    }
    angnode_free(angs_head);
    free(angs_head);
    angs_head = NULL;
#if DEBUG_DELAUNAY
    _print_pointnode(points_head);
    _print_triNet(triNet);
    _log_d("delaunay", "->2.找到公共边链表");
#endif
    ArcNode *comm_arcs_head = (ArcNode *) malloc(sizeof(ArcNode));
    comm_arcs_head->next = NULL;
    comm_arcs_head->arc = NULL;
    for (ArcNode *p = arcs_head->next; p != NULL;) {
        for (ArcNode *np = p->next; np != NULL;) {
            if (p->arc == np->arc) {
                arcnode_add(comm_arcs_head, p->arc);
            }
            np = np->next;
        }
        p = p->next;
    }
#if DEBUG_DELAUNAY
    _print_arcnode(comm_arcs_head);
    _log_d("delaunay", "->3.找到非公共边链表");
#endif
    for (ArcNode *p = comm_arcs_head->next; p != NULL;) {
        for (ArcNode *pd = arcs_head->next; pd != NULL;) {
            if (pd->arc == p->arc) {
                arcnode_remove(arcs_head, pd->arc);
                break;
            }
            pd = pd->next;
        }
        p = p->next;
    }
#if DEBUG_DELAUNAY
    _print_arcnode(arcs_head);
    _log_d("delaunay", "遍历公共边链表删除公共边");
#endif
    for (ArcNode *p = comm_arcs_head->next; p != NULL;) {
        Arc *ah = triNet->arcs_heads[p->arc->nock_vertex];
        arc_remove(ah, p->arc->arrow_vertex);
        p = p->next;
    }
    arcnode_free(comm_arcs_head);
    free(comm_arcs_head);
    comm_arcs_head = NULL;
#if DEBUG_DELAUNAY
    _print_triNet(triNet);
    _log_d("delaunay", "遍历待连接点链表添加新的边");
#endif
    for (PNode *p = points_head->next; p != NULL;) {
        arc_add(triNet, target_point_index, p->index);
        p = p->next;
    }
    pnode_free(points_head);
    free(points_head);
    points_head = NULL;
#if DEBUG_DELAUNAY
    _print_triNet(triNet);
    _log_d("delaunay", "遍历非重复边链表添加三角形并获取新三角形链表");
#endif

#if USE_LOP
    AngNode *new_ang_head = (AngNode *) malloc(sizeof(AngNode));
    new_ang_head->next = NULL;
    new_ang_head->vertex = -1;
    new_ang_head->subtense = NULL;
#endif

    for (ArcNode *p = arcs_head->next; p != NULL;) {
        triangle_add(target_point_index, p->arc, triNet);
#if USE_LOP
        angnode_add(new_ang_head, target_point_index, p->arc);
#endif
        p = p->next;
    }
    arcnode_free(arcs_head);
    free(arcs_head);
    arcs_head = NULL;
#if USE_LOP
#if DEBUG_DELAUNAY
    _print_angnode(new_ang_head, triNet);
    _print_triNet(triNet);
    _log_d("delaunay", "遍历新获取三角形链表进行LOP优化");
#endif
    for (AngNode *p = new_ang_head->next; p != NULL;) {
        LOP(p, triNet);
        p = p->next;
    }
    angnode_free(new_ang_head);
    free(new_ang_head);
    new_ang_head = NULL;
#endif

#if DEBUG_DELAUNAY
    _log_d("delaunay", "end trinet_point_add node %d", target_point_index);
#endif
}

void arc_add(const TriNet *net, int start_vertex, int end_vertices) {
    Arc *head = net->arcs_heads[start_vertex];
    if (head == NULL) {
        _log_e("delaunay", "arc_add: head can not be NULL");
    } else {
        Arc *arc = (Arc *) malloc(sizeof(Arc));
        arc->nock_vertex = start_vertex;
        arc->arrow_vertex = end_vertices;
        arc->next = head->next;
        head->next = arc;
    }
}

void arc_remove(Arc *head, int end_vertices) {
    if (head == NULL) {
        _log_e("delaunay", "arc_remove: can not be NULL");
        return;
    } else {
        for (Arc *p = head; p->next != NULL;) {
            if (p->next->arrow_vertex == end_vertices) {
                Arc *pn = p->next;
                p->next = pn->next;
                free(pn);
            } else {
                p = p->next;
            }
        }
    }
}

void triangle_add(int index_vertex, Arc *subtense, const TriNet *net) {
    Triangle *head = net->triangles_heads[index_vertex];
    if (head == NULL) {
        _log_e("delaunay", "triangle_add head can not be NULL");
        return;
    } else {
        Triangle *angle = (Triangle *) malloc(sizeof(Triangle));
        angle->subtense = subtense;
        angle->next = head->next;
        head->next = angle;
    }
}

void triangle_remove(int index_vertex, Arc *subtense, const TriNet *net) {
    Triangle *head = net->triangles_heads[index_vertex];
    for (Triangle *p = head; p->next != NULL;) {
        if (p->next->subtense == subtense) {
            Triangle *pn = p->next;
            p->next = pn->next;
            free(pn);
        } else {
            p = p->next;
        }
    }
}

void arces_free(Arc *head) {
    if (head == NULL) {
        _log_e("delaunay", "arces_free: head can not be NULL");
        return;
    } else {
        Arc *pn = NULL;
        for (Arc *p = head; p->next != NULL;) {
            pn = p->next;
            p->next = pn->next;
            free(pn);
        }
    }
}

void triangles_free(Triangle *head) {
    if (head == NULL) {
        _log_e("delaunay", "arces_free: head can not be NULL");
        return;
    } else {
        Triangle *pn = NULL;
        for (Triangle *p = head; p->next != NULL;) {
            pn = p->next;
            p->next = pn->next;
            free(pn);
        }
    }
}

void net_free(TriNet *net) {
    for (int i = 0, n = net->length; i < n; ++i) {
        arces_free(net->arcs_heads[i]);
        free(net->arcs_heads[i]);
        triangles_free(net->triangles_heads[i]);
        free(net->triangles_heads[i]);
    }
    free(net);
}

bool in_circumcircle(const int target_point_index, const int vertex, const Arc *arc,
                     const TriNet *net) {
    int arrow = arc->arrow_vertex;
    int nock = arc->nock_vertex;
    const Point p = net->vertices[target_point_index];
    const Point v1 = net->vertices[vertex];
    const Point v2 = net->vertices[arrow];
    const Point v3 = net->vertices[nock];

    float w = 2 * (v2.x - v1.x);
    float h = 2 * (v2.y - v1.y);
    float a = 2 * (v3.x - v2.x);
    float t = 2 * (v3.y - v2.y);
    float f = powf(v2.x, 2.0f) + powf(v2.y, 2.0f);
    float f_ = powf(v1.x, 2.0f) + powf(v1.y, 2.0f) - f;
    float f__ = f - powf(v3.x, 2.0f) - powf(v3.y, 2.0f);
    float u = (f__ * h - f_ * t) / (t * w - h * a);
    float c = (w * f__ - a * f_) / (a * h - w * t);
    float k = powf(v1.x - u, 2.0f) + powf(v1.y - c, 2.0f);

#if DEBUG_DELAUNAY
    bool in_c = powf(p.x - u, 2.0f) + powf(p.y - c, 2.0f) < k;
    _log_d("delaunay", "in_circumcircle :(%d) <%d, %d, %d>|%s", target_point_index,
           vertex, nock, arrow, in_c ? "true" : "false");
    return in_c;
#else
    return powf(p.x - u, 2.0f) + powf(p.y - c, 2.0f) < k;
#endif
}

#if USE_LOP

void LOP(AngNode *target_arc, const TriNet *net) {
    //Unwanted
    _log_w("delaunay", "LOP not impl");
}

#endif

void angnode_add(AngNode *head, int vertex, Arc *subtense) {
    if (head == NULL) {
        _log_e("delaunay", "angnode_add: head can not be NULL");
        return;
    } else {
        AngNode *node = (AngNode *) malloc(sizeof(AngNode));
        node->vertex = vertex;
        node->subtense = subtense;
        node->next = head->next;
        head->next = node;
    }
}

void angnode_free(AngNode *head) {
    if (head == NULL) {
        _log_e("delaunay", "angnode_free: head can not be NULL");
    } else {
        AngNode *pn = NULL;
        for (AngNode *p = head; p->next != NULL;) {
            pn = p->next;
            p->next = pn->next;
            free(pn);
        }
    }
}

void arcnode_add(ArcNode *head, Arc *arc) {
    if (head == NULL) {
        _log_e("delaunay", "arcnode_add: head can not be NULL");
        return;
    } else {
        ArcNode *node = (ArcNode *) malloc(sizeof(ArcNode));
        node->arc = arc;
        node->next = head->next;
        head->next = node;
    }
}

void arcnode_remove(ArcNode *head, Arc *arc) {
    if (head == NULL) {
        _log_e("delaunay", "arcnode_remove: head can not be NULL");
        return;
    } else {
        for (ArcNode *p = head; p->next != NULL;) {
            if (p->next->arc == arc) {
                ArcNode *pn = p->next;
                p->next = pn->next;
                free(pn);
            } else {
                p = p->next;
            }
        }
    }
}

void arcnode_free(ArcNode *head) {
    if (head == NULL) {
        _log_e("delaunay", "arcnode_free: head can not be NULL");
        return;
    } else {
        ArcNode *pn = NULL;
        for (ArcNode *p = head; p->next != NULL;) {
            pn = p->next;
            p->next = pn->next;
            free(pn);
        }
    }
}

void pnode_unique_add(PNode *head, int index) {
    bool has = false;
    for (PNode *p = head->next; p != NULL;) {
        if (p->index == index) {
            has = true;
            break;
        }
        p = p->next;
    }
    if (!has) {
        PNode *node = (PNode *) malloc(sizeof(PNode));
        node->index = index;
        node->next = head->next;
        head->next = node;
    }
}

void pnode_free(PNode *head) {
    if (head == NULL) {
        _log_e("delaunay", "arcnode_free: head can not be NULL");
        return;
    } else {
        PNode *pn = NULL;
        for (PNode *p = head; p->next != NULL;) {
            pn = p->next;
            p->next = pn->next;
            free(pn);
        }
    }
}

Arc *get_arc(Arc *head, int end) {
    if (head == NULL) {
        _log_e("delaunay", "get_arc: head can not be NULL");
        return NULL;
    } else {
        for (Arc *p = head->next; p != NULL;) {
            if (p->arrow_vertex == end) {
                return p;
            }
            p = p->next;
        }
    }
    _log_e("delaunay", "get_arc: not found");
    return NULL;
}

void pnode_add(PNode *head, int index) {
    if (head == NULL) {
        _log_e("delaunay", "pnode_add: head can not be NULL");
    } else {
        PNode *node = (PNode *) malloc(sizeof(PNode));
        node->index = index;
        node->next = head->next;
        head->next = node;
    }
}

#if DEBUG_DELAUNAY

void _print_triNet(const TriNet *triNet) {
    _log_d("delaunay", "start _print_triNet");
    _log_d("delaunay", "arc: ");
    Arc **pArc = triNet->arcs_heads;
    for (int i = 0, n = triNet->length; i < n; i++) {
        _log_d("delaunay", "TriNet--> node %d:", i);
        Arc *arc_head = pArc[i];
        for (Arc *p = arc_head->next; p != NULL;) {
            _log_d("delaunay", "TriNet-------> %d-%d  :%x", i, p->arrow_vertex, p);
            p = p->next;
        }
    }
    _log_d("delaunay", "triangle: ");
    Triangle **pTriangle = triNet->triangles_heads;
    for (int i = 0, n = triNet->length; i < n; i++) {
        _log_d("delaunay", "TriNet--> node %d:", i);
        Triangle *arc_head = pTriangle[i];
        for (Triangle *p = arc_head->next; p != NULL;) {
            int v1 = p->subtense->arrow_vertex;
            int v2 = p->subtense->nock_vertex;
            _log_d("delaunay", "TriNet-------> <%d, %d, %d>", i, v2, v1);
            p = p->next;
        }
    }
    _log_d("delaunay", "end _print_triNet");
}

void _print_angnode(const AngNode *head, const TriNet *net) {
    _log_d("delaunay", "start _print_angnode");
    if (head != NULL) {
        for (AngNode *p = head->next; p != NULL;) {
            _log_d("delaunay", "<%d,%d,%d>",
                   p->vertex, p->subtense->nock_vertex, p->subtense->arrow_vertex);
            p = p->next;
        }
    } else {
        _log_e("delaunay", "_print_angnode...");
    }
    _log_d("delaunay", "end _print_angnode");
}

void _print_arcnode(const ArcNode *head) {
    _log_d("delaunay", "start _print_arcnode");
    if (head != NULL) {
        for (ArcNode *p = head->next; p != NULL;) {
            _log_d("delaunay", "%d-%d", p->arc->nock_vertex, p->arc->arrow_vertex);
            p = p->next;
        }
    } else {
        _log_e("delaunay", "_print_arcnode...");
    }
    _log_d("delaunay", "end _print_arcnode");
}

void _print_pointnode(const PNode *head) {
    _log_d("delaunay", "start _print_pointnode");
    if (head != NULL) {
        for (PNode *p = head->next; p != NULL;) {
            _log_d("delaunay", "(%d)", p->index);
            p = p->next;
        }
    } else {
        _log_e("delaunay", "_print_pointnode...");
    }
    _log_d("delaunay", "end _print_pointnode");
}

#endif