/******************************************************************************
* Convex.c - test convexity and converts polygons to convex ones.	      *
*******************************************************************************
* Written by Gershon Elber, March 1990.					      *
******************************************************************************/

/* #define DEBUG2		   Defines some printing/debugging routines. */

#include <stdio.h>
#include <ctype.h>
#include <math.h>
#include <string.h>
#include "allocate.h"
#include "convex.h"
#include "poly_cln.h"
#include "geomat3d.h"
#include "intrnrml.h"
#include "priorque.h"

/* Used to hold edges (V | V -> Pnext) that intersect with level y = Ylevel. */
typedef struct InterYVrtxList {
    ByteType InterYType;
    IPVertexStruct *V;
    struct InterYVrtxList *Pnext;
} InterYVrtxList;

#define CONVEX_EPSILON  1e-8	   /* Colinearity of two normalized vectors. */

#define	INTER_Y_NONE	0		       /* Y level intersection type. */
#define	INTER_Y_START	1
#define	INTER_Y_MIDDLE	2

#define LOOP_ABOVE_Y	0      /* Type of open loops extracted from polygon. */
#define LOOP_BELOW_Y	1

/* Used to sort and combine the polygons above Ylevel together if possible.  */
typedef struct SortPolysInX {
    IPVertexStruct *VMinX, *VMaxX;
    int Reverse; 	 /* If TRUE than VMinX vertex is AFTER VMaxX vertex. */
} SortPolysInX;

/* The following are temporary flags used to mark vertices that were visited */
/* by the loop tracing, at list once. As each vertex may be visited two at   */
/* the most (as starting & as end point of open loop), this is enough.	     */
/* INTER_TAG is used to mark vertices that created brand new with intersected*/
/* with the line y = Ylevel. Those are used to detect INTERNAL edges - if    */
/* at list one end of it is INTER_TAG, that edge is INTERNAL.		     */
#define INTER_TAG   0x40
#define VISITED_TAG 0x80

#define	IS_INTER_VRTX(Vrtx)	((Vrtx)->Tags & INTER_TAG)
#define	SET_INTER_VRTX(Vrtx)	((Vrtx)->Tags |= INTER_TAG)
#define	RST_INTER_VRTX(Vrtx)	((Vrtx)->Tags &= ~INTER_TAG)

#define	IS_VISITED_VRTX(Vrtx)	((Vrtx)->Tags & VISITED_TAG)
#define	SET_VISITED_VRTX(Vrtx)	((Vrtx)->Tags |= VISITED_TAG)
#define	RST_VISITED_VRTX(Vrtx)	((Vrtx)->Tags &= ~VISITED_TAG)

static int SplitPolyIntoTwo(IPPolygonStruct *Pl,
			    IPVertexStruct *V,
			    IPPolygonStruct **Pl1,
			    IPPolygonStruct **Pl2);
static IPVertexStruct *FindRayPolyInter(IPPolygonStruct *Pl,
					IPVertexStruct *VRay,
					PointType RayDir,
					PointType PInter);
static void TestConvexityDir(IPPolygonStruct *Pl);

#ifdef DEBUG2
static void PrintVrtxList(IPVertexStruct *V);
static void PrintPoly(IPPolygonStruct *P);
#endif /* DEBUG2 */

/*****************************************************************************
* DESCRIPTION:                                                               M
*   Routine to prepare a transformation martix to rotate such that Dir is    M
* parallel to the Z axes. Used by the convex decomposition to rotate the     M
* polygons to be XY plane parallel.					     M
*    Algorithm: form a 4 by 4 matrix from Dir as follows:		     M
*                |  Tx  Ty  Tz  0 |   A transformation which takes the coord V
*                |  Bx  By  Bz  0 |  system into T, N & B as required.	     V
* [X  Y  Z  1] * |  Nx  Ny  Nz  0 |					     V
*                |  0   0   0   1 |					     V
*   N is exactly Dir, but we got freedom on T & B which must be on	     M
* a plane perpendicular to N and perpendicular between them but thats all!   M
*   T is therefore selected using this (heuristic ?) algorithm:		     M
*   Let P be the axis of which the absolute N coefficient is the smallest.   M
*   Let B be (N cross P) and T be (B cross N).				     M
*                                                                            *
* PARAMETERS:                                                                M
*   Mat:     To place the constructed homogeneous transformation.            M
*   Dir:     To derive a transformation such that Dir goes to Z axis.        M
*                                                                            *
* RETURN VALUE:                                                              M
*   void                                                                     M
*                                                                            *
* KEYWORDS:                                                                  M
*   GenRotateMatrix, transformations                                         M
*****************************************************************************/
void GenRotateMatrix(MatrixType Mat, VectorType Dir)
{
    int i, j;
    RealType R;
    VectorType DirN, T, B, P;

    PT_COPY(DirN, Dir);
    PT_NORMALIZE(DirN);
    PT_CLEAR(P);
    for (i = 1, j = 0, R = ABS(DirN[0]); i < 3; i++)
	if (R > ABS(DirN[i])) {
	    R = DirN[i];
	    j = i;
	}
    P[j] = 1.0;/* Now P is set to the axis with the biggest angle from DirN. */

    GMVecCrossProd(B, DirN, P);			      /* calc the bi-normal. */
    PT_NORMALIZE(B);
    GMVecCrossProd(T, B, DirN);				/* calc the tangent. */

    MatGenUnitMat(Mat);
    for (i = 0; i < 3; i++) {
	Mat[i][0] = T[i];
	Mat[i][1] = B[i];
	Mat[i][2] = DirN[i];
    }
}

/*****************************************************************************
* DESCRIPTION:                                                               M
*   Routine to test all polygons in a given object for convexity, and split  M
* non convex ones, non destructively - the original object is not modified.  M
*   This function will introduce new vertices to the split polygons.         M
*                                                                            *
* PARAMETERS:                                                                M
*   PObj:       To test for convexity of its polygons.                       M
*                                                                            *
* RETURN VALUE:                                                              M
*   IPObjectStruct *:  A duplocate of PObj, but with convex polygons only.   M
*                                                                            *
* KEYWORDS:                                                                  M
*   ConvexPolyObjectN, convexity, convex polygon                             M
*****************************************************************************/
IPObjectStruct *ConvexPolyObjectN(IPObjectStruct *PObj)
{
    IPObjectStruct
	*PObjCopy= CopyObject(NULL, PObj, FALSE);

    ConvexPolyObject(PObjCopy);

    return PObjCopy;
}

/*****************************************************************************
* DESCRIPTION:                                                               M
*   Routine to test all polygons in a given object for convexity, and split  M
* non convex ones, in place.                                                 M
*   This function will introduce new vertices to the split polygons.         M
*                                                                            *
* PARAMETERS:                                                                M
*   PObj:       To test for convexity of its polygons, and split into convex M
*               polygons non convex polygons found, in place. Either a       M
*		polygonal object or a list of polygonal objects.	     M
*                                                                            *
* RETURN VALUE:                                                              M
*   void	                                                             M
*                                                                            *
* KEYWORDS:                                                                  M
*   ConvexPolyObject, convexity, convex polygon                              M
*****************************************************************************/
void ConvexPolyObject(IPObjectStruct *PObj)
{
    IPPolygonStruct *Pl, *PlSplit, *PlTemp,
	*PlPrev = NULL;

    if (IP_IS_OLST_OBJ(PObj)) {
	int i = 0;
	IPObjectStruct *Obj;

	while ((Obj = ListObjectGet(PObj, i++)) != NULL)
	    ConvexPolyObject(Obj);

	return;
    }

    if (!IP_IS_POLY_OBJ(PObj) || IP_IS_POLYLINE_OBJ(PObj))
        return;

    Pl = PObj -> U.Pl;
    while (Pl != NULL) {
	if (!ConvexPolygon(Pl)) {
	    PlSplit = SplitNonConvexPoly(Pl);
	    CleanUpPolygonList(&PlSplit);	   /* Zero length edges etc. */
	    if (PlSplit != NULL) {	 /* Something is wrong here, ignore. */
		if (Pl == PObj -> U.Pl)
		    PObj -> U.Pl = PlSplit;			   /* First. */
		else
		    PlPrev -> Pnext = PlSplit;

		PlTemp = PlSplit;
		while (PlTemp -> Pnext != NULL)
		    PlTemp = PlTemp -> Pnext;
		PlTemp -> Pnext = Pl -> Pnext;
		PlPrev = PlTemp;
		IPFreePolygon(Pl);			/* Free old polygon. */
		Pl = PlPrev -> Pnext;
	    }
	    else {
		if (Pl == PObj -> U.Pl)
		    PObj -> U.Pl = Pl -> Pnext;
		PlPrev = Pl -> Pnext;
		IPFreePolygon(Pl);			/* Free old polygon. */
		Pl = PlPrev;
	    }
	}
	else {
	    PlPrev = Pl;
	    Pl = Pl -> Pnext;
	}
    }
}

/*****************************************************************************
* DESCRIPTION:                                                               M
*   Routine to test if the given polygon is convex or not.		     M
* Algorithm: The polygon is convex iff the normals generated from cross      M
* products of two consecutive edges points to the same direction.            M
*   Note a 5 star polygon satisfies this constraint but it is self           M
* intersectingand we assume given polygon is not selft intersecting.         M
*   The computed direction is alos verified against the polygon's plane      M
* normal.								     M
*   The routine returns TRUE iff the polygon is convex. In addition the      M
* polygon CONVEX tag (see IPPolygonStruct) is also updated.		     M
*   If the polygon is already marked as convex, nothing is tested!	     M
*                                                                            *
* PARAMETERS:                                                                M
*   Pl:        To test its convexity condition.                              M
*                                                                            *
* RETURN VALUE:                                                              M
*   int:       TRUE if convex, FALSE otherwise.                              M
*                                                                            *
* KEYWORDS:                                                                  M
*   ConvexPolygon, convexity, convex polygon                                 M
*****************************************************************************/
int ConvexPolygon(IPPolygonStruct *Pl)
{
    int FirstTime = TRUE;
    RealType Size,
	NormalSign = 0.0;
    VectorType V1, V2, PolyNormal, Normal;
    IPVertexStruct *VNext,
	*V = Pl -> PVertex;

    if (IP_IS_CONVEX_POLY(Pl))
	return TRUE;			       /* Nothing to do around here. */

    /* Copy only A, B, C from Ax+By+Cz+D = 0: */
    PT_COPY(PolyNormal, Pl -> Plane);

    do {
	VNext = V -> Pnext;

	PT_SUB(V1, VNext -> Coord, V -> Coord);
	if ((Size = PT_LENGTH(V1)) > IRIT_EPSILON) {
	    Size = 1.0 / Size;
	    PT_SCALE(V1, Size);
	}
	PT_SUB(V2, VNext -> Pnext -> Coord, VNext -> Coord);
	if ((Size = PT_LENGTH(V2)) > IRIT_EPSILON) {
	    Size = 1.0 / Size;
	    PT_SCALE(V2, Size);
	}
	GMVecCrossProd(Normal, V1, V2);

	if (PT_LENGTH(Normal) < CONVEX_EPSILON) {
	    V = VNext;
	    continue;				    /* Skip colinear points. */
	}

	if (FirstTime) {
	    FirstTime = FALSE;
	    NormalSign = DOT_PROD(Normal, PolyNormal);
	}
	else if (NormalSign * DOT_PROD(Normal, PolyNormal) < 0.0) {
	    IP_RST_CONVEX_POLY(Pl);
	    return FALSE;		  /* Different signs --> not convex. */
	}

	V = VNext;
    }
    while (V != Pl -> PVertex);

    IP_SET_CONVEX_POLY(Pl);

    if (NormalSign < 0.0)
        IritPrsrReverseVrtxList(Pl);

    return TRUE;	/* All signs are the same --> the polygon is convex. */
}

/*****************************************************************************
* DESCRIPTION:                                                               M
*   Routine to split non convex polygon into a list of convex ones.	     M
* 1. Remove a polygon from GlblList. If non exists stop.		     M
* 2. Search for non convex corner. If not found stop - polygon is convex.    M
*    Otherwise let the non convex polygon found be V(i).		     M
* 3. Fire a ray from V(i) in the opposite direction to V(i-1). Find the      M
*    closest intersection of the ray with polygon boundary P.		     M
* 4. Split the polygon into two at V(i)-P edge and push the two new polygons M
*    on the GlblList.							     M
* 5. Goto 1.								     M
*                                                                            *
* PARAMETERS:                                                                M
*   Pl:        Non convex polygon to split into convex ones.                 M
*                                                                            *
* RETURN VALUE:                                                              M
*   IPPolygonStruct *:  A list of convex polygons resulting from splitting   M
*                       up Pl.                                               M
*                                                                            *
* KEYWORDS:                                                                  M
*   SplitNonConvexPoly, convexity, convex polygon                            M
*****************************************************************************/
IPPolygonStruct *SplitNonConvexPoly(IPPolygonStruct *Pl)
{
    int IsConvex;
    RealType Size;
    IPPolygonStruct *GlblList, *Pl1, *Pl2,
	*GlblSplitPl = NULL;
    VectorType V1, V2, PolyNormal, Normal;
    IPVertexStruct *V, *VNext;

    TestConvexityDir(Pl);

    GlblList = IPAllocPolygon(0, 0, CopyVertexList(Pl -> PVertex), NULL);
    PLANE_COPY(GlblList -> Plane, Pl -> Plane);

    /* Copy only A, B, C from Ax+By+Cz+D = 0 plane equation: */
    PT_COPY(PolyNormal, Pl -> Plane);

    while (GlblList != NULL) {
	Pl = GlblList;
	GlblList = GlblList -> Pnext;
	Pl -> Pnext = NULL;

	IsConvex = TRUE;
	V = Pl -> PVertex;
	do {
	    VNext = V -> Pnext;

	    PT_SUB(V1, VNext -> Coord, V -> Coord);
 	    if ((Size = PT_LENGTH(V1)) > IRIT_EPSILON) {
		Size = 1.0 / Size;
		PT_SCALE(V1, Size);
	    }
	    PT_SUB(V2, VNext -> Pnext -> Coord, VNext -> Coord);
 	    if ((Size = PT_LENGTH(V2)) > IRIT_EPSILON) {
		Size = 1.0 / Size;
		PT_SCALE(V2, Size);
	    }
	    GMVecCrossProd(Normal, V1, V2);
	    if (PT_LENGTH(Normal) < CONVEX_EPSILON) {
		V = VNext;
		continue;			    /* Skip colinear points. */
	    }

	    if (DOT_PROD(Normal, PolyNormal) < 0.0 &&
		SplitPolyIntoTwo(Pl, V, &Pl1, &Pl2)) {
		Pl -> PVertex = NULL; /* Dont free vertices - used in Pl1/2. */
		IPFreePolygon(Pl);

		Pl1 -> Pnext = GlblList;       /* Push polygons on GlblList. */
		GlblList = Pl1;
		Pl2 -> Pnext = GlblList;
		GlblList = Pl2;

		IsConvex = FALSE;
		break;
	    }

	    V = VNext;
	}
	while (V != Pl -> PVertex);

	if (IsConvex) {
	    IP_SET_CONVEX_POLY(Pl);
	    Pl -> Pnext = GlblSplitPl;
	    GlblSplitPl = Pl;
	}
    }

    return GlblSplitPl;
}

/*****************************************************************************
* DESCRIPTION:                                                               *
*   Split polygon Pl at the vertex specified by V -> Pnext, given V, into    *
* two, by firing a ray from V -> Pnext in the opposite direction to V and    *
* finding closest intersection with the polygon Pl. (V -> Pnext, P) is the   *
* edge to split the polygon at.						     *
*                                                                            *
* PARAMETERS:                                                                *
*   Pl:        Polygon to split at vertex V -> Pnext.                        *
*   V:         One vertex before the vertex to split Pl at.                  *
*   Pl1, Pl2:  Where the two new polygons should end up.                     *
*                                                                            *
* RETURN VALUE:                                                              *
*   int:       TRUE if succesful, FALSE otehrwise.                           *
*****************************************************************************/
static int SplitPolyIntoTwo(IPPolygonStruct *Pl,
			    IPVertexStruct *V,
			    IPPolygonStruct **Pl1,
			    IPPolygonStruct **Pl2)
{
    PointType Vl, PInter;
    IPVertexStruct *VInter, *VNew1, *VNew2;

    PT_SUB(Vl, V -> Pnext -> Coord, V -> Coord);
    VInter = FindRayPolyInter(Pl, V -> Pnext, Vl, PInter);
    V = V -> Pnext;

    if (VInter == NULL ||
	VInter == V ||
	VInter -> Pnext == V)
	return FALSE;

    /* Make the two polygon vertices lists. */
    VNew1 = IPAllocVertex(V -> Count, V -> Tags, NULL, V -> Pnext);
    PT_COPY(VNew1 -> Coord, V -> Coord);
    PT_COPY(VNew1 -> Normal, V -> Normal);
    IP_SET_INTERNAL_VRTX(V);
    if (PT_APX_EQ(VInter -> Coord, PInter)) {
	/* Intersection points is close to VInter point. */
	VNew2 = IPAllocVertex(VInter -> Count, VInter -> Tags, NULL,
							VInter -> Pnext);
	PT_COPY(VNew2 -> Coord, VInter -> Coord);
	PT_COPY(VNew2 -> Normal, VInter -> Normal);
	VInter -> Pnext = VNew1;
	IP_SET_INTERNAL_VRTX(VInter);
	V -> Pnext = VNew2;
    }
    else if (PT_APX_EQ(VInter -> Pnext -> Coord, PInter)) {
	/* Intersection points is close to VInter -> Pnext point. */
	VNew2 = IPAllocVertex(VInter -> Pnext -> Count,
			      VInter -> Pnext -> Tags,
			      NULL, VInter -> Pnext -> Pnext);
	PT_COPY(VNew2 -> Coord, VInter -> Pnext -> Coord);
	PT_COPY(VNew2 -> Normal, VInter -> Pnext -> Normal);
	VInter -> Pnext -> Pnext = VNew1;
	IP_SET_INTERNAL_VRTX(VInter -> Pnext);
	V -> Pnext = VNew2;
    }
    else {
	/* PInter is in the middle of (VInter, VInter -> Pnext) edge: */
	VNew2 = IPAllocVertex(VInter -> Count, VInter -> Tags, NULL,
							VInter -> Pnext);
	PT_COPY(VNew2 -> Coord, PInter);
	VInter -> Pnext = IPAllocVertex(0, 0, NULL, VNew1);
	PT_COPY(VInter -> Pnext -> Coord, PInter);
	InterpNrmlBetweenTwo(VNew2, VInter, VNew2 -> Pnext);
	PT_COPY(VInter -> Pnext -> Normal, VNew2 -> Normal);
	IP_SET_INTERNAL_VRTX(VInter -> Pnext);
	V -> Pnext = VNew2;
    }

    *Pl1 = IPAllocPolygon(0, 0, VNew1, NULL);
    PLANE_COPY((*Pl1) -> Plane, Pl -> Plane);
    *Pl2 = IPAllocPolygon(0, 0, VNew2, NULL);
    PLANE_COPY((*Pl2) -> Plane, Pl -> Plane);

    return TRUE;
}

/*****************************************************************************
* DESCRIPTION:                                                               *
*   Finds where a ray first intersect a given polygon. The ray starts at one *
* of the polygon's vertices and so distance less than EPSILON is ignored.    *
*   Returns the vertex V in which (V, V -> Pnext) has the closest	     *
* intersection with the ray PRay, DRay at Inter.			     *
*                                                                            *
* PARAMETERS:                                                                *
*   Pl:         Polygon to test for intersection with ray.                   *
*   VRay:       Origin of ray.                                               *
*   RayDir:     Direction of ray.                                            *
*   PInter:     location of intersection is to be placed herein.             *
*                                                                            *
* RETURN VALUE:                                                              *
*   IPVertexStruct *:   Vertex V such that edge (V, V -> Pnext) has the      *
*                       needed intersection.                                 *
*****************************************************************************/
static IPVertexStruct *FindRayPolyInter(IPPolygonStruct *Pl,
					IPVertexStruct *VRay,
					PointType RayDir,
					PointType PInter)
{
    RealType t1, t2,
	MinT = INFINITY;
    PointType Vl, Ptemp1, Ptemp2, PRay;
    IPVertexStruct *VNext,
	*V = Pl -> PVertex,
	*VInter = NULL;

    PT_COPY(PRay, VRay -> Coord);

    do {
	VNext = V -> Pnext;
	if (V != VRay && VNext != VRay) {
	    PT_SUB(Vl, VNext -> Coord, V -> Coord);
	    if (CGDistPointLine(PRay, V -> Coord, Vl) > IRIT_EPSILON) {
		/* Only if the point the ray is shoot from is not on line: */
		CG2PointsFromLineLine(PRay, RayDir, V -> Coord, Vl,
				      Ptemp1, &t1, Ptemp2, &t2);
		if (CGDistPointPoint(Ptemp1, Ptemp2) < IRIT_EPSILON * 10.0 &&
		    t1 > IRIT_EPSILON && t1 < MinT &&
		    (t2 <= 1.0 || APX_EQ(t2, 1.0)) &&
		    (t2 >= 0.0 || APX_EQ(t2, 0.0))) {
		    PT_COPY(PInter, Ptemp2);
		    VInter = V;
		    MinT = t1;
		}
	    }
	}
	V = VNext;
    }
    while (V != Pl -> PVertex && V -> Pnext != NULL);

    return VInter;
}

/*****************************************************************************
* DESCRIPTION:                                                               *
*   Test convexity direction - a cross product of two edges of a convex      *
* corner of the polygon should point in the normal direction. if this is not *
* the case - the polygon vertices are reveresed.			     *
*                                                                            *
* PARAMETERS:                                                                *
*   Pl:         To test of convexity direction.                              *
*                                                                            *
* RETURN VALUE:                                                              *
*   void                                                                     *
*****************************************************************************/
static void TestConvexityDir(IPPolygonStruct *Pl)
{
    int Coord = 0;
    VectorType V1, V2, Normal;
    IPVertexStruct *V, *VExtrem;

    /* Find the minimum component direction of the normal and used that axes */
    /* to find an extremum point on the polygon - that extrmum point must be */
    /* a convex corner so we can find the normal direction of convex corner. */
    if (ABS(Pl -> Plane[1]) < ABS(Pl -> Plane[Coord]))
	Coord = 1;
    if (ABS(Pl -> Plane[2]) < ABS(Pl -> Plane[Coord]))
	Coord = 2;
    V = VExtrem = Pl -> PVertex;
    do {
	if (V -> Coord[Coord] > VExtrem -> Coord[Coord])
	    VExtrem = V;
	V = V -> Pnext;
    }
    while (V != Pl -> PVertex && V != NULL);

    /* Make sure next vertex is not at the extremum value: */
    while (APX_EQ(VExtrem -> Coord[Coord], VExtrem -> Pnext -> Coord[Coord]))
	VExtrem = VExtrem -> Pnext;

    /* O.K. V form a convex corner - evaluate its two edges cross product:   */
    for (V = Pl -> PVertex; V -> Pnext != VExtrem; V = V -> Pnext); /* Prev. */
    PT_SUB(V1, VExtrem -> Coord, V -> Coord);
    PT_SUB(V2, VExtrem -> Pnext -> Coord, VExtrem -> Coord);
    GMVecCrossProd(Normal, V1, V2);

    if (DOT_PROD(Normal, Pl -> Plane) < 0.0)
	IritPrsrReverseVrtxList(Pl);
}

#ifdef DEBUG2

/*****************************************************************************
* DESCRIPTION:                                                               *
*   Print the content of the given vertex list, to standard output.	     *
*                                                                            *
* PARAMETERS:                                                                *
*   P:          Polygon to print its vertex list.                            *
*                                                                            *
* RETURN VALUE:                                                              *
*   void                                                                     *
*****************************************************************************/
static void PrintPoly(IPPolygonStruct *P)
{
    IPVertexStruct
	*VFirst = P -> PVertex,
	*V = VFirst;

    if (V == NULL)
	return;

    printf("VERTEX LIST:\n");
    do {
	printf("%12lg %12lg %12lg, Tags = %02x\n",
	    V -> Coord[0], V -> Coord[1], V -> Coord[2], V -> Tags);
	V = V -> Pnext;
    }
    while (V != NULL && V != VFirst);

    if (V == NULL)
	printf("Loop is not closed (NULL terminated)\n");
}

#endif /* DEBUG2 */
