/*****************************************************************************
*   "Irit" - the 3d polygonal solid modeller.				     *
*									     *
* Written by:  Gershon Elber				Ver 0.2, Mar. 1990   *
******************************************************************************
*   Module to handle convexity of polygons: Test for convexity, and split    *
* none convex polygons into convex ones.				     *
*****************************************************************************/

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

#ifdef __MSDOS__
#include <alloc.h>
#endif /* __MSDOS__ */

#include <stdio.h>
#include <ctype.h>
#include <math.h>
#include <string.h>
#include "program.h"
#include "geomat3d.h"
#include "booleang.h"
#include "objects.h"
#include "allocatg.h"
#include "priorqg.h"
#include "convexg.h"
#include "convexl.h"

static void SplitPolyIntoTwo(PolygonStruct *Pl, VertexStruct *V,
				PolygonStruct **Pl1, PolygonStruct **Pl2);
static VertexStruct *FindRayPolyInter(PolygonStruct *Pl,
		VertexStruct *VRay, PointType RayDir, PointType PInter);
static void TestConvexityDir(PolygonStruct *Pl);

#ifdef DEBUG2
static void PrintVrtxList(VertexStruct *V);
#endif /* DEBUG2 */

/*****************************************************************************
*   Routine to prepare transformation martix to rotate such that Dir is	     *
* parallel to the Z axes. Used by the convex decomposition to rotate the     *
* polygons to be XY plane parallel.					     *
*    Algorithm: form a 4 by 4 matrix from Dir as follows:		     *
*                |  Tx  Ty  Tz  0 |   A transformation which takes the coord *
*                |  Bx  By  Bz  0 |  system into T, N & B as required.	     *
* [X  Y  Z  1] * |  Nx  Ny  Nz  0 |					     *
*                |  0   0   0   1 |					     *
* N is exactly Dir, but we got freedom on T & B - T & B must be on	     *
* a plane perpendicular to N and perpendicular between them but thats all!   *
* T is therefore selected using this (heuristic ?) algorithm:		     *
* Let P be the axis of which the absolute N coefficient is the smallest.     *
* Let B be (N cross P) and T be (B cross N).				     *
*****************************************************************************/
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. */

    VecCrossProd(B, DirN, P);			      /* calc the bi-normal. */
    PT_NORMALIZE(B);
    VecCrossProd(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];
    }
}

/*****************************************************************************
*   Routine to test all polygons in a given object for convexity, and split  *
* non convex ones, non destructively - the original object is not modified.  *
*****************************************************************************/
ObjectStruct *ConvexPolyObjectN(ObjectStruct *PObj)
{
    ObjectStruct *PObjCopy;

    PObjCopy = CopyObject(NULL, PObj, FALSE);
    ConvexPolyObject(PObjCopy);


    return PObjCopy;
}

/*****************************************************************************
*   Routine to test all the polygons in a given object for convexity, and    *
* split the non convex ones.						     *
*****************************************************************************/
void ConvexPolyObject(ObjectStruct *PObj)
{
    PolygonStruct *Pl, *PlPrev = NULL, *PlSplit, *PlTemp;

    if (!IS_GEOM_OBJ(PObj))
	FatalError("ConvexPolyObject: parameter given is not geometric object");

    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;

		Pl -> Pnext = NULL;
		MyFree((char *) Pl, POLYGON_TYPE);	/* Free old polygon. */
		Pl = PlPrev -> Pnext;
	    }
	    else {
		if (Pl == PObj -> U.Pl) PObj -> U.Pl = Pl -> Pnext;
		PlPrev = Pl -> Pnext;
		Pl -> Pnext = NULL;
		MyFree((char *) Pl, POLYGON_TYPE);	/* Free old polygon. */
		Pl = PlPrev;
	    }
	}
	else {
	    PlPrev = Pl;
	    Pl = Pl -> Pnext;
	}
    }
}

/*****************************************************************************
*   Routine to test if the given polygon is convex or not.		     *
* Algorithm: The polygon is convex iff the normals generated from cross      *
* products of two consecutive edges points to the same direction. he same    *
* direction is tested by dot product with the polygon plane normal which     *
* should produce consistent sign for all normals, in order the polygon to    *
* be convex.								     *
*   The routine returns TRUE iff the polygon is convex. In addition the      *
* polygon CONVEX tag (see Irit.h) is also updated.			     *
*   Note that if the polygon IS marked as convex, nothing is tested!	     *
*   Also convex polygons are tested so that the vertices are ordered such    *
* that cross product of two adjacent edges will be in the normal direction.  *
*****************************************************************************/
int ConvexPolygon(PolygonStruct *Pl)
{
    int FirstTime = TRUE;
    RealType NormalSign;
    VectorType V1, V2, PolyNormal, Normal;
    VertexStruct *V = Pl -> V, *VNext;

    if (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 -> Pt, V -> Pt);
	PT_SUB(V2, VNext -> Pnext -> Pt, VNext -> Pt);
	VecCrossProd(Normal, V1, V2);
	if (PT_LENGTH(Normal) < 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) {
	    RST_CONVEX_POLY(Pl);
	    return FALSE;		  /* Different signs --> not convex. */
	}

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

    SET_CONVEX_POLY(Pl);

    if (NormalSign < 0.0) ReverseVrtxList(Pl);

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

/*****************************************************************************
*   Routine to split non convex polygon to list of convex ones.		     *
* This algorithm is much simpler than the one used before:		     *
* 1. Remove a polygon from GlblList. If non exists stop.		     *
* 2. Search for non convex corner. If not found stop - polygon is convex.    *
*    Otherwise let the non convex polygon found be V(i).		     *
* 3. Fire a ray from V(i) in the opposite direction to V(i-1). Find the      *
*    closest intersection of the ray with polygon boundary P.		     *
* 4. Split the polygon into two at V(i)-P edge and push the two new polygons *
*    on the GlblList.							     *
* 5. Goto 1.								     *
*****************************************************************************/
PolygonStruct *SplitNonConvexPoly(PolygonStruct *Pl)
{
    int IsConvex;
    PolygonStruct *GlblList, *GlblSplitPl = NULL, *Pl1, *Pl2;
    VectorType V1, V2, PolyNormal, Normal;
    VertexStruct *V, *VNext;

    TestConvexityDir(Pl);

    GlblList = AllocPolygon(0, 0, CopyVList(Pl -> V), 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 -> V;
	do {
	    VNext = V -> Pnext;

	    PT_SUB(V1, VNext -> Pt, V -> Pt);
	    PT_SUB(V2, VNext -> Pnext -> Pt, VNext -> Pt);
	    VecCrossProd(Normal, V1, V2);
	    if (PT_LENGTH(Normal) < EPSILON) {
		V = VNext;
		continue;			    /* Skip colinear points. */
	    }

	    if (DOT_PROD(Normal, PolyNormal) < 0.0) {
		SplitPolyIntoTwo(Pl, V, &Pl1, &Pl2);

		Pl -> V = NULL;	      /* Dont free vertices - used in Pl1/2. */
		Pl -> Pnext = NULL;
		MyFree((char *) Pl, POLYGON_TYPE);

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

		IsConvex = FALSE;
		break;
	    }

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

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

    return GlblSplitPl;
}

/*****************************************************************************
*   Split polygon 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 P. (V -> Pnext, P) is the edge to    *
* split the polygon at.							     *
*****************************************************************************/
static void SplitPolyIntoTwo(PolygonStruct *Pl, VertexStruct *V,
				PolygonStruct **Pl1, PolygonStruct **Pl2)
{
    PointType Vl, PInter;
    VertexStruct *VInter, *VNew1, *VNew2;

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

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

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

/*****************************************************************************
*   Find where a ray first intersect a given polygon. The ray starts at one  *
* of the polygon 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.			     *
*****************************************************************************/
static VertexStruct *FindRayPolyInter(PolygonStruct *Pl,
			VertexStruct *VRay, PointType RayDir, PointType PInter)
{
    RealType t1, t2, MinT = INFINITY;
    PointType Vl, Ptemp1, Ptemp2, PRay;
    VertexStruct *V = Pl -> V, *VNext, *VInter = NULL;

    PT_COPY(PRay, VRay -> Pt);

    do {
	VNext = V -> Pnext;
	if (V != VRay && VNext != VRay) {
	    PT_SUB(Vl, V -> Pnext -> Pt, V -> Pt);
	    if (CGDistPointLine(PRay, V -> Pt, Vl) > EPSILON) {
		/* Only if the point the ray is shoot from is not on line: */
		CG2PointsFromLineLine(PRay, RayDir, V -> Pt, Vl, Ptemp1, &t1,
								 Ptemp2, &t2);
		if (CGDistPointPoint(Ptemp1, Ptemp2) < EPSILON * 10.0 &&
		    t1 > 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 -> V);

    if (VInter == NULL)
	FatalError("FindRayPolyInter: No Ray intersection in Convex decomp.");

    return VInter;
}

/*****************************************************************************
*   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.			     *
*****************************************************************************/
static void TestConvexityDir(PolygonStruct *Pl)
{
    int Coord = 0;
    VectorType V1, V2, Normal;
    VertexStruct *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 -> V;
    do {
	if (V -> Pt[Coord] > VExtrem -> Pt[Coord]) VExtrem = V;
	V = V -> Pnext;
    }
    while (V != Pl -> V);

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

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

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

/*****************************************************************************
*   Reverse the vertex list of a given polygon. This is used mainly to       *
* reverse polygons such that cross product of consecutive edges which form   *
* a convex corner will point in the polygon normal direction.		     *
*****************************************************************************/
void ReverseVrtxList(PolygonStruct *Pl)
{
    ByteType Tags, Count;
    VertexStruct *V = Pl -> V, *VNext = V -> Pnext, *VNextNext;

    do {
	VNextNext = VNext -> Pnext;
	VNext -> Pnext = V;			     /* Reverse the pointer! */

	V = VNext;			   /* Advance all 3 pointers by one. */
	VNext = VNextNext;
	VNextNext = VNextNext -> Pnext;
    }
    while (V != Pl -> V);

    V = Pl -> V;	  /* Move the Tags/Count by one - to the right edge. */
    Tags = V -> Tags;
    Count = V -> Count;
    do {
	if (V -> Pnext == Pl -> V) {
	    V -> Tags = Tags;
	    V -> Count = Count;
	}
	else {
	    V -> Tags = V -> Pnext -> Tags;
	    V -> Count = V -> Pnext -> Count;
	}

	V = V -> Pnext;
    }
    while (V != Pl -> V);
}

#ifdef DEBUG2

/*****************************************************************************
*   Print the content of the given vertex list, to standard output.	     *
*****************************************************************************/
static void PrintVrtxList(VertexStruct *V)
{
    VertexStruct *VFirst = V;

    if (V == NULL) return;

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

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

#endif /* DEBUG2 */