/*******************************************************************************
* Line plane sweep algorithm implementation - actual code.		       *
*									       *
* Written by:  Gershon Elber                              Ver 1.0, June 1993   *
*******************************************************************************/

#include <stdio.h>
#include <math.h>
#include "irit_sm.h"
#include "imalloc.h"
#include "ln_sweep.h"

#define LS_XBBOX_OVERLAP(L1, L2) (L1 -> _MaxVals[0] > L2 -> _MinVals[0] && \
				  L2 -> _MaxVals[0] > L1 -> _MinVals[0])

#if defined(ultrix) && defined(mips)
static int LsSortCompare(VoidPtr Ptr1, VoidPtr Ptr2);
#else
static int LsSortCompare(const VoidPtr Ptr1, const VoidPtr Ptr2);
#endif /* ultrix && mips (no const support) */

static void LsInitialize(LsLineSegStruct **Lines);
static void LsIntersect(LsLineSegStruct *Lines);
static int LsIntersectOne(LsLineSegStruct *L1, LsLineSegStruct *L2,
			  RealType *t1, RealType *t2);

/*******************************************************************************
* Invokes the right functions in the right order...			       *
*   The Lines segments are updated so the Inters slot holds the list of        *
* intersections with the other segments, NULL if none.			       *
*   Returned list might be in different order than that is given.	       *
*******************************************************************************/
void LineSweep(LsLineSegStruct **Lines)
{
    if (Lines == NULL || *Lines == NULL)
	return;

    LsInitialize(Lines);
    LsIntersect(*Lines);
}

/*******************************************************************************
* Compare two LsLineSegStructs with according to _MinVals[1].		       *
*******************************************************************************/
#if defined(ultrix) && defined(mips)
static int LsSortCompare(VoidPtr Ptr1, VoidPtr Ptr2)
#else
static int LsSortCompare(const VoidPtr Ptr1, const VoidPtr Ptr2)
#endif /* ultrix && mips (no const support) */
{
    RealType
	Diff = ((*(LsLineSegStruct **) Ptr1)) -> _MinVals[1] -
	       ((*(LsLineSegStruct **) Ptr2)) -> _MinVals[1];

    return SIGN(Diff);
}

/*******************************************************************************
* Initialize the data structures for the plane sweep.			       *
*******************************************************************************/
static void LsInitialize(LsLineSegStruct **Lines)
{
    int i;
    LsLineSegStruct *Line, **LineArray, **l;

    for (i = 0, Line = *Lines; Line != NULL; Line = Line -> Pnext, i++) {
	RealType Size;

	Line -> _MinVals[0] = MIN(Line -> Pts[0][0], Line -> Pts[1][0]);
	Line -> _MinVals[1] = MIN(Line -> Pts[0][1], Line -> Pts[1][1]);
	Line -> _MaxVals[0] = MAX(Line -> Pts[0][0], Line -> Pts[1][0]);
	Line -> _MaxVals[1] = MAX(Line -> Pts[0][1], Line -> Pts[1][1]);

	/* Computes the vector from first point to second. */
	Line -> _Vec[0] = Line -> Pts[1][0] - Line -> Pts[0][0];
	Line -> _Vec[1] = Line -> Pts[1][1] - Line -> Pts[0][1];

	/* Computes the line equation for the segment as Ax + By + C = 0,    */
	/* sqrt(A^2 + B^2) = 1.						     */
	Line -> _ABC[0] = Line -> _Vec[1];
	Line -> _ABC[1] = -Line -> _Vec[0];
	Size = sqrt(SQR(Line -> _ABC[0]) + SQR(Line -> _ABC[1]));
	if (Size > 0.0) {
	    Line -> _ABC[0] /= Size;
	    Line -> _ABC[1] /= Size;
	    Line -> _ABC[2] = -(Line -> _ABC[0] * Line -> Pts[0][0] +
				Line -> _ABC[1] * Line -> Pts[0][1]);
	}
	else {
	    /* Zero length segment. Sets the line equation to never yield an */
	    /* intersection.						     */
	    Line -> _ABC[0] = Line -> _ABC[1] = 0.0;
	    Line -> _ABC[2] = 1.0;
	}

	Line -> Inters = NULL;
    }

    LineArray = (LsLineSegStruct **) IritMalloc(sizeof(LsLineSegStruct *) * i);
    for (Line = *Lines, l = LineArray; Line != NULL; Line = Line -> Pnext)
	*l++ = Line;
    qsort(LineArray, i, sizeof(LsLineSegStruct *), LsSortCompare);
    *Lines = *LineArray;
    for (l = LineArray; --i; l++)
	l[0] -> Pnext = l[1];
    l[0] -> Pnext = NULL;
    IritFree((VoidPtr) LineArray);
}

/*******************************************************************************
*   The actual intersections. Not a real plane sweep, but close...	       *
* March along the given list and intersect every line with all lines in the    *
* domain that can affect it.						       *
*******************************************************************************/
static void LsIntersect(LsLineSegStruct *Lines)
{
    static int
	IdNumber = 0;
    LsLineSegStruct *Line, *Line2;

    for (Line = Lines; Line -> Pnext != NULL; Line = Line -> Pnext) {
	RealType t, t2,
	    MaxY = Line -> _MaxVals[1];

	for (Line2 = Line -> Pnext; Line2 != NULL; Line2 = Line2 -> Pnext) {
	    if (Line2 -> _MinVals[1] > MaxY)
		break; /* Cannot intersect any more */

	    if (Line -> Id != Line2 -> Id &&
		LS_XBBOX_OVERLAP(Line, Line2) &&
		LsIntersectOne(Line, Line2, &t, &t2)) {
		LsIntersectStruct
		    *Inter = (LsIntersectStruct *)
			IritMalloc(sizeof(LsIntersectStruct)),
		    *Inter2 = (LsIntersectStruct *)
			IritMalloc(sizeof(LsIntersectStruct));

		Inter -> t = t;
		Inter -> OtherT = t2;
		Inter -> OtherSeg = Line2;
		Inter -> Id = IdNumber;
		Inter -> Pnext = Line -> Inters;
		Line -> Inters = Inter;

		Inter2 -> t = t2;
		Inter2 -> OtherT = t;
		Inter2 -> OtherSeg = Line;
		Inter2 -> Id = IdNumber++;
		Inter2 -> Pnext = Line2 -> Inters;
		Line2 -> Inters = Inter2;
	    }
	}
    }
}

/*******************************************************************************
*   Computes a single intersection between two line segments.		       *
*   Returns TRUE if found an intersection and sets t1/t2 to the parameter      *
* values of the intersection (t == 0 for first point, t == 1 for second).      *
*******************************************************************************/
static int LsIntersectOne(LsLineSegStruct *L1, LsLineSegStruct *L2,
			  RealType *t1, RealType *t2)
{
    RealType Dist11, Dist12, Dist21, Dist22, XDiff, YDiff, Det;

    Dist11 = L1 -> _ABC[0] * L2 -> Pts[0][0] +
	     L1 -> _ABC[1] * L2 -> Pts[0][1] +
	     L1 -> _ABC[2];
    Dist12 = L1 -> _ABC[0] * L2 -> Pts[1][0] +
	     L1 -> _ABC[1] * L2 -> Pts[1][1] +
	     L1 -> _ABC[2];
    if (Dist11 * Dist12 > 0.0) /* L2's two end points are on same size of L1. */
	return FALSE;

    Dist21 = L2 -> _ABC[0] * L1 -> Pts[0][0] +
	     L2 -> _ABC[1] * L1 -> Pts[0][1] +
	     L2 -> _ABC[2];
    Dist22 = L2 -> _ABC[0] * L1 -> Pts[1][0] +
	     L2 -> _ABC[1] * L1 -> Pts[1][1] +
	     L2 -> _ABC[2];
    if (Dist21 * Dist22 > 0.0) /* L1's two end points are on same size of L2. */
	return FALSE;

    /* Solve the following linear system for t1 and t2.			      */
    /*									      */
    /* L1X1 + t1 L1Vx = L2X1 + t2 L2Vx					      */
    /* L1Y1 + t1 L1Vy = L2Y1 + t2 L2Vy					      */
    /*									      */
    /* or,								      */
    /*									      */
    /* t1 L1Vx - t2 L2Vx = L2X1 - L1X1					      */
    /* t1 L1Vy - t2 L2Vy = L2Y1 - L1Y1					      */
    /*									      */
    Det = L1 -> _Vec[1] * L2 -> _Vec[0] - L1 -> _Vec[0] * L2 -> _Vec[1];
    if (Det == 0.0)
	return FALSE;

    XDiff = L2 -> Pts[0][0] - L1 -> Pts[0][0];
    YDiff = L2 -> Pts[0][1] - L1 -> Pts[0][1];

    *t1 = (YDiff * L2 -> _Vec[0] - XDiff * L2 -> _Vec[1]) / Det;
    *t2 = (L1 -> _Vec[0] * YDiff - L1 -> _Vec[1] * XDiff) / Det;

    return *t1 > 0.0 && *t1 <= 1.0 && *t2 > 0.0 && *t2 <= 1.0;
}