/******************************************************************************
* Distance.c - Distance computations.					      *
*******************************************************************************
* Written by Gershon Elber, May 93.					      *
******************************************************************************/

#include <ctype.h>
#include <stdio.h>
#include <string.h>
#include "symb_loc.h"

#define SELF_INTER_REL_EPS 100

static CagdPtStruct
    *GlblSrfDistPtsList = NULL;

static void SymbSrfDistFindPointsAux(CagdSrfStruct *Srf,
				     CagdRType Epsilon,
				     CagdBType SelfInter);
static void SrfDistAddZeroPoint(CagdRType u, CagdRType v, CagdRType Epsilon);

/*****************************************************************************
* DESCRIPTION:                                                               M
* Given a curve and a point, finds the nearest point (if MinDist) or the     M
* farest location (if MinDist FALSE) from the curve to the given point.      M
*   Returned is the parameter value of the curve.			     M
*   Computes the zero set of (Crv(t) - Pt) . Crv'(t).			     M
*                                                                            *
* PARAMETERS:                                                                M
*   Crv:        The curve to find its nearest (farest) point to Pt.          M
*   Pt:         The point to find the nearest (farest) point on Crv to it.   M
*   MinDist:    If TRUE nearest points is needed, if FALSE farest.           M
*   Epsilon:    Accuracy of computation.                                     M
*                                                                            *
* RETURN VALUE:                                                              M
*   CagdRType:   Parameter value in the parameter space of Crv of the        M
*                nearest (farest) point to point Pt.                         M
*                                                                            *
* KEYWORDS:                                                                  M
*   SymbDistCrvPoint, curve point distance                                   M
*****************************************************************************/
CagdRType SymbDistCrvPoint(CagdCrvStruct *Crv,
			   CagdPType Pt,
			   CagdBType MinDist,
			   CagdRType Epsilon)
{
    CagdRType TMin, TMax, t,
	ExtremeDist = MinDist ? INFINITY : -INFINITY;
    CagdPtStruct *TPt,
	*Pts = SymbLclDistCrvPoint(Crv, Pt, Epsilon);

    /* Add the two end point of the domain. */
    CagdCrvDomain(Crv, &TMin, &TMax);
    TPt = CagdPtNew();
    TPt -> Pt[0] = TMin;
    TPt -> Pnext = Pts;
    Pts = TPt;
    TPt = CagdPtNew();
    TPt -> Pt[0] = TMax;
    TPt -> Pnext = Pts;
    Pts = TPt;

    for (TPt = Pts, t = TMin; TPt != NULL; TPt = TPt -> Pnext) {
	int i;
	CagdPType EPt;
	CagdRType
	    Dist = 0.0,
	    *R = CagdCrvEval(Crv, TPt -> Pt[0]);

	CagdCoerceToE3(EPt, &R, - 1, Crv -> PType);

	for (i = 0; i < 3; i++)
	    Dist += SQR(EPt[i] - Pt[i]);

	if (MinDist) {
	    if (Dist < ExtremeDist) {
		t = TPt -> Pt[0];
		ExtremeDist = Dist;
	    }
	}
	else {
	    if (Dist > ExtremeDist) {
		t = TPt -> Pt[0];
		ExtremeDist = Dist;
	    }
	}
    }
    CagdPtFreeList(Pts);

    return t;
}

/*****************************************************************************
* DESCRIPTION:                                                               M
* Given a curve and a point, find the local extremum distance points on the  M
* curve to the given point.                                                  M
*   Returned is a list of parameter value with local extremum.		     M
*   Computes the zero set of (Crv(t) - Pt) . Crv'(t).			     M
*                                                                            *
* PARAMETERS:                                                                M
*   Crv:        The curve to find its extreme distance locations to Pt.      M
*   Pt:         The point to find the extreme distance locations from Crv.   M
*   Epsilon:    Accuracy of computation.                                     M
*                                                                            *
* RETURN VALUE:                                                              M
*   CagdPtStruct *:   A list of parameter values of extreme distance         M
*		      locations.                                             M
*                                                                            *
* KEYWORDS:                                                                  M
*   SymbLclDistCrvPoint, curve point distance                                M
*****************************************************************************/
CagdPtStruct *SymbLclDistCrvPoint(CagdCrvStruct *Crv,
				  CagdPType Pt,
				  CagdRType Epsilon)
{
    int i;
    CagdCrvStruct *ZCrv,
	*DCrv = CagdCrvDerive(Crv);
    CagdPType MinusPt;
    CagdPtStruct *ZeroSet;

    for (i = 0; i < 3; i++)
	MinusPt[i] = -Pt[i];

    Crv = CagdCrvCopy(Crv);
    CagdCrvTransform(Crv, MinusPt, 1.0);

    ZCrv = SymbCrvDotProd(Crv, DCrv);
    CagdCrvFree(Crv);
    CagdCrvFree(DCrv);

    ZeroSet = SymbCrvZeroSet(ZCrv, 1,  Epsilon);
    CagdCrvFree(ZCrv);

    return ZeroSet;
}

/*****************************************************************************
* DESCRIPTION:                                                               M
* Given a curve and a line, finds the nearest point (if MinDist) or the      M
* farest location (if MinDist FALSE) from the curve to the given line.	     M
*   Returned is the parameter value of the curve.			     M
*   Let Crv be (x(t), y(t)). By substituting x(t) and y(t) into the line     M
* equation, we derive the distance function.				     M
*   Its zero set, combined with the zero set of its derivative provide the   M
* needed extreme distances.						     M
*                                                                            *
* PARAMETERS:                                                                M
*   Crv:        The curve to find its nearest (farest) point to Line.        M
*   Line:       The line to find the nearest (farest) point on Crv to it.    M
*   MinDist:    If TRUE nearest points is needed, if FALSE farest.           M
*   Epsilon:    Accuracy of computation.                                     M
*                                                                            *
* RETURN VALUE:                                                              M
*   CagdRType:   Parameter value in the parameter space of Crv of the        M
*                nearest (farest) point to line Line.                        M
*                                                                            *
* KEYWORDS:                                                                  M
*   SymbDistCrvLine, curve line distance                                     M
*****************************************************************************/
CagdRType SymbDistCrvLine(CagdCrvStruct *Crv,
			  CagdLType Line,
			  CagdBType MinDist,
			  CagdRType Epsilon)
{
    CagdRType TMin, TMax, t,
	ExtremeDist = MinDist ? INFINITY : -INFINITY;
    CagdPtStruct *TPt,
	*Pts = SymbLclDistCrvLine(Crv, Line, Epsilon);

    /* Add the two end point of the domain. */
    CagdCrvDomain(Crv, &TMin, &TMax);
    TPt = CagdPtNew();
    TPt -> Pt[0] = TMin;
    TPt -> Pnext = Pts;
    Pts = TPt;
    TPt = CagdPtNew();
    TPt -> Pt[0] = TMax;
    TPt -> Pnext = Pts;
    Pts = TPt;

    for (TPt = Pts, t = TMin; TPt != NULL; TPt = TPt -> Pnext) {
	CagdPType EPt;
	CagdRType
	    Dist = 0.0,
	    *R = CagdCrvEval(Crv, TPt -> Pt[0]);

	CagdCoerceToE2(EPt, &R, - 1, Crv -> PType);

	Dist = EPt[0] * Line[0] + EPt[1] * Line[1] + Line[2];
	Dist = ABS(Dist);

	if (MinDist) {
	    if (Dist < ExtremeDist) {
		t = TPt -> Pt[0];
		ExtremeDist = Dist;
	    }
	}
	else {
	    if (Dist > ExtremeDist) {
		t = TPt -> Pt[0];
		ExtremeDist = Dist;
	    }
	}
    }
    CagdPtFreeList(Pts);

    return t;
}

/*****************************************************************************
* DESCRIPTION:                                                               M
* Given a curve and a line, finds the local extreme distance points on the   M
* curve to the given line.						     M
*   Returned is a list of parameter value with local extreme distances.      M
*   Let Crv be (x(t), y(t)). By substituting x(t) and y(t) into the line     M
* equation, we derive the distance function.				     M
*   Its zero set, combined with the zero set of its derivative provide the   M
* needed extreme distances.						     M
*                                                                            *
* PARAMETERS:                                                                M
*   Crv:        The curve to find its nearest (farest) point to Line.        M
*   Line:       The line to find the nearest (farest) point on Crv to it.    M
*   Epsilon:    Accuracy of computation.                                     M
*                                                                            *
* RETURN VALUE:                                                              M
*   CagdPtStruct *:   A list of parameter values of extreme distance         M
*		      locations.                                             M
*                                                                            *
* KEYWORDS:                                                                  M
*   SymbLclDistCrvLine, curve line distance                                  M
*****************************************************************************/
CagdPtStruct *SymbLclDistCrvLine(CagdCrvStruct *Crv,
				 CagdLType Line,
				 CagdRType Epsilon)
{
    CagdPType Pt;
    CagdCrvStruct *TCrv1, *CrvX, *CrvY, *CrvZ, *CrvW, *DistCrv, *DerivDistCrv;
    CagdPtStruct *ZeroSet1, *ZeroSet2, *TPt;

    SymbCrvSplitScalar(Crv, &CrvW, &CrvX, &CrvY, &CrvZ);
    if (CrvZ)
	CagdCrvFree(CrvZ);

    Pt[0] = Pt[1] = Pt[2] = 0.0;
    CagdCrvTransform(CrvX, Pt, Line[0]);
    CagdCrvTransform(CrvY, Pt, Line[1]);
    TCrv1 = SymbCrvAdd(CrvX, CrvY);
    CagdCrvFree(CrvX);
    CagdCrvFree(CrvY);
    if (CrvW) {
	CagdCrvTransform(CrvW, Pt, Line[2]);
	DistCrv = SymbCrvAdd(TCrv1, CrvW);
	CagdCrvFree(CrvW);
	CagdCrvFree(TCrv1);
    }
    else {
	Pt[0] = Line[2];
	CagdCrvTransform(TCrv1, Pt, 1.0);
	DistCrv = TCrv1;
    }

    ZeroSet1 = SymbCrvZeroSet(DistCrv, 1,  Epsilon);

    DerivDistCrv = CagdCrvDerive(DistCrv);
    CagdCrvFree(DistCrv);

    ZeroSet2 = SymbCrvZeroSet(DerivDistCrv, 1,  Epsilon);
    CagdCrvFree(DerivDistCrv);

    if (ZeroSet1 == NULL)
	return ZeroSet2;
    else if (ZeroSet2 == NULL)
	return ZeroSet1;
    else {
	for (TPt = ZeroSet1; TPt -> Pnext != NULL; TPt = TPt -> Pnext);

	TPt -> Pnext = ZeroSet2;

	return ZeroSet1;
    }
}

/*****************************************************************************
* DESCRIPTION:                                                               M
* Given two curves, creates a bivariate scalar surface representing the      M
* distance function square, between them.				     M
*                                                                            *
* PARAMETERS:                                                                M
*   Crv1, Crv2:  The two curves, Crv1(u) and Crv2(v), to form their distance M
*                function square between them as a bivariate function.       M
*                                                                            *
* RETURN VALUE:                                                              M
*   CagdSrfStruct *:   The distance function square d2(u, v) of the distance M
*                      from Crv1(u) to Crv2(v).                              M
*                                                                            *
* KEYWORDS:                                                                  M
*   SymbSrfDistCrvCrv, curve curve distance                                  M
*****************************************************************************/
CagdSrfStruct *SymbSrfDistCrvCrv(CagdCrvStruct *Crv1, CagdCrvStruct *Crv2)
{
    CagdSrfStruct *TSrf, *DiffSrf, *DotProdSrf,
	*Srf1 = CagdPromoteCrvToSrf(Crv1, CAGD_CONST_U_DIR),
	*Srf2 = CagdPromoteCrvToSrf(Crv2, CAGD_CONST_V_DIR);

    if (CAGD_IS_BSPLINE_SRF(Srf1) || CAGD_IS_BSPLINE_SRF(Srf2)) {
	CagdRType UMin1, UMax1, VMin1, VMax1, UMin2, UMax2, VMin2, VMax2;

	if (CAGD_IS_BEZIER_SRF(Srf1)) {
	    TSrf = CnvrtBezier2BsplineSrf(Srf1);
	    CagdSrfFree(Srf1);
	    Srf1 = TSrf;
	}
	if (CAGD_IS_BEZIER_SRF(Srf2)) {
	    TSrf = CnvrtBezier2BsplineSrf(Srf2);
	    CagdSrfFree(Srf2);
	    Srf2 = TSrf;
	}

	CagdSrfDomain(Srf1, &UMin1, &UMax1, &VMin1, &VMax1);
	CagdSrfDomain(Srf2, &UMin2, &UMax2, &VMin2, &VMax2);
	BspKnotAffineTrans(Srf1 -> VKnotVector,
			   Srf1 -> VLength + Srf1 -> VOrder,
			   VMin2 - VMin1, (VMax2 - VMin2) / (VMax1 - VMin1));
	BspKnotAffineTrans(Srf2 -> UKnotVector,
			   Srf2 -> ULength + Srf1 -> VOrder,
			   UMin1 - UMin2, (UMax1 - UMin1) / (UMax2 - UMin2));
    }

    DiffSrf = SymbSrfSub(Srf1, Srf2);
    DotProdSrf = SymbSrfDotProd(DiffSrf, DiffSrf);
	
    CagdSrfFree(Srf1);
    CagdSrfFree(Srf2);
    CagdSrfFree(DiffSrf);

    return DotProdSrf;
}

/*****************************************************************************
* DESCRIPTION:                                                               M
* Given a scalar surface representing the distance function square between   M
* two curves, finds the zero set of the distance surface, if any, and        M
* returns it.								     M
*   The given surface is a non negative surface and zero set is its minima.  M
*   The returned points will contail the two parameter values of the two     M
* curves that intersect in the detected zero set points.		     M
*                                                                            *
* PARAMETERS:                                                                M
*   Srf:         A bivariate function that represent the distance square     M
*                function between two curves.				     M
*   Epsilon:     Accuracy control.                                           M
*   SelfInter:   Should be consider self intersection? That is, is Srf       M
*                between a curve to itself!?				     M
*                                                                            *
* RETURN VALUE:                                                              M
*   CagdPtStruct *:  A list of parameter values of both curves, at all       M
*                    detected intersection locations.                        M
*                                                                            *
* KEYWORDS:                                                                  M
*   SymbSrfDistFindPoints, curve curve distance, curve curve intersection    M
*****************************************************************************/
CagdPtStruct *SymbSrfDistFindPoints(CagdSrfStruct *Srf,
				    CagdRType Epsilon,
				    CagdBType SelfInter)
{
    GlblSrfDistPtsList = NULL;

    if (Srf -> GType == CAGD_SBEZIER_TYPE) {
	Srf = CnvrtBezier2BsplineSrf(Srf); /* To keep track on the domains. */
	SymbSrfDistFindPointsAux(Srf, Epsilon, SelfInter);
	CagdSrfFree(Srf);
    }
    else
	SymbSrfDistFindPointsAux(Srf, Epsilon, SelfInter);

    return GlblSrfDistPtsList;
}

/*****************************************************************************
* DESCRIPTION:                                                               *
* Auxiliary function of SymbSrfDistFindPoints - does the hard work.	     *
*                                                                            *
* PARAMETERS:                                                                *
*   Srf:         A bivariate function that represent the distance square     *
*                function between two curves.				     *
*   Epsilon:     Accuracy control.                                           *
*   SelfInter:   Should be consider self intersection? That is, is Srf       *
*                between a curve to itself!?				     *
*                                                                            *
* RETURN VALUE:                                                              *
*   None	                                                             *
*****************************************************************************/
static void SymbSrfDistFindPointsAux(CagdSrfStruct *Srf,
				     CagdRType Epsilon,
				     CagdBType SelfInter)
{
    int i, j,
	ULength = Srf -> ULength,
	VLength = Srf -> VLength;
    CagdRType
	*XPts = Srf -> Points[1];

    for (i = 0; i < ULength; i++) {
	for (j = 0; j < VLength; j++) {
	    if (*XPts++ <= 0.0) {
		CagdRType UMin, UMax, VMin, VMax;

		CagdSrfDomain(Srf, &UMin, &UMax, &VMin, &VMax);

		if (SelfInter) {
		    CagdRType
			SelfInterEps = Epsilon * SELF_INTER_REL_EPS;

		    if (UMax - UMin < SelfInterEps &&
			VMax - VMin < SelfInterEps &&
			UMax - VMin < SelfInterEps &&
			VMax - UMin < SelfInterEps)
		        return;
		}

		/* The surface may have a zero here. Test the domain size   */
		/* to make sure it makes sense to subdivide.		    */
		if (UMax - UMin < Epsilon && VMax - VMin < Epsilon) {
		    SrfDistAddZeroPoint((UMin + UMax) / 2.0,
					(VMin + VMax) / 2.0, Epsilon);
		}
		else {
		    /* Subdivide the surface and invoke recursively. */
		    CagdSrfStruct *Srf1, *Srf2;
		    CagdRType t;
		    CagdSrfDirType Dir;

		    if (UMax - UMin > VMax - VMin) {
			t = (UMin + UMax) / 2.0;
			Dir = CAGD_CONST_U_DIR;
		    }
		    else {
			t = (VMin + VMax) / 2.0;
			Dir = CAGD_CONST_V_DIR;
		    }
		    Srf1 = CagdSrfSubdivAtParam(Srf, t, Dir);
		    Srf2 = Srf1 -> Pnext;
		    SymbSrfDistFindPointsAux(Srf1, Epsilon, SelfInter);
		    SymbSrfDistFindPointsAux(Srf2, Epsilon, SelfInter);
		    CagdSrfFree(Srf1);
		    CagdSrfFree(Srf2);
		}
		return;
	    }
	}
    }
}

/*****************************************************************************
* DESCRIPTION:                                                               *
* Auxiliary function of SymbSrfDistFindPoints - part of the hard work.	     *
*                                                                            *
* PARAMETERS:                                                                *
*   u, v:   A zero set location found to be added to global parameter list.  *
*   Epsilon:    To match against existing zero set point, for simiarity,     *
*                                                                            *
* RETURN VALUE:                                                              *
*   void                                                                     *
*****************************************************************************/
static void SrfDistAddZeroPoint(CagdRType u, CagdRType v, CagdRType Epsilon)
{
    CagdPtStruct *Pt;

    for (Pt = GlblSrfDistPtsList; Pt != NULL; Pt = Pt -> Pnext) {
	if (ABS(Pt -> Pt[0] - u) < Epsilon * 10 &&
	    ABS(Pt -> Pt[1] - v) < Epsilon * 10)
	    return;			         /* Point is already there. */
    }

    Pt = CagdPtNew();

    Pt -> Pt[0] = u;
    Pt -> Pt[1] = v;
    Pt -> Pnext = GlblSrfDistPtsList;
    GlblSrfDistPtsList = Pt;
}
