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

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

#define SELF_INTER_REL_EPS 100

static CagdPtStruct
    *GlblSrfDistPtsList = NULL;

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

/******************************************************************************
* Given a curve and a point, find the closest point (if MinDist) or the far   *
* location (if !MinDist) from the curve to the given point.		      *
*   Returned is the parameter value of the curve.			      *
******************************************************************************/
CagdRType CagdDistCrvPoint(CagdCrvStruct *Crv, CagdPType Pt, CagdBType MinDist,
							     CagdRType Epsilon)
{
    CagdRType TMin, TMax, t,
	ExtremeDist = MinDist ? INFINITY : -INFINITY;
    CagdPtStruct *TPt,
	*Pts = CagdLclDistCrvPoint(Crv, Pt, Epsilon);

    /* Add the two end point of the domain. */
    CagdCrvDomain(Crv, &TMin, &TMax);
    TPt = IritMalloc(sizeof(CagdPtStruct));
    TPt -> Pt[0] = TMin;
    TPt -> Pnext = Pts;
    Pts = TPt;
    TPt = IritMalloc(sizeof(CagdPtStruct));
    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;
	    }
	}
    }

    return t;
}

/******************************************************************************
* Given a curve and a point, find the local extremum distance points on the   *
* curve to the given point. Return is a list of parametr value with local     *
* extremum.								      *
*   Computes the zero set of (Crv(t) - Pt) . Crv'(t).			      *
******************************************************************************/
CagdPtStruct *CagdLclDistCrvPoint(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 = CagdCrvDotProd(Crv, DCrv);
    CagdCrvFree(Crv);
    CagdCrvFree(DCrv);

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

    return ZeroSet;
}

/******************************************************************************
* Given a curve and a line, find the closest point (if MinDist) or the far    *
* location (if !MinDist) from the curve to the given line.		      *
*   Returned is the parameter value of the curve.			      *
******************************************************************************/
CagdRType CagdDistCrvLine(CagdCrvStruct *Crv, CagdLType Line,
					  CagdBType MinDist, CagdRType Epsilon)
{
    CagdRType TMin, TMax, t,
	ExtremeDist = MinDist ? INFINITY : -INFINITY;
    CagdPtStruct *TPt,
	*Pts = CagdLclDistCrvLine(Crv, Line, Epsilon);

    /* Add the two end point of the domain. */
    CagdCrvDomain(Crv, &TMin, &TMax);
    TPt = IritMalloc(sizeof(CagdPtStruct));
    TPt -> Pt[0] = TMin;
    TPt -> Pnext = Pts;
    Pts = TPt;
    TPt = IritMalloc(sizeof(CagdPtStruct));
    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;
	    }
	}
    }

    return t;
}

/******************************************************************************
* Given a curve and a line, find the local extremum distance points on the    *
* curve to the given line. Return is a list of parametr value with local      *
* extremum.								      *
*   Let Crv be x(t), y(t). By substituting x(t) and y(t) into the line	      *
* equation, we derive the distance function. Its zero set, combined with the  *
* zero set of its derivative provide the needed extrema.		      *
******************************************************************************/
CagdPtStruct *CagdLclDistCrvLine(CagdCrvStruct *Crv, CagdLType Line,
							     CagdRType Epsilon)
{
    CagdPType Pt;
    CagdCrvStruct *TCrv1, *CrvX, *CrvY, *CrvZ, *CrvW, *DistCrv, *DerivDistCrv;
    CagdPtStruct *ZeroSet1, *ZeroSet2, *TPt;

    CagdCrvSplitScalar(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 = CagdCrvAdd(CrvX, CrvY);
    CagdCrvFree(CrvX);
    CagdCrvFree(CrvY);
    if (CrvW) {
	CagdCrvTransform(CrvW, Pt, Line[2]);
	DistCrv = CagdCrvAdd(TCrv1, CrvW);
	CagdCrvFree(CrvW);
	CagdCrvFree(TCrv1);
    }
    else {
	Pt[0] = Line[2];
	CagdCrvTransform(TCrv1, Pt, 1.0);
	DistCrv = TCrv1;
    }

    ZeroSet1 = CagdCrvZeroSet(DistCrv, 1,  Epsilon);

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

    ZeroSet2 = CagdCrvZeroSet(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;
    }
}

/******************************************************************************
* Given two curves, creates a scalar surface representing the distance square *
* between them.								      *
******************************************************************************/
CagdSrfStruct *CagdSrfDistCrvCrv(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 = CagdSrfSub(Srf1, Srf2);
    DotProdSrf = CagdSrfDotProd(DiffSrf, DiffSrf);
	
    CagdSrfFree(Srf1);
    CagdSrfFree(Srf2);
    CagdSrfFree(DiffSrf);

    return DotProdSrf;
}

/******************************************************************************
* Given a scalar surface representing the distance square between two curves, *
* find the zero set of the surface, if any and return it.		      *
*   The given surface is a non negative surface and zero set is its minima.   *
*   The returned points will contail the two parameter values of the two      *
* curves that intersect in point's X & Y.				      *
******************************************************************************/
CagdPtStruct *CagdSrfDistFindPoints(CagdSrfStruct *Srf, CagdRType Epsilon,
							CagdBType SelfInter)
{
    GlblSrfDistPtsList = NULL;

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

    return GlblSrfDistPtsList;
}

/******************************************************************************
* Aux. function of CagdSrfDistFindPoints - does the hard work.		      *
******************************************************************************/
static void CagdSrfDistFindPointsAux(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;
		    CagdSrfDistFindPointsAux(Srf1, Epsilon, SelfInter);
		    CagdSrfDistFindPointsAux(Srf2, Epsilon, SelfInter);
		    CagdSrfFree(Srf1);
		    CagdSrfFree(Srf2);
		}
		return;
	    }
	}
    }
}

/******************************************************************************
* Aux. function of CagdSrfDistFindPoints - does the hard work.		      *
******************************************************************************/
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 = IritMalloc(sizeof(CagdPtStruct));

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