trapezoid.c

Go to the documentation of this file.

 
/*************************************************************************
 * Copyright (c) 2011 AT&T Intellectual Property
 * All rights reserved. This program and the accompanying materials
 * are made available under the terms of the Eclipse Public License v2.0
 * which accompanies this distribution, and is available at
 * https://www.eclipse.org/org/documents/epl-2.0/EPL-2.0.html
 *
 * Contributors: Details at https://graphviz.org
 *************************************************************************/
 
 
#include "config.h"
#include <string.h>
#include <assert.h>
#include <float.h>
#include <stdbool.h>
#include <stddef.h>
#include <stdint.h>
#include <stdio.h>
#include <math.h>
#include <common/geom.h>
#include <common/types.h>
#include <ortho/trap.h>
#include <util/alloc.h>
#include <util/gv_math.h>
#include <util/list.h>
#include <util/unreachable.h>
#include <util/unused.h>
 
/* Node types */
 
#define T_X     1
#define T_Y     2
#define T_SINK  3
 
#define FIRSTPT 1       /* checking whether pt. is inserted */
#define LASTPT  2
 
#define S_LEFT 1        /* for merge-direction */
#define S_RIGHT 2
 
static double cross(pointf v0, pointf v1, pointf v2) {
  return (v1.x - v0.x) * (v2.y - v0.y) - (v1.y - v0.y) * (v2.x - v0.x);
}
 
typedef struct {
  int nodetype;         /* Y-node or S-node */
  size_t segnum;
  pointf yval;
  size_t trnum;
  size_t parent;        
  size_t left, right;   
} qnode_t;
 
typedef LIST(qnode_t) qnodes_t;
 
/* Return a new node to be added into the query tree */
static size_t newnode(qnodes_t *qs) {
  LIST_APPEND(qs, (qnode_t){0});
  return LIST_SIZE(qs) - 1;
}
 
/* Return a free trapezoid */
static size_t newtrap(traps_t *tr) {
  LIST_APPEND(tr, (trap_t){0});
  return LIST_SIZE(tr) - 1;
}
 
static pointf max_(pointf v0, pointf v1) {
  if (v0.y > v1.y + C_EPS)
    return v0;
  if (fp_equal(v0.y, v1.y)) {
      if (v0.x > v1.x + C_EPS)
        return v0;
      return v1;
    }
  return v1;
}
 
static pointf min_(pointf v0, pointf v1) {
  if (v0.y < v1.y - C_EPS)
    return v0;
  if (fp_equal(v0.y, v1.y)) {
      if (v0.x < v1.x)
        return v0;
      return v1;
    }
  return v1;
}
 
static bool greater_than_equal_to(pointf v0, pointf v1) {
  return greater_than(v0, v1) || equal_to(v0, v1);
}
 
static bool less_than(pointf v0, pointf v1) {
  return !greater_than_equal_to(v0, v1);
}
 
/* Initialize the query structure (Q) and the trapezoid table (T)
 * when the first segment is added to start the trapezoidation. The
 * query-tree starts out with 4 trapezoids, one S-node and 2 Y-nodes
 *
 *                4
 *   -----------------------------------
 *                \
 *      1          \        2
 *                  \
 *   -----------------------------------
 *                3
 */
 
static size_t init_query_structure(size_t segnum, segment_t *seg, traps_t *tr,
                                   qnodes_t *qs) {
  segment_t *s = &seg[segnum];
 
  const size_t i1 = newnode(qs);
  LIST_AT(qs, i1)->nodetype = T_Y;
  LIST_AT(qs, i1)->yval = max_(s->v0, s->v1); // root
  const size_t root = i1;
 
  const size_t i2 = newnode(qs);
  LIST_AT(qs, i1)->right = i2;
  LIST_AT(qs, i2)->nodetype = T_SINK;
  LIST_AT(qs, i2)->parent = i1;
 
  const size_t i3 = newnode(qs);
  LIST_AT(qs, i1)->left = i3;
  LIST_AT(qs, i3)->nodetype = T_Y;
  LIST_AT(qs, i3)->yval = min_(s->v0, s->v1); // root
  LIST_AT(qs, i3)->parent = i1;
 
  const size_t i4 = newnode(qs);
  LIST_AT(qs, i3)->left = i4;
  LIST_AT(qs, i4)->nodetype = T_SINK;
  LIST_AT(qs, i4)->parent = i3;
 
  const size_t i5 = newnode(qs);
  LIST_AT(qs, i3)->right = i5;
  LIST_AT(qs, i5)->nodetype = T_X;
  LIST_AT(qs, i5)->segnum = segnum;
  LIST_AT(qs, i5)->parent = i3;
 
  const size_t i6 = newnode(qs);
  LIST_AT(qs, i5)->left = i6;
  LIST_AT(qs, i6)->nodetype = T_SINK;
  LIST_AT(qs, i6)->parent = i5;
 
  const size_t i7 = newnode(qs);
  LIST_AT(qs, i5)->right = i7;
  LIST_AT(qs, i7)->nodetype = T_SINK;
  LIST_AT(qs, i7)->parent = i5;
 
  const size_t t1 = newtrap(tr); // middle left
  const size_t t2 = newtrap(tr); // middle right
  const size_t t3 = newtrap(tr); // bottom-most
  const size_t t4 = newtrap(tr); // topmost
 
  LIST_AT(tr, t1)->hi = LIST_GET(qs, i1).yval;
  LIST_AT(tr, t2)->hi = LIST_GET(qs, i1).yval;
  LIST_AT(tr, t4)->lo = LIST_GET(qs, i1).yval;
  LIST_AT(tr, t1)->lo = LIST_GET(qs, i3).yval;
  LIST_AT(tr, t2)->lo = LIST_GET(qs, i3).yval;
  LIST_AT(tr, t3)->hi = LIST_GET(qs, i3).yval;
  LIST_AT(tr, t4)->hi.y = DBL_MAX;
  LIST_AT(tr, t4)->hi.x = DBL_MAX;
  LIST_AT(tr, t3)->lo.y = -DBL_MAX;
  LIST_AT(tr, t3)->lo.x = -DBL_MAX;
  LIST_AT(tr, t1)->rseg = segnum;
  LIST_AT(tr, t2)->lseg = segnum;
  LIST_AT(tr, t1)->u0 = t4;
  LIST_AT(tr, t2)->u0 = t4;
  LIST_AT(tr, t1)->d0 = t3;
  LIST_AT(tr, t2)->d0 = t3;
  LIST_AT(tr, t4)->d0 = t1;
  LIST_AT(tr, t3)->u0 = t1;
  LIST_AT(tr, t4)->d1 = t2;
  LIST_AT(tr, t3)->u1 = t2;
 
  LIST_AT(tr, t1)->sink = i6;
  LIST_AT(tr, t2)->sink = i7;
  LIST_AT(tr, t3)->sink = i4;
  LIST_AT(tr, t4)->sink = i2;
 
  LIST_AT(tr, t1)->is_valid = true;
  LIST_AT(tr, t2)->is_valid = true;
  LIST_AT(tr, t3)->is_valid = true;
  LIST_AT(tr, t4)->is_valid = true;
 
  LIST_AT(qs, i2)->trnum = t4;
  LIST_AT(qs, i4)->trnum = t3;
  LIST_AT(qs, i6)->trnum = t1;
  LIST_AT(qs, i7)->trnum = t2;
 
  s->is_inserted = true;
  return root;
}
 
/* Return true if the vertex v is to the left of line segment no.
 * segnum. Takes care of the degenerate cases when both the vertices
 * have the same y--cood, etc.
 */
static bool is_left_of(size_t segnum, segment_t *seg, pointf *v) {
  segment_t *s = &seg[segnum];
  double area;
 
  if (greater_than(s->v1, s->v0)) { // segment going upwards
      if (fp_equal(s->v1.y, v->y)) {
          if (v->x < s->v1.x)
            area = 1.0;
          else
            area = -1.0;
        }
      else if (fp_equal(s->v0.y, v->y)) {
          if (v->x < s->v0.x)
            area = 1.0;
          else
            area = -1.0;
        }
      else
        area = cross(s->v0, s->v1, *v);
    }
  else                          /* v0 > v1 */
    {
      if (fp_equal(s->v1.y, v->y)) {
          if (v->x < s->v1.x)
            area = 1.0;
          else
            area = -1.0;
        }
      else if (fp_equal(s->v0.y, v->y)) {
          if (v->x < s->v0.x)
            area = 1.0;
          else
            area = -1.0;
        }
      else
        area = cross(s->v1, s->v0, *v);
    }
 
  return area > 0.0;
}
 
/* Returns true if the corresponding endpoint of the given segment is */
/* already inserted into the segment tree. Use the simple test of */
/* whether the segment which shares this endpoint is already inserted */
static bool inserted(size_t segnum, segment_t *seg, int whichpt) {
  if (whichpt == FIRSTPT)
    return seg[seg[segnum].prev].is_inserted;
  else
    return seg[seg[segnum].next].is_inserted;
}
 
/* This is query routine which determines which trapezoid does the
 * point v lie in. The return value is the trapezoid number.
 */
static size_t locate_endpoint(pointf *v, pointf *vo, size_t r, segment_t *seg,
                           qnodes_t *qs) {
  qnode_t *rptr = LIST_AT(qs, r);
 
  switch (rptr->nodetype) {
    case T_SINK:
      return rptr->trnum;
 
    case T_Y:
      if (greater_than(*v, rptr->yval)) // above
        return locate_endpoint(v, vo, rptr->right, seg, qs);
      if (equal_to(*v, rptr->yval)) { // the point is already inserted
          if (greater_than(*vo, rptr->yval)) // above
            return locate_endpoint(v, vo, rptr->right, seg, qs);
          return locate_endpoint(v, vo, rptr->left, seg, qs); // below
        }
      return locate_endpoint(v, vo, rptr->left, seg, qs); // below
 
    case T_X:
      if (equal_to(*v, seg[rptr->segnum].v0) ||
          equal_to(*v, seg[rptr->segnum].v1)) {
          if (fp_equal(v->y, vo->y)) { // horizontal segment
              if (vo->x < v->x)
                return locate_endpoint(v, vo, rptr->left, seg, qs); /* left */
              return locate_endpoint(v, vo, rptr->right, seg, qs); // right
            }
 
          if (is_left_of(rptr->segnum, seg, vo))
            return locate_endpoint(v, vo, rptr->left, seg, qs); /* left */
          return locate_endpoint(v, vo, rptr->right, seg, qs); // right
        }
      if (is_left_of(rptr->segnum, seg, v))
        return locate_endpoint(v, vo, rptr->left, seg, qs); /* left */
      return locate_endpoint(v, vo, rptr->right, seg, qs); // right
 
    default:
      break;
    }
    UNREACHABLE();
}
 
/* Thread in the segment into the existing trapezoidation. The
 * limiting trapezoids are given by tfirst and tlast (which are the
 * trapezoids containing the two endpoints of the segment. Merges all
 * possible trapezoids which flank this segment and have been recently
 * divided because of its insertion
 */
static void merge_trapezoids(size_t segnum, size_t tfirst, size_t tlast,
                             int side, traps_t *tr, qnodes_t *qs) {
  /* First merge polys on the LHS */
  size_t t = tfirst;
  while (is_valid_trap(t) &&
         greater_than_equal_to(LIST_GET(tr, t).lo, LIST_GET(tr, tlast).lo)) {
      size_t tnext;
      bool cond;
      if (side == S_LEFT)
        cond = (is_valid_trap(tnext = LIST_GET(tr, t).d0) && LIST_GET(tr, tnext).rseg == segnum) ||
                (is_valid_trap(tnext = LIST_GET(tr, t).d1) && LIST_GET(tr, tnext).rseg == segnum);
      else
        cond = (is_valid_trap(tnext = LIST_GET(tr, t).d0) && LIST_GET(tr, tnext).lseg == segnum) ||
                (is_valid_trap(tnext = LIST_GET(tr, t).d1) && LIST_GET(tr, tnext).lseg == segnum);
 
      if (cond)
        {
          if (LIST_GET(tr, t).lseg == LIST_GET(tr, tnext).lseg &&
              LIST_GET(tr, t).rseg == LIST_GET(tr, tnext).rseg) // good neighbors
            {                                 /* merge them */
              /* Use the upper node as the new node i.e. t */
 
              const size_t ptnext = LIST_GET(qs, LIST_GET(tr, tnext).sink).parent;
 
              if (LIST_GET(qs, ptnext).left == LIST_GET(tr, tnext).sink)
                LIST_AT(qs, ptnext)->left = LIST_GET(tr, t).sink;
              else
                LIST_AT(qs, ptnext)->right = LIST_GET(tr, t).sink; // redirect parent
 
 
              /* Change the upper neighbours of the lower trapezoids */
 
              if (is_valid_trap(LIST_AT(tr, t)->d0 = LIST_GET(tr, tnext).d0)) {
                if (LIST_GET(tr, LIST_GET(tr, t).d0).u0 == tnext)
                  LIST_AT(tr, LIST_GET(tr, t).d0)->u0 = t;
                else if (LIST_GET(tr, LIST_GET(tr, t).d0).u1 == tnext)
                  LIST_AT(tr, LIST_GET(tr, t).d0)->u1 = t;
              }
 
              if (is_valid_trap(LIST_AT(tr, t)->d1 = LIST_GET(tr, tnext).d1)) {
                if (LIST_GET(tr, LIST_GET(tr, t).d1).u0 == tnext)
                  LIST_AT(tr, LIST_GET(tr, t).d1)->u0 = t;
                else if (LIST_GET(tr, LIST_GET(tr, t).d1).u1 == tnext)
                  LIST_AT(tr, LIST_GET(tr, t).d1)->u1 = t;
              }
 
              LIST_AT(tr, t)->lo = LIST_GET(tr, tnext).lo;
              LIST_AT(tr, tnext)->is_valid = false; // invalidate the lower
                                            /* trapezium */
            }
          else              /* not good neighbours */
            t = tnext;
        }
      else                  /* do not satisfy the outer if */
        t = tnext;
 
    } /* end-while */
 
}
 
static void update_trapezoid(segment_t *s, segment_t *seg, traps_t *tr,
                             size_t t, size_t tn) {
  if (is_valid_trap(LIST_GET(tr, t).u0) && is_valid_trap(LIST_GET(tr, t).u1))
  {                     /* continuation of a chain from abv. */
    if (is_valid_trap(LIST_GET(tr, t).usave)) { // three upper neighbours
      if (LIST_GET(tr, t).uside == S_LEFT)
      {
        LIST_AT(tr, tn)->u0 = LIST_GET(tr, t).u1;
        LIST_AT(tr, t)->u1 = SIZE_MAX;
        LIST_AT(tr, tn)->u1 = LIST_GET(tr, t).usave;
 
        LIST_AT(tr, LIST_GET(tr, t).u0)->d0 = t;
        LIST_AT(tr, LIST_GET(tr, tn).u0)->d0 = tn;
        LIST_AT(tr, LIST_GET(tr, tn).u1)->d0 = tn;
      }
      else              /* intersects in the right */
      {
        LIST_AT(tr, tn)->u1 = SIZE_MAX;
        LIST_AT(tr, tn)->u0 = LIST_GET(tr, t).u1;
        LIST_AT(tr, t)->u1 = LIST_GET(tr, t).u0;
        LIST_AT(tr, t)->u0 = LIST_GET(tr, t).usave;
 
        LIST_AT(tr, LIST_GET(tr, t).u0)->d0 = t;
        LIST_AT(tr, LIST_GET(tr, t).u1)->d0 = t;
        LIST_AT(tr, LIST_GET(tr, tn).u0)->d0 = tn;
      }
 
      LIST_AT(tr, t)->usave = 0;
      LIST_AT(tr, tn)->usave = 0;
    }
    else                /* No usave.... simple case */
    {
      LIST_AT(tr, tn)->u0 = LIST_GET(tr, t).u1;
      LIST_AT(tr, t)->u1 = SIZE_MAX;
      LIST_AT(tr, tn)->u1 = SIZE_MAX;
      LIST_AT(tr, LIST_GET(tr, tn).u0)->d0 = tn;
    }
  }
  else
  {                     /* fresh seg. or upward cusp */
    const size_t tmp_u = LIST_GET(tr, t).u0;
    size_t td0;
    if (is_valid_trap(td0 = LIST_GET(tr, tmp_u).d0) && is_valid_trap(LIST_GET(tr, tmp_u).d1))
    {           /* upward cusp */
      if (LIST_GET(tr, td0).rseg > 0 && !is_left_of(LIST_GET(tr, td0).rseg, seg, &s->v1))
      {
        LIST_AT(tr, t)->u0 = SIZE_MAX;
        LIST_AT(tr, t)->u1 = SIZE_MAX;
        LIST_AT(tr, tn)->u1 = SIZE_MAX;
        LIST_AT(tr, LIST_GET(tr, tn).u0)->d1 = tn;
      }
      else              /* cusp going leftwards */
      {
        LIST_AT(tr, tn)->u0 = SIZE_MAX;
        LIST_AT(tr, tn)->u1 = SIZE_MAX;
        LIST_AT(tr, t)->u1 = SIZE_MAX;
        LIST_AT(tr, LIST_GET(tr, t).u0)->d0 = t;
      }
    }
    else                /* fresh segment */
    {
      LIST_AT(tr, LIST_GET(tr, t).u0)->d0 = t;
      LIST_AT(tr, LIST_GET(tr, t).u0)->d1 = tn;
    }
  }
}
 
/* Add in the new segment into the trapezoidation and update Q and T
 * structures. First locate the two endpoints of the segment in the
 * Q-structure. Then start from the topmost trapezoid and go down to
 * the  lower trapezoid dividing all the trapezoids in between .
 *
 * @return 0 on success
 */
static WUR int add_segment(size_t segnum, segment_t *seg, traps_t *tr,
                           qnodes_t *qs) {
  segment_t s;
  size_t tfirst, tlast;
  size_t tfirstr = 0, tlastr = 0;
  bool tribot = false;
  bool is_swapped;
  size_t tmptriseg;
 
  s = seg[segnum];
  if (greater_than(s.v1, s.v0)) { // Get higher vertex in v0
      SWAP(&s.v0, &s.v1);
      SWAP(&s.root0, &s.root1);
      is_swapped = true;
    }
  else is_swapped = false;
 
  if (!inserted(segnum, seg, is_swapped ? LASTPT : FIRSTPT))
    /* insert v0 in the tree */
    {
      size_t tmp_d;
 
      const size_t tu = locate_endpoint(&s.v0, &s.v1, s.root0, seg, qs);
      const size_t tl = newtrap(tr); // tl is the new lower trapezoid
      LIST_SET(tr, tl, LIST_GET(tr, tu));
      LIST_AT(tr, tu)->lo = s.v0;
      LIST_AT(tr, tl)->hi = s.v0;
      LIST_AT(tr, tu)->d0 = tl;
      LIST_AT(tr, tu)->d1 = 0;
      LIST_AT(tr, tl)->u0 = tu;
      LIST_AT(tr, tl)->u1 = 0;
 
      if (is_valid_trap(tmp_d = LIST_GET(tr, tl).d0) && LIST_GET(tr, tmp_d).u0 == tu)
        LIST_AT(tr, tmp_d)->u0 = tl;
      if (is_valid_trap(tmp_d = LIST_GET(tr, tl).d0) && LIST_GET(tr, tmp_d).u1 == tu)
        LIST_AT(tr, tmp_d)->u1 = tl;
 
      if (is_valid_trap(tmp_d = LIST_GET(tr, tl).d1) && LIST_GET(tr, tmp_d).u0 == tu)
        LIST_AT(tr, tmp_d)->u0 = tl;
      if (is_valid_trap(tmp_d = LIST_GET(tr, tl).d1) && LIST_GET(tr, tmp_d).u1 == tu)
        LIST_AT(tr, tmp_d)->u1 = tl;
 
      /* Now update the query structure and obtain the sinks for the */
      /* two trapezoids */
 
      const size_t i1 = newnode(qs); // Upper trapezoid sink
      const size_t i2 = newnode(qs); // Lower trapezoid sink
      const size_t sk = LIST_GET(tr, tu).sink;
 
      LIST_AT(qs, sk)->nodetype = T_Y;
      LIST_AT(qs, sk)->yval = s.v0;
      LIST_AT(qs, sk)->segnum = segnum; // not really required … maybe later
      LIST_AT(qs, sk)->left = i2;
      LIST_AT(qs, sk)->right = i1;
 
      LIST_AT(qs, i1)->nodetype = T_SINK;
      LIST_AT(qs, i1)->trnum = tu;
      LIST_AT(qs, i1)->parent = sk;
 
      LIST_AT(qs, i2)->nodetype = T_SINK;
      LIST_AT(qs, i2)->trnum = tl;
      LIST_AT(qs, i2)->parent = sk;
 
      LIST_AT(tr, tu)->sink = i1;
      LIST_AT(tr, tl)->sink = i2;
      tfirst = tl;
    }
  else                          /* v0 already present */
    {       /* Get the topmost intersecting trapezoid */
      tfirst = locate_endpoint(&s.v0, &s.v1, s.root0, seg, qs);
    }
 
 
  if (!inserted(segnum, seg, is_swapped ? FIRSTPT : LASTPT))
    /* insert v1 in the tree */
    {
      size_t tmp_d;
 
      const size_t tu = locate_endpoint(&s.v1, &s.v0, s.root1, seg, qs);
 
      const size_t tl = newtrap(tr); // tl is the new lower trapezoid
      LIST_SET(tr, tl, LIST_GET(tr, tu));
      LIST_AT(tr, tu)->lo = s.v1;
      LIST_AT(tr, tl)->hi = s.v1;
      LIST_AT(tr, tu)->d0 = tl;
      LIST_AT(tr, tu)->d1 = 0;
      LIST_AT(tr, tl)->u0 = tu;
      LIST_AT(tr, tl)->u1 = 0;
 
      if (is_valid_trap(tmp_d = LIST_GET(tr, tl).d0) && LIST_GET(tr, tmp_d).u0 == tu)
        LIST_AT(tr, tmp_d)->u0 = tl;
      if (is_valid_trap(tmp_d = LIST_GET(tr, tl).d0) && LIST_GET(tr, tmp_d).u1 == tu)
        LIST_AT(tr, tmp_d)->u1 = tl;
 
      if (is_valid_trap(tmp_d = LIST_GET(tr, tl).d1) && LIST_GET(tr, tmp_d).u0 == tu)
        LIST_AT(tr, tmp_d)->u0 = tl;
      if (is_valid_trap(tmp_d = LIST_GET(tr, tl).d1) && LIST_GET(tr, tmp_d).u1 == tu)
        LIST_AT(tr, tmp_d)->u1 = tl;
 
      /* Now update the query structure and obtain the sinks for the */
      /* two trapezoids */
 
      const size_t i1 = newnode(qs); // Upper trapezoid sink
      const size_t i2 = newnode(qs); // Lower trapezoid sink
      const size_t sk = LIST_GET(tr, tu).sink;
 
      LIST_AT(qs, sk)->nodetype = T_Y;
      LIST_AT(qs, sk)->yval = s.v1;
      LIST_AT(qs, sk)->segnum = segnum; // not really required … maybe later
      LIST_AT(qs, sk)->left = i2;
      LIST_AT(qs, sk)->right = i1;
 
      LIST_AT(qs, i1)->nodetype = T_SINK;
      LIST_AT(qs, i1)->trnum = tu;
      LIST_AT(qs, i1)->parent = sk;
 
      LIST_AT(qs, i2)->nodetype = T_SINK;
      LIST_AT(qs, i2)->trnum = tl;
      LIST_AT(qs, i2)->parent = sk;
 
      LIST_AT(tr, tu)->sink = i1;
      LIST_AT(tr, tl)->sink = i2;
      tlast = tu;
    }
  else                          /* v1 already present */
    {       /* Get the lowermost intersecting trapezoid */
      tlast = locate_endpoint(&s.v1, &s.v0, s.root1, seg, qs);
      tribot = true;
    }
 
  /* Thread the segment into the query tree creating a new X-node */
  /* First, split all the trapezoids which are intersected by s into */
  /* two */
 
  size_t t = tfirst; // topmost trapezoid
 
  while (is_valid_trap(t) &&
         greater_than_equal_to(LIST_GET(tr, t).lo, LIST_GET(tr, tlast).lo))
                                /* traverse from top to bot */
    {
      const size_t sk = LIST_GET(tr, t).sink;
      const size_t i1 = newnode(qs); // left trapezoid sink
      const size_t i2 = newnode(qs); // right trapezoid sink
 
      LIST_AT(qs, sk)->nodetype = T_X;
      LIST_AT(qs, sk)->segnum = segnum;
      LIST_AT(qs, sk)->left = i1;
      LIST_AT(qs, sk)->right = i2;
 
      LIST_AT(qs, i1)->nodetype = T_SINK; // left trapezoid (use existing one)
      LIST_AT(qs, i1)->trnum = t;
      LIST_AT(qs, i1)->parent = sk;
 
      LIST_AT(qs, i2)->nodetype = T_SINK; // right trapezoid (allocate new)
      const size_t tn = newtrap(tr);
      LIST_AT(qs, i2)->trnum = tn;
      LIST_AT(tr, tn)->is_valid = true;
      LIST_AT(qs, i2)->parent = sk;
 
      if (t == tfirst)
        tfirstr = tn;
      if (equal_to(LIST_GET(tr, t).lo, LIST_GET(tr, tlast).lo))
        tlastr = tn;
 
      LIST_SET(tr, tn, LIST_GET(tr, t));
      LIST_AT(tr, t)->sink = i1;
      LIST_AT(tr, tn)->sink = i2;
      const size_t t_sav = t;
      const size_t tn_sav = tn;
 
      if (!is_valid_trap(LIST_GET(tr, t).d0) &&
          !is_valid_trap(LIST_GET(tr, t).d1)) { // error
          fprintf(stderr, "trapezoid segment construction failed\n");
          return -1;
        }
 
      /* only one trapezoid below. partition t into two and make the */
      /* two resulting trapezoids t and tn as the upper neighbours of */
      /* the sole lower trapezoid */
 
      else if (is_valid_trap(LIST_GET(tr, t).d0) &&
               !is_valid_trap(LIST_GET(tr, t).d1)) { // only one trapezoid below
          update_trapezoid(&s, seg, tr, t, tn);
 
          if (fp_equal(LIST_GET(tr, t).lo.y, LIST_GET(tr, tlast).lo.y) &&
              fp_equal(LIST_GET(tr, t).lo.x, LIST_GET(tr, tlast).lo.x) && tribot)
            {           /* bottom forms a triangle */
 
              if (is_swapped)
                tmptriseg = seg[segnum].prev;
              else
                tmptriseg = seg[segnum].next;
 
              if (tmptriseg > 0 && is_left_of(tmptriseg, seg, &s.v0))
                {
                                /* L-R downward cusp */
                  LIST_AT(tr, LIST_GET(tr, t).d0)->u0 = t;
                  LIST_AT(tr, tn)->d0 = SIZE_MAX;
                  LIST_AT(tr, tn)->d1 = SIZE_MAX;
                }
              else
                {
                                /* R-L downward cusp */
                  LIST_AT(tr, LIST_GET(tr, tn).d0)->u1 = tn;
                  LIST_AT(tr, t)->d0 = SIZE_MAX;
                  LIST_AT(tr, t)->d1 = SIZE_MAX;
                }
            }
          else
            {
              if (is_valid_trap(LIST_GET(tr, LIST_GET(tr, t).d0).u0) &&
                  is_valid_trap(LIST_GET(tr, LIST_GET(tr, t).d0).u1)) {
                  if (LIST_GET(tr, LIST_GET(tr, t).d0).u0 == t) { // passes through LHS
                      LIST_AT(tr, LIST_GET(tr, t).d0)->usave = LIST_GET(tr, LIST_GET(tr, t).d0).u1;
                      LIST_AT(tr, LIST_GET(tr, t).d0)->uside = S_LEFT;
                    }
                  else
                    {
                      LIST_AT(tr, LIST_GET(tr, t).d0)->usave = LIST_GET(tr, LIST_GET(tr, t).d0).u0;
                      LIST_AT(tr, LIST_GET(tr, t).d0)->uside = S_RIGHT;
                    }
                }
              LIST_AT(tr, LIST_GET(tr, t).d0)->u0 = t;
              LIST_AT(tr, LIST_GET(tr, t).d0)->u1 = tn;
            }
 
          t = LIST_GET(tr, t).d0;
        }
 
 
      else if (!is_valid_trap(LIST_GET(tr, t).d0) &&
               is_valid_trap(LIST_GET(tr, t).d1)) { // only one trapezoid below
          update_trapezoid(&s, seg, tr, t, tn);
 
          if (fp_equal(LIST_GET(tr, t).lo.y, LIST_GET(tr, tlast).lo.y) &&
              fp_equal(LIST_GET(tr, t).lo.x, LIST_GET(tr, tlast).lo.x) && tribot)
            {           /* bottom forms a triangle */
 
              if (is_swapped)
                tmptriseg = seg[segnum].prev;
              else
                tmptriseg = seg[segnum].next;
 
              if (tmptriseg > 0 && is_left_of(tmptriseg, seg, &s.v0))
                {
                  /* L-R downward cusp */
                  LIST_AT(tr, LIST_GET(tr, t).d1)->u0 = t;
                  LIST_AT(tr, tn)->d0 = SIZE_MAX;
                  LIST_AT(tr, tn)->d1 = SIZE_MAX;
                }
              else
                {
                  /* R-L downward cusp */
                  LIST_AT(tr, LIST_GET(tr, tn).d1)->u1 = tn;
                  LIST_AT(tr, t)->d0 = SIZE_MAX;
                  LIST_AT(tr, t)->d1 = SIZE_MAX;
                }
            }
          else
            {
              if (is_valid_trap(LIST_GET(tr, LIST_GET(tr, t).d1).u0) &&
                  is_valid_trap(LIST_GET(tr, LIST_GET(tr, t).d1).u1)) {
                  if (LIST_GET(tr, LIST_GET(tr, t).d1).u0 == t) { // passes through LHS
                      LIST_AT(tr, LIST_GET(tr, t).d1)->usave = LIST_GET(tr, LIST_GET(tr, t).d1).u1;
                      LIST_AT(tr, LIST_GET(tr, t).d1)->uside = S_LEFT;
                    }
                  else
                    {
                      LIST_AT(tr, LIST_GET(tr, t).d1)->usave = LIST_GET(tr, LIST_GET(tr, t).d1).u0;
                      LIST_AT(tr, LIST_GET(tr, t).d1)->uside = S_RIGHT;
                    }
                }
              LIST_AT(tr, LIST_GET(tr, t).d1)->u0 = t;
              LIST_AT(tr, LIST_GET(tr, t).d1)->u1 = tn;
            }
 
          t = LIST_GET(tr, t).d1;
        }
 
      /* two trapezoids below. Find out which one is intersected by */
      /* this segment and proceed down that one */
 
      else
        {
          double y0, yt;
          pointf tmppt;
          size_t tnext;
          bool i_d0 = false;
          if (fp_equal(LIST_GET(tr, t).lo.y, s.v0.y)) {
              if (LIST_GET(tr, t).lo.x > s.v0.x)
                i_d0 = true;
            }
          else
            {
              tmppt.y = y0 = LIST_GET(tr, t).lo.y;
              yt = (y0 - s.v0.y)/(s.v1.y - s.v0.y);
              tmppt.x = s.v0.x + yt * (s.v1.x - s.v0.x);
 
              if (less_than(tmppt, LIST_GET(tr, t).lo))
                i_d0 = true;
            }
 
          /* check continuity from the top so that the lower-neighbour */
          /* values are properly filled for the upper trapezoid */
 
          update_trapezoid(&s, seg, tr, t, tn);
 
          if (fp_equal(LIST_GET(tr, t).lo.y, LIST_GET(tr, tlast).lo.y) &&
              fp_equal(LIST_GET(tr, t).lo.x, LIST_GET(tr, tlast).lo.x) && tribot)
            {
              /* this case arises only at the lowest trapezoid.. i.e.
                 tlast, if the lower endpoint of the segment is
                 already inserted in the structure */
 
              LIST_AT(tr, LIST_GET(tr, t).d0)->u0 = t;
              LIST_AT(tr, LIST_GET(tr, t).d0)->u1 = SIZE_MAX;
              LIST_AT(tr, LIST_GET(tr, t).d1)->u0 = tn;
              LIST_AT(tr, LIST_GET(tr, t).d1)->u1 = SIZE_MAX;
 
              LIST_AT(tr, tn)->d0 = LIST_GET(tr, t).d1;
              LIST_AT(tr, t)->d1 = SIZE_MAX;
              LIST_AT(tr, tn)->d1 = SIZE_MAX;
 
              tnext = LIST_GET(tr, t).d1;
            }
          else if (i_d0)
                                /* intersecting d0 */
            {
              LIST_AT(tr, LIST_GET(tr, t).d0)->u0 = t;
              LIST_AT(tr, LIST_GET(tr, t).d0)->u1 = tn;
              LIST_AT(tr, LIST_GET(tr, t).d1)->u0 = tn;
              LIST_AT(tr, LIST_GET(tr, t).d1)->u1 = SIZE_MAX;
 
              /* new code to determine the bottom neighbours of the */
              /* newly partitioned trapezoid */
 
              LIST_AT(tr, t)->d1 = SIZE_MAX;
 
              tnext = LIST_GET(tr, t).d0;
            }
          else                  /* intersecting d1 */
            {
              LIST_AT(tr, LIST_GET(tr, t).d0)->u0 = t;
              LIST_AT(tr, LIST_GET(tr, t).d0)->u1 = SIZE_MAX;
              LIST_AT(tr, LIST_GET(tr, t).d1)->u0 = t;
              LIST_AT(tr, LIST_GET(tr, t).d1)->u1 = tn;
 
              /* new code to determine the bottom neighbours of the */
              /* newly partitioned trapezoid */
 
              LIST_AT(tr, tn)->d0 = LIST_GET(tr, t).d1;
              LIST_AT(tr, tn)->d1 = SIZE_MAX;
 
              tnext = LIST_GET(tr, t).d1;
            }
 
          t = tnext;
        }
 
      LIST_AT(tr, t_sav)->rseg = segnum;
      LIST_AT(tr, tn_sav)->lseg = segnum;
    } /* end-while */
 
  /* Now combine those trapezoids which share common segments. We can */
  /* use the pointers to the parent to connect these together. This */
  /* works only because all these new trapezoids have been formed */
  /* due to splitting by the segment, and hence have only one parent */
 
  const size_t tfirstl = tfirst;
  const size_t tlastl = tlast;
  merge_trapezoids(segnum, tfirstl, tlastl, S_LEFT, tr, qs);
  merge_trapezoids(segnum, tfirstr, tlastr, S_RIGHT, tr, qs);
 
  seg[segnum].is_inserted = true;
  return 0;
}
 
/* Update the roots stored for each of the endpoints of the segment.
 * This is done to speed up the location-query for the endpoint when
 * the segment is inserted into the trapezoidation subsequently
 */
static void find_new_roots(size_t segnum, segment_t *seg, traps_t *tr,
                           qnodes_t *qs) {
  segment_t *s = &seg[segnum];
 
  if (s->is_inserted) return;
 
  s->root0 = (size_t)locate_endpoint(&s->v0, &s->v1, s->root0, seg, qs);
  s->root0 = LIST_GET(tr, s->root0).sink;
 
  s->root1 = (size_t)locate_endpoint(&s->v1, &s->v0, s->root1, seg, qs);
  s->root1 = LIST_GET(tr, s->root1).sink;
}
 
/* Get log*n for given n */
static size_t math_logstar_n(size_t n) {
  assert(n >= 1);
  size_t i = 0;
  for (double v = (double)n; v >= 1; i++)
      v = log2(v);
 
  return i - 1;
}
 
static size_t math_N(size_t n, size_t h) {
  double v = (double)n;
  for (size_t i = 0; i < h; i++)
      v = log2(v);
 
  return (size_t)ceil((double)n / v);
}
 
/* Main routine to perform trapezoidation */
traps_t construct_trapezoids(size_t nseg, segment_t *seg, size_t *permute) {
    size_t segi = 0;
 
    // we will append later nodes by expanding this on-demand
    qnodes_t qs = {0};
 
    // First trapezoid is reserved as a sentinel. We will append later
    // trapezoids by expanding this on-demand.
    traps_t tr = {0};
    LIST_APPEND(&tr, (trap_t){0});
 
  /* Add the first segment and get the query structure and trapezoid */
  /* list initialised */
 
    const size_t root = init_query_structure(permute[segi++], seg, &tr, &qs);
 
    for (size_t i = 1; i <= nseg; i++)
        seg[i].root0 = seg[i].root1 = root;
 
    const size_t logstar = math_logstar_n(nseg);
    for (size_t h = 1; h <= logstar; h++) {
        for (size_t i = math_N(nseg, h -1) + 1; i <= math_N(nseg, h); i++) {
            if (add_segment(permute[segi++], seg, &tr, &qs) != 0) {
                LIST_FREE(&tr);
                goto done;
            }
        }
 
      /* Find a new root for each of the segment endpoints */
        for (size_t i = 1; i <= nseg; i++)
            find_new_roots(i, seg, &tr, &qs);
    }
 
    for (size_t i = math_N(nseg, logstar) + 1; i <= nseg; i++) {
        if (add_segment(permute[segi++], seg, &tr, &qs) != 0) {
            LIST_FREE(&tr);
            goto done;
        }
    }
 
done:
    LIST_FREE(&qs);
    return tr;
}