/*
 File: DCEL.c
 Authors: K.R. Sloan,
          J. Painter
 Last Modified: 1 November 1992
 Purpose: Doubly-Connected Edge List (improved),
          along with a few utilities for triangulating planar
          point sets

          This version is limited to ONE, CONVEX, TRIANGULATED region,
          with up to ArraySize points (see DCEL.h).

          Obvious things to do next are to allow MULTIPLE, CONCAVE,
          POLYGONALIZED regions, with dynamic allocation of the point set.
          Go ahead, make my day. 

          USE_STORAGE allows you to choose between:
                * malloc/free (#define USE_STORAGE  0)
                * private storage management (#define USE_STORAGE 1)

          malloc/free is a performance hot spot - but it works

          private storage management is fast - but it's a memory hog

*/

#include <stdio.h>
#include <DCEL.h>
double fabs();

#if (0 == USE_STORAGE)
 char *malloc();
 free();
#endif

#if (1 == USE_STORAGE)
 PointSetPointer PointFreeList    = (PointSetPointer)0;
 EdgePointer     EdgeFreeList     = (EdgePointer    )0;
 TrianglePointer TriangleFreeList = (TrianglePointer)0;
#endif

#define EPSILON (1.0e-16)

int ANIMATING;
FILE *AnimationFile;
char *AnimationFileName;
double AnimateColor, AnimateErase;
int DCELVERBOSE = 1;
Point Points[ArraySize];
PointSetPointer Hull;
TrianglePointer Triangles;
EdgePointer Suspects = (EdgePointer)0;
int ProbeRoot;

static void FatalError(s)
 char *s;
 {
  fprintf(stderr,"DCEL: %s\n",s); exit(-1);; 
 } 

void Animate (Animation)
 char *Animation;
 {
  ANIMATING = true;
  AnimationFileName = Animation;
  if (0 == strcmp("-",AnimationFileName))
   AnimationFile = stdout;
  else
   AnimationFile = fopen(AnimationFileName, "w+");
  AnimateColor = 0.0; AnimateErase = 0.5;
  fprintf(AnimationFile,"%%!\n%%%% created by DCEL\n\n");
  fprintf(AnimationFile,"sgpColorDisplay sgpInit\n");
  fprintf(AnimationFile,"-1.0 -1.0 1.0 1.0 sgpSetWindow\n");
  fprintf(AnimationFile,"0.0 0.0 1.0 1.0 sgpSetViewport\n");  
  fprintf(AnimationFile,"%f sgpGrayLevel\n", AnimateErase);
  fprintf(AnimationFile,"sgpClearScreen\n");
 } 

 /*
  Add and Delete Suspect handle a queue of edges.

  AddSuspect allows the same edge to be added multiple times. 
  The first addition puts the edge at the end of the queue.
  Subsequent additions do nothing.

  RemoveSuspect may be called even when the edge is not in the queue.
 */ 
void AddSuspect(E)
 EdgePointer E;
 { 
  if (E == (EdgePointer)0)
   {
    if (DCELVERBOSE)
     fprintf(stderr,"AddSuspect: null EdgePointer!\n");
    return;
   }
  if (E->Previous == (EdgePointer)0)
   { /* a new suspect */
    if (Suspects == (EdgePointer)0)
     { /* only one */
      Suspects = E;  E->Previous = E; E->Next = E;
     } /* only one */
    else
     { /* at the end */   
      E->Previous = Suspects->Previous; E->Next = Suspects;
      Suspects->Previous->Next = E; Suspects->Previous = E;
     } /* at the end */   
   } /* a new suspect */
 }/* AddSuspect */

void RemoveSuspect(E)
 EdgePointer E;
 {
  if (E == (EdgePointer)0)
   {
    if (DCELVERBOSE)
     fprintf(stderr,"RemoveSuspect: null EdgePointer!\n");
    return;
   }
  if (E->Previous != (EdgePointer)0)
   { /* once a suspect, now cleared */
    E->Previous->Next = E->Next;
    E->Next->Previous = E->Previous;
    if (Suspects == E)
     {
      Suspects = E->Next;
      if (Suspects == E) Suspects = (EdgePointer)0; /* all gone */
     }
    E->Next = (EdgePointer)0; E->Previous = (EdgePointer)0;
   } /* once a suspect, now cleared */
 } /* RemoveSuspect */

 /*
  FindEdge tries to find an existing edge (P1,P2) in the edge list for P1.
 */
EdgePointer FindEdge(P1, P2)
 int P1, P2;
 {
  EdgePointer E, E0;
  int found;

  E = Points[P1].Edges; found = (E==(EdgePointer)0);
  E0 = E;
  for (;!found;)
   { /* is this the edge? */
    if (E->Tail.V == P1)
     found = (E->Head.V == P2);
    else 
     found = (E->Tail.V == P2);
    if (!found)
     {
      if (E->Tail.V == P1) E = E->Tail.Next; else E = E->Head.Next;
      if (E == E0) { E = (EdgePointer)0; found = true; }
     }     
   }  /* is this the edge? */
  return (E);  /* or not */
 }  /* FindEdge */

 /*
  Does the path A -> B -> C turn to the left?

  Watch out for colinear points - you may get a random answer.
  Since we are asking for "LeftTurns" - make sure it REALLY turns
  left...  Note that we may miss a VERY SLIGHT, legitimate, LeftTurn.
 */
int LeftTurn (A, B, C)
 int A, B, C;
  {
   return     ( ( ((Points[A].x-Points[B].x)*(Points[C].y-Points[B].y))
                 -((Points[A].y-Points[B].y)*(Points[C].x-Points[B].x))
                )
               < -EPSILON   /* 0.0 ? */
              );
  } /* LeftTurn */

 /*
  This little gem is due to Rob Fowler.  Given two points A, and B
  we calculate a number in [0.0, 8.0) which is a monotonic function of
  the direction from A to B. 

  (0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0) correspond to
  (  0,  45,  90, 135, 180, 225, 270, 315, 360) degrees, measured
  counter-clockwise from the positive x axis.
 */
double FowlerAngle(A, B)
 int A, B;
 {
  double dx, dy, adx, ady;
  int code;

  dx = Points[B].x-Points[A].x; if (dx<0.0) adx = -dx; else adx = dx;
  dy = Points[B].y-Points[A].y; if (dy<0.0) ady = -dy; else ady = dy;
  if (adx<ady) code = 1; else code = 0;
  if (dx<0.0)  code = code+2;
  if (dy<0.0)  code = code+4;
  switch (code)
   {
    case 0: if (dx == 0.0) 
             return (0.0);                 /* A = B */
            else 
             return (ady/adx);        /*   0 <= a <=  45 */ 
    case 1: return (2.0 - (adx/ady)); /*  45 <  a <=  90 */
    case 2: return (4.0 - (ady/adx)); /* 135 <= a <= 180 */
    case 3: return (2.0 + (adx/ady)); /*  90 <  a <  135 */
    case 4: return (8.0 - (ady/adx)); /* 315 <= a <  360 */
    case 5: return (6.0 + (adx/ady)); /* 270 <= a <  315 */
    case 6: return (4.0 + (ady/adx)); /* 180 <  a <= 225 */
    case 7: return (6.0 - (adx/ady)); /* 225 <  a <  270 */
   }
  } /* FowlerAngle */

  /*
    FindNeighbors locates the edges which would be the left
    and right neighbors of the edge O-P, in the list of edges
    emanating from O.

    ASSUMPTION: there must be at least 1 Edge emanating from O; check
                before you call...

    Really special case: if O-P is colinear with L and R, make sure
    that P is on the same side of O as R.  This choice ensures that 
    a triangle will be found (by Search) if it is at all possible.

    The above doesn't work for all cases!  But - we've pushed off
    the last (!) special case to Search - go and read Search.

  */

void FindNeighbors (O, P, LeftNeighbor, RightNeighbor)
 int O,P;
 EdgePointer *LeftNeighbor, *RightNeighbor;
 {
  int found;
  double Ox, Oy, Px, Py, Ax, Ay, Bx, By, AOP, BOP, BOA;
  int A;
  EdgePointer L, R;
  TrianglePointer T;

  Ox = Points[O].x;      Oy = Points[O].y;
  Px = Points[P].x - Ox; Py = Points[P].y - Oy;
  L = Points[O].Edges;
  if ((EdgePointer)0 == L)
   {
    if (DCELVERBOSE)
     fprintf(stderr,"FindNeighbors: NO EDGES!\n");
    *LeftNeighbor = (EdgePointer)0; *RightNeighbor = (EdgePointer)0;
    return;
   }
  if (L->Tail.V == O)  A = L->Head.V; else A = L->Tail.V;
  Ax = Points[A].x - Ox; Ay = Points[A].y - Oy;  
  AOP = (Ax * Py) - (Ay * Px);
  if (L->Tail.V == O)  R = L->Tail.Next; else R = L->Head.Next;
  found = (L==R); /* if only one edge, we are done */
  for (;!found;)
   {
    R = L; Bx = Ax; By = Ay; BOP = AOP;
    if (R->Tail.V == O) L = R->Tail.Next; else L = R->Head.Next;
    if (L->Tail.V == O) A = L->Head.V;    else A = L->Tail.V;
    Ax = Points[A].x - Ox; Ay = Points[A].y - Oy;  

    AOP = (Ax * Py) - (Ay * Px);
    BOA = (Bx * Ay) - (By * Ax);      

    /* 
      L and R are currently neighbors, L is to the left of R
      <Ax,Ay> and <Bx, By> are the other ends of L and R
      AOP is the signed area of the trapezoid defined by OA and OP
      BOP is the signed area of the trapezoid defined by OB and OP
      BOA is the signed area of the trapezoid defined by OB and OA
      a positive sign means that the angle (at O) is less than pi
      a negative sign means that the angle (at O) is greater than pi
      a zero area means that the points are colinear
        in this case, we resort to the dot product, to see if
        the angle is 0 or pi

      The determination that P would fit between A and B in the Edge list
      requires a small amount of case analysis, viz.
     */

    if      (BOA > 0.0) /* less than pi from B to A */
     found = ((BOP >= 0.0) && (AOP <= 0.0));
    else if (BOA < 0.0) /* more than pi from B to A */
     found = ((BOP >= 0.0) || (AOP <= 0.0));
    else if (((Ax*Bx) + (Ay*By)) < 0.0) 
     { /* pi from B to A */
      if      (BOP > 0.0)  found = true;
      else if (BOP < 0.0)  found = false; 
      else found = (((Bx*Px)+(By*Py)) >= 0.0);
      /*
       when A,O,P, and B are colinear, we prefer to return
       with B and P on the same side of O.  This helps find
       triangles that otherwise might be missed.
       When B and P are on the same side of O,  the
       RightNeighbor points towards P.  If RightNeighbor is
       part of ONLY ONE triangle,  that triangle is on its
       left.  If we return with A and P on the same side of O
       (i.e., with LeftNeighbor pointing at P)  Search
       may conclude that P cannot be part of any triangle.

       Making this arbitrary choice here makes Search's job
       easier.  

       But not trivial - see the comments in Search for the last(!)
       special case.
      */ 
     } /* pi from B to A */
    else 
     { /* 0 from B to A */
      if (R->Tail.V == O) T = R->Tail.T; else T = R->Head.T;    
       found =   (T==0) 
               || ((BOP == 0.0) && (((Bx*Px)+(By*Py)) >= 0.0));
     }   /* 0 from B to A */
   }
  *LeftNeighbor = L; *RightNeighbor = R;
 }  /* FindNeighbors */

 /*
  AddEdge inserts the edge P1-P2, and the information that this edge
  has the Triangle T on its left.
 */
EdgePointer AddEdge (P1, P2, T)
 int P1, P2;
 TrianglePointer T;
 {
  EdgePointer E, L, R;

  E = FindEdge(P1, P2);
  if ((EdgePointer)0 !=  E)
   if (E->Tail.V == P1)  E->Tail.T = T; else E->Head.T = T;
  else 
   { /* new edge - put <P1, T> at the Tail, <P2, 0> at the Head */
    E = NewEdge();
    if ((EdgePointer)0 == E) FatalError("NewEdge Failed!");
    E->Tail.V = P1;    E->Tail.T = T;
    E->Head.V = P2;    E->Head.T = (TrianglePointer)0;
    E->Previous = (EdgePointer)0; E->Next = (EdgePointer)0;
    if (Points[P1].Edges == (EdgePointer)0)
     { /* empty list */
      Points[P1].Edges = E;
      E->Tail.Previous = E; E->Tail.Next = E;
     } /* empty list */
    else
     { /* non-empty list */
      FindNeighbors(P1, P2, &L, &R);
      E->Tail.Previous = R; E->Tail.Next = L;
      if (R->Tail.V == P1)  R->Tail.Next = E;
      else                  R->Head.Next = E;
      if (L->Tail.V == P1)  L->Tail.Previous = E;
      else                  L->Head.Previous = E;
     }; /* non-empty list */

    if (Points[P2].Edges == (EdgePointer)0)
     { /* empty list */
      Points[P2].Edges = E;
      E->Head.Previous = E; E->Head.Next = E;
     } /* empty list */
    else
     { /* non-empty list */
      FindNeighbors(P2, P1, &L, &R);
      E->Head.Previous = R; E->Head.Next = L;
      if (R->Tail.V == P2) R->Tail.Next = E;
      else                 R->Head.Next = E;
      if (L->Tail.V == P2) L->Tail.Previous = E;
      else                 L->Head.Previous = E;
     }   /* non-empty list */
   } /* new edge */
  return (E);
 }  /* AddEdge */

 /*
  RemoveEdge removes the Triangle T as support for Edge E.
  If there is no triangle on the other side of E,  E is destroyed.
 */
void RemoveEdge(E, T)
 EdgePointer E;
 TrianglePointer T;
 {
  if (E->Tail.T == T) E->Tail.T = (TrianglePointer)0;
  else                E->Head.T = (TrianglePointer)0;
  if (    ((TrianglePointer)0 == E->Tail.T)
       && ((TrianglePointer)0 == E->Head.T) )
   { /* all gone */
    if (E->Tail.Next == E)
     Points[E->Tail.V].Edges = (EdgePointer)0;
    else
     { /* one of many */
      if (E->Tail.Next->Tail.Previous == E)
       E->Tail.Next->Tail.Previous  = E->Tail.Previous;
      else
       E->Tail.Next->Head.Previous  = E->Tail.Previous;
      if (E->Tail.Previous->Tail.Next == E)
       E->Tail.Previous->Tail.Next  = E->Tail.Next;
      else
       E->Tail.Previous->Head.Next  = E->Tail.Next;
      Points[E->Tail.V].Edges = E->Tail.Next;
     }  /* one of many */
    if (E->Head.Next == E)
     Points[E->Head.V].Edges = (EdgePointer)0;
    else
     { /* one of many */
      if (E->Head.Next->Tail.V == E->Head.V)
       E->Head.Next->Tail.Previous  = E->Head.Previous;
      else
       E->Head.Next->Head.Previous  = E->Head.Previous;
      if (E->Head.Previous->Tail.V == E->Head.V)
       E->Head.Previous->Tail.Next  = E->Head.Next;
      else E->Head.Previous->Head.Next  = E->Head.Next;
      Points[E->Head.V].Edges = E->Head.Next;
     }  /* one of many */
    RemoveSuspect(E);
    DisposeEdge(E); E = (EdgePointer)0;
   }   /* all gone */
 } /* RemoveEdge */

 /*
  AddTriangle creates the triangle A,B,C - with figure of merit R.
  (See Rho and Shrink for the definition and use of R)
 */
void AddTriangle (A, B, C, R)
 int A, B, C;
 double R;
 {
  TrianglePointer T;

  if (ANIMATING)
   { /* animation trace */
    fprintf(AnimationFile,"%f sgpGrayLevel\n",AnimateColor);
    fprintf(AnimationFile,
      "[[%f %f]\n [%f %f]\n [%f %f]\n [%f %f]] sgpPolyLine\n",
                                    Points[A].x, Points[A].y, 
                                    Points[B].x, Points[B].y,
                                    Points[C].x, Points[C].y,
                                    Points[A].x, Points[A].y);
   }  /* animation trace */

  T = NewTriangle();
  if ((TrianglePointer)0 == T) FatalError("NewTriangle Fails!");

  if ((TrianglePointer)0 == Triangles)
   { /* only one */
    Triangles = T; T->Previous = T; T->Next = T;
   } /* empty list */
  else
   { /* at the end */
    T->Previous = Triangles->Previous; T->Next = Triangles;
    Triangles->Previous->Next = T; Triangles->Previous = T;
   } /* at the end */

  T->P1 = A; T->P2 = B; T->P3 = C;
  T->E1 =  AddEdge(A, B, T);
  T->E2 =  AddEdge(B, C, T);
  T->E3 =  AddEdge(C, A, T);
  T->Merit = R;
 } /*AddTriangle*/

 /*
  RemoveTriangle destroys T
 */
void RemoveTriangle (T)
 TrianglePointer T;
 {
  EdgePointer Edge1, Edge2, Edge3;
  TrianglePointer ThisTriangle;

  if (ANIMATING)
   { /* animation trace */
    fprintf(AnimationFile,"%f sgpGrayLevel\n",AnimateErase);
    fprintf(AnimationFile,
      "[[%f %f]\n [%f %f]\n [%f %f]\n [%f %f]] sgpPolyLine\n",
                     Points[T->P1].x, Points[T->P1].y,
                     Points[T->P2].x, Points[T->P2].y,
                     Points[T->P3].x, Points[T->P3].y,
                     Points[T->P1].x, Points[T->P1].y);
   }  /* animation trace */
  T->Previous->Next = T->Next;
  T->Next->Previous = T->Previous;
  if (Triangles == T)
   {
    Triangles = T->Next;
    if (Triangles == T) Triangles = (TrianglePointer)0; /* all gone */
   };
  RemoveEdge(T->E1, T);
  RemoveEdge(T->E2, T);
  RemoveEdge(T->E3, T);
  DisposeTriangle(T); T = (TrianglePointer)0;
 } /* RemoveTriangle */


 /*
   InsideTriangle decides if a point P is Inside, or OnEdge
   of the triangle defined by A, B, C.

   If P is on an edge, V1 and V2 identify that edge
 */
void InsideTriangle (A, B, C, P, Inside, OnEdge, V1, V2)
 int A, B, C, P;
 int *Inside, *OnEdge;
 int *V1, *V2;
 {
  double ax, ay, bx, by, cx, cy, apx, apy, bpx, bpy, cpx, cpy;
  double cCROSSap, bCROSScp, aCROSSbp, DOT;
  int OnEdgeAB, OnEdgeBC, OnEdgeCA;

  *Inside = false; *OnEdge = false;
  OnEdgeAB = false; OnEdgeBC = false; OnEdgeCA = false;
  ax = Points[C].x - Points[B].x;  ay = Points[C].y - Points[B].y;
  bx = Points[A].x - Points[C].x;  by = Points[A].y - Points[C].y;
  cx = Points[B].x - Points[A].x;  cy = Points[B].y - Points[A].y;
  apx= Points[P].x - Points[A].x;  apy= Points[P].y - Points[A].y;
  bpx= Points[P].x - Points[B].x;  bpy= Points[P].y - Points[B].y;
  cpx= Points[P].x - Points[C].x;  cpy= Points[P].y - Points[C].y;

  aCROSSbp = ax*bpy - ay*bpx;
  cCROSSap = cx*apy - cy*apx;
  bCROSScp = bx*cpy - by*cpx;

  *Inside = ((aCROSSbp > 0.0) && (bCROSScp > 0.0) && (cCROSSap > 0.0));

  if (!(*Inside))
   {
    if ((EPSILON > aCROSSbp) && (aCROSSbp > -EPSILON))
     {
      DOT = (ax*bpx)+(ay*bpy);
      OnEdgeBC = ((0.0 <= DOT) && (DOT <= (ax*ax)+(ay*ay)));
     }

    if ((EPSILON > bCROSScp) && (bCROSScp > -EPSILON))
     {
      DOT  = (bx*cpx)+(by*cpy);
      OnEdgeCA = ((0.0 <= DOT) && (DOT <= (bx*bx)+(by*by)));
     }

    if ((EPSILON > cCROSSap) && (cCROSSap > -EPSILON))
     {
      DOT = (cx*apx)+(cy*apy);
      OnEdgeAB = ((0.0 <= DOT) && (DOT <= (cx*cx)+(cy*cy)));
     }
    if      (OnEdgeAB) { *V1 = A; *V2 = B; *OnEdge = true; }
    else if (OnEdgeBC) { *V1 = B; *V2 = C; *OnEdge = true; }
    else if (OnEdgeCA) { *V1 = C; *V2 = A; *OnEdge = true; }
   }
 } /* InsideTriangle */

  /*
    Given (the GLOBAL variable) ProbeRoot - a vertex in a 
    CONVEX, TRIANGULATED region
    of the plane, Search attempts to find the Triangle containing P.
    This is accomplished by traversing the triangulation from ProbeRoot
    towards P until an enclosing triangle is found, or P is shown to be
    outside the convex hull. 

    If an enclosing triangle is found, Search also determines if P is
    strictly Inside, or OnEdge.  If OnEdge,  V1 and V2 identify
    the edge.  

    LeftNeighbor and RightNeighbor are two edges emanating from a vertex
    in the triangulated region which bracket the point P.  If P is Inside
    or OnEdge,  these are edges of the enclosing triangle.  If no 
    enclosing triangle is found,  these are edges from the convex hull
    of the triangulated region.  In the latter case, V1 identifies the
    shared vertex.

    Thus, if P is known to be inside the convex hull,  Search will
    always return the enclosing triangle.

    If P is OUTSIDE the convex hull,  V1 is visible from P, and the
    Neighboring edges are the hull edges emanating from V1.
  */
TrianglePointer Search(P, LeftNeighbor, RightNeighbor,Inside, OnEdge, V1, V2)
 int P;
 EdgePointer *LeftNeighbor, *RightNeighbor;
 int *Inside, *OnEdge;
 int *V1, *V2;
 {
  int O;
  EdgePointer L, R;
  TrianglePointer T;
  int done;

  O = ProbeRoot; done = false; *Inside = false; *OnEdge = false;
  for (;!done;)
   {
    FindNeighbors(O, P, &L, &R);

    if (VERBOSE)  /* take this out soon */
     {
      fprintf(stderr,"FindNeighbors returns: L = (%d,%d), R = (%d,%d)\n",
                     L->Tail.V,L->Head.V,R->Tail.V,R->Head.V);
     }

    if (R->Tail.V == O)  T = R->Tail.T; else T = R->Head.T;
    if ((TrianglePointer)0 == T) 
     { 
      /* 
        We should have a triangle - but we don't.
        It may be that we are ON the hull, and there is a triangle
        on the other side (to the right) of R.

        Look for the other triangle.
        If we find one, see if we are OnEdge (or, perhaps even Inside?)

        If we are not Inside or OnEdge, then we are outside the hull,
        and should return the original L and R, and a null Triangle.

        But...if we do turn out to be Inside or OnEdge, adjust L,R,T
        and heave a sigh of relief.
       */
      if (R->Tail.V == O) T = R->Head.T; else T = R->Tail.T;
      if ((TrianglePointer)0 == T)
       { *V1 = O; done = true; }   /* this should never happen, but... */
      else
       {
        InsideTriangle(T->P1, T->P2, T->P3, P, Inside, OnEdge, V1, V2);
        if ((*OnEdge) || (*Inside))
         {
          /* Huzzah!  We dodged a bullet there! */
          L = R;  
          if (L->Tail.V == O)  R = L->Tail.Next; else R = L->Head.Next;
          done = true;
         }
        else
         {
          /* Sigh.... this P is truly outside the hull */
          T = (TrianglePointer)0;
          *V1 = O;
          done = true; 
         }
       }
     }
    else
     {
      InsideTriangle(T->P1, T->P2, T->P3, P, Inside, OnEdge, V1, V2);
      if ((*Inside) || (*OnEdge))   
       done = true;
      else 
       if (L->Tail.V == O)  O = L->Head.V; else O = L->Tail.V;
     } 
   }
  *LeftNeighbor = L; *RightNeighbor = R;
  return (T);
 }

 /*
  Rho calculates 4r^2, where r is the radius of the circle passing 
  through A, B, C

  The calculation of Rho depends on the observations that:

                            2
                (   abc    )     2 2 2             2
   Rho = 4*r*r= (-------   )  = a b c / (a cross c)
                (a cross c )

             2   2   2
   and that a , b , c , and (a cross c) are all easy to calculate.

    2
   a  is the square of the length of the side opposite vertex A

    2
   b  is the square of the length of the side opposite vertex B

    2
   c  is the square of the length of the side opposite vertex C
   
 */

double Rho (A, B, C)
 int A, B, C;
 {
  double vAx, vAy, vBx, vBy, vCx, vCy; /* vertex coordinates      */
  double ax, ay, bx, by, cx, cy;       /* edge vector coordinates */
  double ax2, ay2, bx2, by2, cx2, cy2; /* squares                 */
  double aCROSSb;                      /* cross-product */

  vAx = Points[A].x; vAy = Points[A].y;
  vBx = Points[B].x; vBy = Points[B].y;
  vCx = Points[C].x; vCy = Points[C].y;
  ax = vCx-vBx; ay = vCy-vBy;
  bx = vAx-vCx; by = vAy-vCy;
  aCROSSb = ax*by - ay*bx;
  if      (aCROSSb > 0.0)
   { /* correctly oriented triangle */

    if (aCROSSb < EPSILON) aCROSSb = EPSILON; /* fault protection */

                                      ax2 = ax*ax; ay2 = ay*ay;    
                                      bx2 = bx*bx; by2 = by*by;
    cx = vBx-vAx; cy = vBy-vAy;       cx2 = cx*cx; cy2 = cy*cy;
    return ( (ax2+ay2)*(bx2+by2)*(cx2+cy2)/(aCROSSb*aCROSSb) );
   }   /* correctly oriented triangle */
  else
   return ( -1.0 ); /* backwards | colinear triangles don"t count */
 } /* Rho */

 /*
   Shrink is the absolute HOT SPOT - so we've optimized as much as
   is seemly.  Given a pair of triangles ABC and BAD, we want to know if
   the pair ADC, BCD would be better.  If it is,  we make the switch.
   
   To determine which local configuration is best, we calculate

    Rabc = the square of the radius of the circle through ABC
    Rbad = the square of the radius of the circle through BAD

    Radc = the square of the radius of the circle through ADC
    Rbcd = the square of the radius of the circle through BCD
    
    Rabc and Rbad are associated with the current configuration;
    Radc and Rbcd are associated with the proposed configuration.

    The calculation of the  R"s depends on the observations that:

                            2
               (   abc    )
        4*r*r= (-------   )
               (a cross c )

    so, we get:
               2 2 2              2
     Rabc =   a c e  / (a cross c)

               2 2 2              2
     Racd =   a d f  / (a cross d)

               2 2 2              2 
     Rbad =   b d e  / (b cross d)

               2 2 2              2
     Rbcd =   b c f  / (b cross c)

   where
        a is the edge connecting A and C
        b is the edge connecting B and D
        c is the edge connecting C and B
        d is the edge connecting D and A  
        e is the existing diagonal  connecting A and B
        f is the candidate diagonal connecting C and D

  Of the four circles, the smallest one is guaranteed to EXCLUDE the fourth 
  point - that is the configuration we want.  So, we first pick the minimum
  from each pair ([Rabc, Rabd] and [Racd, Rbcd]) and  compare the minima.
  if min(Rabc,Rabd) > min(Racd, Rbcd)  SWAP else DO NOT SWAP.

  BE CAREFUL: Rho is negative for colinear or "backwards" triangles;
              negative Rhos should compare GREATER than positive
              This should only happen when one of the already created
              triangles is linear...so we add an extra check.

  Finally, this criterion is not valid when the quadrilateral ADBC is not
  convex. So, we first check the angles DAC and CBD: 
  If (a cross d) and (b cross c) are both positive
  all is well.  If either (a cross d) of (b cross c) are negative,
  all of the above is moot, and no swap is possible.  
  Here, it is important that:
        
         a is the DIRECTED edge from A to C
         b is the DIRECTED edge from B to D
         c is the DIRECTED edge from C to B
         d is the DIRECTED edge from D to A

  Rabc and Rbad have already been calculated.
  Radc and Rbcd need to be calculated - we do it ourself, for speed.

 */
void Shrink(SharedEdge)
 EdgePointer SharedEdge;
 {
  int A, B, C, D;
  double vAx, vAy, vBx, vBy, vCx, vCy, vDx, vDy;      /* vertex coordinates */
  double ax, ay, bx, by, cx, cy, dx, dy, fx, fy;      /* edge vector deltas */
  double ax2, ay2, bx2, by2, cx2, cy2, dx2, dy2, fx2, fy2;
  double aCROSSd, bCROSSc;
  double Rabc, Radc, Rbad, Rbcd, R1, R2;

  A = SharedEdge->Tail.V;       B = SharedEdge->Head.V;

  C = SharedEdge->Tail.Next->Tail.V;
  if (C == A)  C = SharedEdge->Tail.Next->Head.V;

  D = SharedEdge->Tail.Previous->Tail.V;
  if (D == A)  D = SharedEdge->Tail.Previous->Head.V;

  vAx = Points[A].x; vAy = Points[A].y;
  vBx = Points[B].x; vBy = Points[B].y;
  vCx = Points[C].x; vCy = Points[C].y;
  vDx = Points[D].x; vDy = Points[D].y;

  ax = vCx-vAx; ay = vCy-vAy;      
  bx = vDx-vBx; by = vDy-vBy;
  cx = vBx-vCx; cy = vBy-vCy;
  dx = vAx-vDx; dy = vAy-vDy;

  aCROSSd = ax*dy - ay*dx;
  bCROSSc = bx*cy - by*cx;   
  if ((aCROSSd > 0.0) && (bCROSSc > 0.0))
   { /* ADBC is convex */
                                  ax2 = ax*ax; ay2 = ay*ay;
                                  bx2 = bx*bx; by2 = by*by;
                                  cx2 = cx*cx; cy2 = cy*cy;
                                  dx2 = dx*dx; dy2 = dy*dy;
    fx = vCx-vDx; fy = vCy-vDy; fx2 = fx*fx; fy2 = fy*fy;

    if (aCROSSd < EPSILON) aCROSSd = EPSILON; /* fault protection */
    if (bCROSSc < EPSILON) bCROSSc = EPSILON; /* fault protection */

    Radc = (ax2+ay2)*(dx2+dy2)*(fx2+fy2)/(aCROSSd*aCROSSd);
    Rbcd = (bx2+by2)*(cx2+cy2)*(fx2+fy2)/(bCROSSc*bCROSSc);
    if (Radc < Rbcd)  R2 = Radc; else R2 = Rbcd;

    Rabc = SharedEdge->Tail.T->Merit;
    Rbad = SharedEdge->Head.T->Merit;
    if (Rabc < Rbad)
     if (Rabc >= 0.0)  R1 = Rabc; else R1 = Rbad;
    else if (Rbad >= 0.0)  R1 = Rbad; else R1 = Rabc;

    if (R1 > R2)
     { /* swap */
      RemoveTriangle(SharedEdge->Tail.T);
      RemoveTriangle(SharedEdge->Head.T);
      AddTriangle(A, D, C, Radc);
      AddTriangle(B, C, D, Rbcd);
      AddSuspect(FindEdge(A,D));
      AddSuspect(FindEdge(A,C));
      AddSuspect(FindEdge(B,D));
      AddSuspect(FindEdge(B,C));
     }  /* swap */
   }   /* ADBC is convex */
 } /* Shrink */

 /*
   Optimize is called to clean up a triangulation.

   On entry: the set pointed at by Suspects contains "suspect" edges.  

   Optimize considers each suspect edge, in turn.  For each such edge,
   the local quadrilateral is examined.  If the suspect edge is OK,
   it is simply removed from the suspect queue.  If not,  the
   two triangles which share this edge are removed, and two other triangles
   are inserted.  The four outer edges of these two triangles  become 
   suspect, and are added to the end of the queue.

   Parameters?  WE DON"T GOT TO SHOW YOU NO STINKING PARAMETERS!
 */
void Optimize()
 {
  EdgePointer E;
  TrianglePointer T1, T2;

  for (;Suspects != (EdgePointer)0;)
   { /* cross-examine */
    E = Suspects; RemoveSuspect(E);
    T1 = E->Tail.T; T2 = E->Head.T;
    if ((T1 != (TrianglePointer)0) && (T2 != (TrianglePointer)0)) Shrink(E);
   } /* cross-examine */
 } /* Optimize */

 /*
   Duplicate checks for (within EPSILON) duplicate points
  */

static int Duplicate(A,B)
 int A,B;
 {
  return (  (EPSILON > fabs(Points[A].x-Points[B].x))
          &&(EPSILON > fabs(Points[A].y-Points[B].y))
         );
 }
 /*
   InsertPoint adds a new point.

   The current version only understands how to do that when P is
   inside an already triangulated convex region.

   In case of error, return false.

   Coming soon, a version that will do on-line insertions
 */ 
 int InsertPoint (P)
 int P;
 {
  int Inside, OnEdge;
  TrianglePointer T, T2;
  EdgePointer L, R, E;
  int A, B, C, D;

  T = Search(P, &L, &R, &Inside, &OnEdge, &A, &B);

  if (VERBOSE) /* take this out soon */
   {
    fprintf(stderr,"InsertPoint: Search returns:\n");
    fprintf(stderr,"\tT = %x, L = %x, R = %x\n",T,L,R);
    fprintf(stderr,"\tInside = %d, OnEdge = %d\n",Inside, OnEdge);
    fprintf(stderr,"\tA = %d, B = %d\n",A,B);
   }

  if (Inside)
   { 
    A = T->P1; B = T->P2; C = T->P3;
    RemoveTriangle (T);
    AddTriangle(A, B, P, Rho(A,B,P));
    AddTriangle(B, C, P, Rho(B,C,P));
    AddTriangle(C, A, P, Rho(C,A,P));
    AddSuspect(FindEdge(A, B));
    AddSuspect(FindEdge(B, C));
    AddSuspect(FindEdge(C, A));
   }
  else if (OnEdge)
   {
    if (Duplicate(A,P))
     {
      if (DCELVERBOSE)
       {
        fprintf(stderr,
         "Insert Point fails: %5d at %15.8e,%15.8e is a duplicate\n",
                           P,Points[P].x, Points[P].y);
        fprintf(stderr,
         "duplicate is   ...  %5d at %15.8e,%15.8e\n",
                           A,Points[A].x,Points[A].y);
       }   
      return (0);
     }
    else if (Duplicate(B,P))
     {
      if (DCELVERBOSE)
       {
        fprintf(stderr,
         "Insert Point fails: %5d at %15.8e,%15.8e is a duplicate\n",
                         P,Points[P].x, Points[P].y);
        fprintf(stderr,
         "duplicate is   ...  %5d at %15.8e,%15.8e\n",
                         B,Points[B].x,Points[B].y);
       }
      return (0);
     }
    E = FindEdge(A, B);
    A = E->Tail.V; B = E->Head.V;
    C = E->Tail.Next->Tail.V;
    if (C == A)  C = E->Tail.Next->Head.V;
    D = E->Tail.Previous->Tail.V;
    if (D == A)  D = E->Tail.Previous->Head.V;
    T = E->Tail.T; T2 = E->Head.T;
    if (T  != (TrianglePointer)0)  RemoveTriangle(T);
    if (T2 != (TrianglePointer)0)  RemoveTriangle(T2);
    if (T  != (TrianglePointer)0)
     { /* replace T */
      AddTriangle(A, P, C,Rho(A,P,C));
      AddTriangle(C, P, B,Rho(C,P,B));
      AddSuspect(FindEdge(A, C));
      AddSuspect(FindEdge(C, B));
      AddSuspect(FindEdge(P, A));
      AddSuspect(FindEdge(P, B));
     }  /* replace T */
    if (T2 != (TrianglePointer)0)
     { /* replace T2 */
      AddTriangle(B, P, D,Rho(B,P,D));
      AddTriangle(D, P, A,Rho(D,P,A));
      AddSuspect(FindEdge(B, D)); 
      AddSuspect(FindEdge(D, A));
      AddSuspect(FindEdge(P, A));
      AddSuspect(FindEdge(P, B));
     }   /* replace T2 */
   }
  else
   {
    if (DCELVERBOSE)
     fprintf(stderr,
      "Insert Point fails: %5d at %15.8e,%15.8e is not in hull\n",
                         P,Points[P].x, Points[P].y);
    return (0);
   }
  return (1);
 } /*InsertPoint*/

 /*
  ConvexTriangles will triangulate a CONVEX polygon.

  ASSUMPTION: the polygon is "strictly convex" - there are no
              configurations of the form ....A B C.... where B
              is on the line segment A-C.

  This assumption is only important at the very beginning,
  and the very end of the triangulation.  If it is violated,
  the triangulation will include zero-area triangles.
 */
void ConvexTriangles (Hull)
 PointSetPointer Hull;
 {
  PointSetPointer Top, Bottom, Trailer;
  int incrementTop;

  Trailer = Hull;              /* first Hull point */
  Top     = Trailer->Next;     /* second Hull point */
  Bottom  = Trailer->Previous; /* last Hull point */
  incrementTop = false;
  for (;Top != Bottom;)
   {
    AddTriangle(Trailer->P,Top->P,Bottom->P,
                Rho(Trailer->P,Top->P,Bottom->P));
    AddSuspect(FindEdge(Top->P,Bottom->P));
    incrementTop = !incrementTop;
    if (incrementTop)
     { Trailer = Top;    Top    = Top->Next; }
    else
     { Trailer = Bottom; Bottom = Bottom->Previous; }
   }
 } /*ConvexTriangles*/

 /*
  Is A in PointSet P ?
 */
int InPointSet(A, P)
 int A;
 PointSetPointer P;
 {
  PointSetPointer ThisP;
  for (ThisP=P;
       ((ThisP->Next) != P) && ((ThisP->P) != A);
       ThisP = ThisP->Next)
   { /* just looking, thanks */ }
  return (ThisP->P == A);
 }

 /*
    Is A in Convex Polygon H?
  */
int InsideH(A, H)
 int A;
 PointSetPointer H;
 {
  PointSetPointer H1,H2;
  H1 = H;
  for (;;)
   {
    H2=H1->Next;
    if (LeftTurn(H1->P,A,H2->P)) return (0);
    if (H2 == H) return(1);
    H1 = H2;
   }
 }

 /*
     given three points, calculates the center of the circle thru
     those points.

     WARNING: if the points are colinear, we print a message, and 
              return something reasonble.  Best not to call this routine
              with colinear points, eh? 
  */

void VoronoiPt(x1, y1, x2, y2, x3, y3, x, y)
 double x1, y1, x2, y2, x3, y3;
 double *x, *y;
 {
  double a1,a2,a3,b1,b2,b3,c1,c2,c3;
  double A1,A2,A3,B1,B2,B3;
  double bc1, bc2, bc3, ac1, ac2, ac3;
  double Ap1, Ap2, Ap3, Bp1, Bp2, Bp3;
  double v1, v2, v3;

  /* see Newman & Sproull, p 494-496 */

  /* transform points to homogeneous coords */
  a1 = x1; a2 = y1; a3 = 1.0;
  b1 = x2; b2 = y2; b3 = 1.0;
  c1 = x3; c2 = y3; c3 = 1.0;

  /* find the lines from b-c (call it A) and a-c (call it B) */
  A1 = (b2*c3)-(b3*c2); A2 = (b3*c1)-(b1*c3); A3 = (b1*c2)-(b2*c1);
  B1 = (a2*c3)-(a3*c2); B2 = (a3*c1)-(a1*c3); B3 = (a1*c2)-(a2*c1);

  /* find the midpoints on these lines */
  bc1 = b1+c1; bc2 = b2+c2; bc3 = 2.0;    /* we're being clever here... */
  ac1 = a1+c1; ac2 = a2+c2; ac3 = 2.0;

  /* find lines perpendicular to A and B, through bc and ac */
  Ap1 = A2*bc3; Ap2 = -A1*bc3; Ap3 = (A1*bc2)-(A2*bc1);
  Bp1 = B2*ac3; Bp2 = -B1*ac3; Bp3 = (B1*ac2)-(B2*ac1);

  /* intersect the perpendicular bisectors */
  v1=(Bp3*Ap2)-(Bp2*Ap3); v2=(Bp1*Ap3)-(Bp3*Ap1); v3=(Bp2*Ap1)-(Bp1*Ap2);

  if ((-EPSILON < v3) && (v3 < EPSILON))
   {
    if (DCELVERBOSE)
     fprintf(stderr,"VoronoiPt: degenerate triangle\n");
    if (v3 < 0.0) v3 = -EPSILON; else v3 = EPSILON;
   }
  
  /* back to 2D */
  *x = v1/v3; *y = v2/v3;
 }
