//==============================================================================================
//  geometry.cc                                                                    Font3D
//----------------------------------------------------------------------------------------------
//
//  Copyright (c) 1994-1996 by Todd A. Prater                                   Version 1.60
//  All rights reserved.
//
//----------------------------------------------------------------------------------------------
//
//  Permission to copy and distribute Font3D in its entirety, for noncommercial purposes,
//  is hereby granted without fee, provided that this license information and copyright 
//  notice appear in all copies. 
//
//  If you redistribute Font3D, the entire contents of this distribution must be distributed,
//  including the readme.txt, and register.txt files, and the complete set of documentation,
//  both ASCII text, and PostScript files. 
//
//  The software may be modified for your own purposes, but modified versions may not be
//  distributed without prior consent of the author.
//
//  This software is provided 'asis', without any express or implied warranty.  In no event
//  will the author be held liable for any damages arising from the use of this software.  
//
//  If you would like to do something with Font3D that this copyright prohibits (such as 
//  distributing it with a commercial product, using portions of the source in some other
//  program, distributing registered copies, etc.), please contact the author (preferably
//  via email).  Arrangements can probably be worked out.
//
//==============================================================================================

#include <math.h>
#include <stddef.h>
#include <stdio.h>
#include <iostream.h>

#include "vector.h"
#include "geometry.h"


//==============================================================================================
//  TRIANGLELIST::Empty()                                                             (PUBLIC)
//----------------------------------------------------------------------------------------------
//
//  This function removes all of the triangles from a triangle list.
//
//==============================================================================================

   void TRIANGLELIST::Empty(void)
   {
      int i;
      TRIANGLELISTLINK* tempnext;
      gotoFirst();
      for (i=0;i<count;i++)
      {
         delete current->Obj();
         gotoNext();
      }
      gotoFirst();
      for (i=0;i<count;i++)
      {
         tempnext = current->Next();
         if (tempnext!=NULL) delete current;
         current = tempnext;
      }
      count=0;
      first=NULL;
      current=NULL;
      last=NULL;
   }


//==============================================================================================
//  TRIANGLELIST::Add()                                                               (PUBLIC)
//----------------------------------------------------------------------------------------------
//
//  This function adds a triangle to a triangle list.  If successful it returns TRUE, FALSE
//  otherwise.
//
//==============================================================================================

   int TRIANGLELIST::Add (TRIANGLE* object)
   {
      TRIANGLELISTLINK* newlink;

      newlink = new TRIANGLELISTLINK(object,NULL);

      if (!newlink)
      {
         return FALSE;
      }

      if (count==0)
      {
         first = newlink;
         current = newlink;
         last = newlink;
         last->setNext(NULL);
      }
      else
      {
         last->setNext(newlink);
         last = newlink;
      }

      count++;
      return TRUE;

   }


//=============================================================================================
//  ostream << POLYGON                                                                (DEBUG)
//---------------------------------------------------------------------------------------------
//
//  This is provided as a debugging aid.  Sometimes it is useful to print out the contents
//  of a polygon (ie. its number of vertices and the actual vertices).
//
//=============================================================================================

   ostream& operator << (ostream& s, const POLYGON& p)
   {
       if (p.numpoints==0)
           s<<"EMPTY POLYGON\n";
       else
       {
          s<<"POLYGON with "<<p.numpoints<<" sides:\n";
          for (int i=0;i<p.numpoints;i++)
             s<<"   "<<p.pointlist[i]<<"\n";
       }

       return s;
   }
 

//=============================================================================================
//  POLYGON::POLYGON()                                                                (PUBLIC)
//---------------------------------------------------------------------------------------------
//
//  These are the constructors for the POLYGON class.  The first creates a POLYGON with no
//  vertices, the second creates a POLYGON given an array of n points, and the third is a
//  copy constructor.
//
//=============================================================================================

   POLYGON::POLYGON(void)
   {
      numpoints   = 0;              
      pointlist   = NULL;
      orientation = CLOCKWISE;
   }
                                  
   POLYGON::POLYGON(int n, vector* p)
   {
      numpoints   = n;
      pointlist   = p;
      orientation = findOrientation();
   }

   POLYGON::POLYGON(const POLYGON& P)
   {
      numpoints   = P.numpoints;
      pointlist   = new vector[numpoints];
      for (int i=0;i<numpoints;i++)
         pointlist[i]=P.pointlist[i];
      orientation = P.orientation;
   }


//=============================================================================================
//  POLYGON::findOrientation()                                                       (PRIVATE)
//---------------------------------------------------------------------------------------------
//
//  This function calculates the orientation of a POLYGON.
//
//=============================================================================================

   int POLYGON::findOrientation(void)
   {
      double area;
      int    i;

      area =  pointlist[numpoints-1].x * pointlist[0].y
            - pointlist[0].x * pointlist[numpoints-1].y;

      for (i=0;i<numpoints-1;i++)
          area +=  pointlist[i].x * pointlist[i+1].y
                 - pointlist[i+1].x * pointlist[i].y;

      if (area >= 0.0) 
          return ANTICLOCKWISE;
      else
          return CLOCKWISE;
   }


//=============================================================================================
//  POLYGON::findDeterminant()                                                       (PRIVATE)
//---------------------------------------------------------------------------------------------
//
//  Finds the orientation of the triangle formed by connecting the POLYGON's p1, p2, and
//  p3 vertices.
//
//=============================================================================================

   int POLYGON::findDeterminant(int p1, int p2, int p3)
   {
      double determinant;

      determinant = (pointlist[p2].x-pointlist[p1].x)
                   *(pointlist[p3].y-pointlist[p1].y)
                   -(pointlist[p3].x-pointlist[p1].x)
                   *(pointlist[p2].y-pointlist[p1].y);

      if (determinant > 0.0)
          return ANTICLOCKWISE;
      else if (determinant==0.0)
      {
         if(   pointlist[p1]==pointlist[p2] 
            || pointlist[p1]==pointlist[p3] 
            || pointlist[p2]==pointlist[p3])
         {
            return CLOCKWISE;
         }
         else
         {
            return ANTICLOCKWISE;
         }
      }
      else
          return CLOCKWISE;
   }


//=============================================================================================
//  POLYGON::noneInside()                                                            (PRIVATE)
//---------------------------------------------------------------------------------------------
//
//  Returns 'FALSE' if any of the POLYGON's vertices in 'vlist' are inside the triangle formed
//  by connecting the vertices p1, p2, and p3.  'n' is the number of vertices in 'vlist'.
//  Returns 'TRUE' if no vertices are inside that triangle.
//
//=============================================================================================

   int POLYGON::noneInside(int p1, int p2, int p3, int n, int* vlist)
   {
      int i,p;

      for(i=0;i<n;i++)
      {
         p=vlist[i];
         if((p==p1)||(p==p2)||(p==p3)) continue;
         if (   (findDeterminant(p2,p1,p)==orientation)
             || (findDeterminant(p1,p3,p)==orientation)
             || (findDeterminant(p3,p2,p)==orientation))  continue;
         else
         {
            if (  (pointlist[p].x==pointlist[p1].x && pointlist[p].y==pointlist[p1].y)
                ||(pointlist[p].x==pointlist[p2].x && pointlist[p].y==pointlist[p2].y)
                ||(pointlist[p].x==pointlist[p3].x && pointlist[p].y==pointlist[p3].y))
               continue;
            else
               return FALSE;
         }
      }
      return TRUE;
   }


//=============================================================================================
//  POLYGON::Correct()                                                                (PUBLIC)
//---------------------------------------------------------------------------------------------
//
//  This routine just makes sure there are no two consecutive points in the polygon that are
//  the same.  If there are the duplicates are deleted from the vertex list.
//
//=============================================================================================

   void POLYGON::Correct() 
   { 
      int i,j; 
      for (i=0;i<numpoints-1;i++) 
      { 
         if (pointlist[i]==pointlist[i+1]) 
         {
            for (j=i;j<numpoints-1;j++) 
               pointlist[j]=pointlist[j+1]; 
            numpoints--; i--; 
         }
      }
   }   


//=============================================================================================
//  POLYGON::Triangulate()                                                            (PUBLIC)
//---------------------------------------------------------------------------------------------
//
//  Slices the POLYGON up into a list of triangles.  The POLYGON must be non-intersecting.
//  This is done by checking each vertex to see whether or not it can be 'chopped off'.
//  If so, that vertex is removed, and the process is repeated until there are only three
//  vertices left (only one triangle left).  Returns the following values upon completion:
//
//       GEOM_ERR_NoPolyFound .......... If there were more than three vertices left, but none
//                                       of them could be 'chopped off'.  This usually happens
//                                       if the polygon intersects itself.
//
//       GEOM_GEOM_ERR_NoError .............. If the polygon was successfully triangulated.
//
//
//=============================================================================================*/

   int POLYGON::Triangulate(TRIANGLELIST& trilist, int verbose)
   {
      TRIANGLE*         current_triangle;
      int               previous;
      int               current;
      int               next;
      int*              rvl;
      int               vertex_count;
      int               current_determinant;
      int               current_position;
      int               i;
      vector            p1,p2,p3;
      int               done;
      vector            n1(0,0,1);
      vector            n2(0,0,1);
      vector            n3(0,0,1);

      char*             progChar  = "/-\\|";   //  Character Array to animate progress with
      int               progCount = 0;         //  Keeps track of progress
      int               progSize  = 4;         //  Number of characters in animation sequence

      rvl = new int[numpoints];
      for (i=0;i<numpoints;i++) 
         rvl[i]=i;

      vertex_count=numpoints;
      while (vertex_count>3)
      {
         if (verbose) { cout<<progChar[(progCount++)%progSize]; cout.flush(); }

         done=FALSE;
         previous=vertex_count-1;
         current=0;
         next=1;
         while (current<vertex_count && !done)
         {
            previous = current-1;
            next     = current+1;

            if (current==0)
               previous=vertex_count-1;
            else if (current==vertex_count-1)
               next=0;

            current_determinant = findDeterminant(rvl[previous],
                                                  rvl[current],
                                                  rvl[next]);
            current_position = noneInside(rvl[previous] ,
                                          rvl[current]  ,
                                          rvl[next]     ,
                                          vertex_count  ,
                                          rvl          );

            if (   (current_determinant==orientation)
                && (current_position==TRUE))
            {
               done=TRUE;
            }
            else
            {
               current++;
            }
         }

         if (!done)
         {
            return GEOM_ERR_NoPolyFound;
         }

         p1=vector(pointlist[rvl[previous]]);
         p2=vector(pointlist[rvl[current]]);
         p3=vector(pointlist[rvl[next]]);

         current_triangle = new TRIANGLE(p1,p2,p3,n1,n2,n3);
         trilist.Add(current_triangle);

         vertex_count-=1;
         for (i=current;i<vertex_count;i++) rvl[i]=rvl[i+1];

         if (verbose) { cout<<'\b'; cout.flush(); }

      }

      p1=vector(pointlist[rvl[0]]);
      p2=vector(pointlist[rvl[1]]);
      p3=vector(pointlist[rvl[2]]);

      current_triangle = new TRIANGLE(p1,p2,p3,n1,n2,n3);
      trilist.Add(current_triangle);

      delete rvl;

      return GEOM_ERR_NoError;

   }


//=============================================================================================
//  POLYGON::Combine()                                                                (PUBLIC)
//---------------------------------------------------------------------------------------------
//
//  This function combines two polygons by cutting between them at their point of closest
//  approach (PCA).  The resulting polygon is found by tracing around it's own vertices from
//  the PCA all the way back around to and including the PCA.  Then, adding an edge to the
//  PCA of the polygon we're combining, tracing around the polygon we're combining (in the
//  opposite direction), and finally adding an edge from the inner PCA to the outer PCA.
//
//=============================================================================================

   void POLYGON::Combine(POLYGON& p)
   {
      int      i,ni,j;
      double   current_dist;
      double   min_dist;
      int      min_i=0;
      int      min_j=0;
      vector   currToPrev, currToNext, mintoPrev, mintoNext;
      vector   testvector;
      double   distCP,distCN,distMP,distMN;
      vector*  newpl;

      newpl = new vector[numpoints+p.numpoints+2];

      min_dist = BIG;
      for (i=0;i<numpoints;i++)
      {
         for (j=0;j<p.numpoints;j++)
         {
            current_dist = dist(pointlist[i],p.pointlist[j]);

            if (current_dist==min_dist)
            {
               if (i>0) currToPrev = pointlist[i-1]-pointlist[i];
               else currToPrev = pointlist[numpoints-1]-pointlist[i];
               if (i<numpoints-1) currToNext = pointlist[i+1]-pointlist[i];
               else currToNext = pointlist[0]-pointlist[i];

               if (min_i>0) mintoPrev = pointlist[min_i-1]-pointlist[min_i];
               else mintoPrev = pointlist[numpoints-1]-pointlist[min_i];
               if (min_i<numpoints-1) mintoNext = pointlist[min_i+1]-pointlist[min_i];
               else mintoNext = pointlist[0]-pointlist[min_i];

               testvector = p.pointlist[j]-pointlist[i];
 
               distCP = dist(~currToPrev,testvector); // Changed from being normalized...
               distCN = dist(~currToNext,testvector);
               distMP = dist(~mintoPrev ,testvector);
               distMN = dist(~mintoNext ,testvector);

               if (  (distCP+distCN)<(distMP+distMN))
               {
                  min_dist=current_dist;
                  min_i=i;
                  min_j=j;
               }
            }
            else if (current_dist<min_dist)
            {
               min_dist = current_dist;
               min_i    = i;
               min_j    = j;
            }
         }
      }

      ni=0;

      for(i=0     ; i<=min_i      ;i++)
      {
         newpl[ni]=pointlist[i];
         ni++; 
      }
      for(i=min_j ; i<p.numpoints ;i++)
      {
         newpl[ni]=p.pointlist[i];
         ni++;
      }
      for(i=0     ; i<=min_j      ;i++)
      {
         newpl[ni]=p.pointlist[i];
         ni++; 
      }
      for(i=min_i ; i<numpoints   ;i++)
      {
         newpl[ni]=pointlist[i];
         ni++;
      }

      numpoints = ni;
      delete pointlist;
      pointlist = newpl;

   }


//=============================================================================================
//  POLYGON::isInside                                                                 (PUBLIC)
//---------------------------------------------------------------------------------------------
//
//  This function determines whether or not a polygon (the one it is called upon) is entirely
//  inside another polygon (the one passed as a parameter: 'p').  If it is, the function ret-
//  urns TRUE, FALSE otherwise.
//
//=============================================================================================

   int POLYGON::isInside(POLYGON& p)
   {
      int i,j,c=0;
      double x,y;

      x = pointlist[0].x;
      y = pointlist[0].y;

      for (i=0, j=p.numpoints-1; i<p.numpoints; j=i++)
      {
         if ((((p.pointlist[i].y<=y) && (y<p.pointlist[j].y)) ||
              ((p.pointlist[j].y<=y) && (y<p.pointlist[i].y))) &&
             (x < (p.pointlist[j].x - p.pointlist[i].x) * (y - p.pointlist[i].y) /
             (p.pointlist[j].y - p.pointlist[i].y) + p.pointlist[i].x))

            c = !c;
      }
      return c;
   }


//=============================================================================================
//  POLYGON::Shrink                                                                   (PUBLIC)
//---------------------------------------------------------------------------------------------
//
//  This function shifts each vertex of a polygon inward a small amount, along a direction
//  vector that bisects the angle created by the vertex's two adjacent edges.  The amount
//  of movement is specified by 'shrinkFactor'; if it is positive the movement is inward, if
//  it is negative the movement is toward the outside of the polygon.  The resulting 'shrunk'
//  polygon is stored in 'newPoly'.  If this polygon is not already empty (if it has any
//  vertices at all), then its vertices are deleted before creating the new polygon.
//
//  NOTE:  The algorithm used here is not foolproof.  If relatively large shrinkFactors are
//         given, the resulting 'shrunk' polygon can become self-intersecting.
//
//         Also, two versions are provided; one that creates a new polygon, one that modifies 
//         the points of the old polygon.
//
//=============================================================================================

//---------------------------------------------------------------------------------------------
//  VERSION 1:  Does not modify the original polygon.
//---------------------------------------------------------------------------------------------

   void POLYGON::Shrink(POLYGON& newPoly, double shrinkFactor)
   {
  
      int i;
      vector current,previous,next;
      vector toPrevious,toNext,inPrevious,inNext;
      vector inward;
      double angle;
      double shrinkDist;
      vector zDir(0,0,1);
      double bisectorLength;

      if (shrinkFactor==0) return;

      if (newPoly.numpoints>0)
      {
         delete newPoly.pointlist;
         newPoly.numpoints = 0;
      }
 
      newPoly.pointlist = new vector[numpoints];
      newPoly.numpoints = numpoints;

      previous = pointlist[numpoints-1];
      current = pointlist[0];
      next = pointlist[1];

      toPrevious = ~(previous-current);
      toNext     = ~(next-current);

      inPrevious = zDir^toPrevious;
      inNext     = toNext^zDir;

      bisectorLength = toPrevious%toNext;
      if (bisectorLength < -1.0) 
         bisectorLength = -1.0;
      else if (bisectorLength > 1.0)
         bisectorLength = 1.0;

      angle  = 0.5*acos(bisectorLength);
      if (angle<MIN_SHRINK_ANGLE) angle=MIN_SHRINK_ANGLE;
      shrinkDist = shrinkFactor/sin(angle);
      inward = ~(inPrevious+inNext)*shrinkDist;

      newPoly.pointlist[0]=current+inward;

      previous = pointlist[numpoints-2];
      current = pointlist[numpoints-1];
      next = pointlist[0];

      toPrevious = ~(previous-current);
      toNext     = ~(next-current);

      inPrevious = zDir^toPrevious;
      inNext     = toNext^zDir;

      bisectorLength = toPrevious%toNext;
      if (bisectorLength < -1.0) 
         bisectorLength = -1.0;
      else if (bisectorLength > 1.0)
         bisectorLength = 1.0;

      angle  = 0.5*acos(bisectorLength);
      if (angle<MIN_SHRINK_ANGLE) angle=MIN_SHRINK_ANGLE;
      shrinkDist = shrinkFactor/sin(angle);
      inward = ~(inPrevious+inNext)*shrinkDist;

      newPoly.pointlist[numpoints-1]=current+inward;

 
      for (i=1;i<numpoints-1;i++)
      {
         previous = pointlist[i-1];
         current = pointlist[i];
         next = pointlist[i+1];

         toPrevious = ~(previous-current);
         toNext     = ~(next-current);

         inPrevious = zDir^toPrevious;
         inNext     = toNext^zDir;

         bisectorLength = toPrevious%toNext;
         if (bisectorLength < -1.0) 
            bisectorLength = -1.0;
         else if (bisectorLength > 1.0)
            bisectorLength = 1.0;

         angle  = 0.5*acos(bisectorLength);
         if (angle<MIN_SHRINK_ANGLE) angle=MIN_SHRINK_ANGLE;
         shrinkDist = shrinkFactor/sin(angle);
         inward = ~(inPrevious+inNext)*shrinkDist;

         newPoly.pointlist[i]=current+inward;
      }

   }


//---------------------------------------------------------------------------------------------
//  VERSION 2:  Modifies the original polygon's points
//---------------------------------------------------------------------------------------------

   void POLYGON::Shrink(double shrinkFactor)
   {
      int i;
      vector current,previous,next;
      vector toPrevious,toNext,inPrevious,inNext;
      vector inward;
      double angle;
      double shrinkDist;
      vector zDir(0,0,1);
      vector* newpointlist;
      double bisectorLength;

      if (shrinkFactor==0) return;

      newpointlist = new vector[numpoints];

      previous = pointlist[numpoints-1];
      current = pointlist[0];
      next = pointlist[1];

      toPrevious = ~(previous-current);
      toNext     = ~(next-current);

      inPrevious = zDir^toPrevious;
      inNext     = toNext^zDir;

      bisectorLength = toPrevious%toNext;
      if (bisectorLength < -1.0) 
         bisectorLength = -1.0;
      else if (bisectorLength > 1.0)
         bisectorLength = 1.0;

      angle  = 0.5*acos(bisectorLength);
      if (angle<MIN_SHRINK_ANGLE) angle=MIN_SHRINK_ANGLE;
      shrinkDist = shrinkFactor/sin(angle);
      inward = ~(inPrevious+inNext)*shrinkDist;

      newpointlist[0]=current+inward;

      previous = pointlist[numpoints-2];
      current = pointlist[numpoints-1];
      next = pointlist[0];

      toPrevious = ~(previous-current);
      toNext     = ~(next-current);

      inPrevious = zDir^toPrevious;
      inNext     = toNext^zDir;

      bisectorLength = toPrevious%toNext;
      if (bisectorLength < -1.0) 
         bisectorLength = -1.0;
      else if (bisectorLength > 1.0)
         bisectorLength = 1.0;

      angle  = 0.5*acos(bisectorLength);
      if (angle<MIN_SHRINK_ANGLE) angle=MIN_SHRINK_ANGLE;
      shrinkDist = shrinkFactor/sin(angle);
      inward = ~(inPrevious+inNext)*shrinkDist;

      newpointlist[numpoints-1]=current+inward;

 
      for (i=1;i<numpoints-1;i++)
      {
         previous = pointlist[i-1];
         current = pointlist[i];
         next = pointlist[i+1];

         toPrevious = ~(previous-current);
         toNext     = ~(next-current);

         inPrevious = zDir^toPrevious;
         inNext     = toNext^zDir;

         bisectorLength = toPrevious%toNext;
         if (bisectorLength < -1.0) 
            bisectorLength = -1.0;
         else if (bisectorLength > 1.0)
            bisectorLength = 1.0;

         angle  = 0.5*acos(bisectorLength);
         if (angle<MIN_SHRINK_ANGLE) angle=MIN_SHRINK_ANGLE;
         shrinkDist = shrinkFactor/sin(angle);
         inward = ~(inPrevious+inNext)*shrinkDist;

         newpointlist[i]=current+inward;
      }

      delete pointlist;
      pointlist = newpointlist;

   }



//=============================================================================================
//  POLYGON::SetDepth()                                                               (PUBLIC)
//---------------------------------------------------------------------------------------------
//
//  This function simply sets the z-coordinate of each of the vertices in the polygon to a
//  specified value 'depth'.
//
//=============================================================================================

   void POLYGON::SetDepth(double depth)
   {
      for (int i=0;i<numpoints;i++)
         pointlist[i].z = depth;
   }


//=============================================================================================
//  POLYGON::SetDepth()                                                               (PUBLIC)
//---------------------------------------------------------------------------------------------
//
//  This function translates each point in the polygon by adding 'offset' to each of its
//  points.
//
//=============================================================================================

   void POLYGON::Translate(vector offset)
   {
      for (int i=0;i<numpoints;i++)
         pointlist[i] = pointlist[i]+offset;
   }


//=============================================================================================
//  ApproximateQuadraticSpline()                                                      (PUBLIC)
//---------------------------------------------------------------------------------------------
//
//  This function evaluates a quadratic B-spline curve, specified by its three control points
//  (cp1,cp2,cp3), at an arbitrary point along that curve.  The return value is only meaning-
//  ful if the parameter 't' is between 0 and 1, inclusively.
//
//=============================================================================================

   vector ApproximateQuadraticSpline(vector cp1, vector cp2, vector cp3, double t)
   {
      double i1 = (1-t)*(1-t);
      double i2 = 2*t*(1-t);
      double i3 = t*t;

      double tx = i1*cp1.x + i2*cp2.x + i3*cp3.x;
      double ty = i1*cp1.y + i2*cp2.y + i3*cp3.y;
      double tz = cp1.z;

      return vector(tx,ty,tz);
   }