/******************************************************************************
* CagdCrvt.c - curvature computation of curves and surfaces.		      *
*******************************************************************************
* Written by Gershon Elber, March 93.					      *
******************************************************************************/

#include "cagd_loc.h"

#define MAX_POS_REF_ITERATION 20

/******************************************************************************
* Computes a scalar curve representing the curvature of a planar curve.       *
* The given curve is assumed to be planar and only its x and y coordinates    *
* are considered.							      *
*   Then the curvature k is equal to					      *
*      .  ..    .  ..							      *
*      X  Y  -  Y  X							      *
* k =  -------------							      *
*         .2   .2  3/2 							      *
*      (  X  + Y  )							      *
*   Since we cannot represent k because of the square root, we compute and    *
* represent k^2.							      *
******************************************************************************/
CagdCrvStruct *CagdCrvCurvatureSqr(CagdCrvStruct *Crv)
{
    CagdBType
        IsRational = CAGD_IS_RATIONAL_CRV(Crv);
    CagdCrvStruct *Crv1W, *Crv1X, *Crv1Y, *Crv1Z,
		  *Crv2W, *Crv2X, *Crv2Y, *Crv2Z,
		  *CTmp1, *CTmp2, *CTmp3, *Numer, *Denom, *CurvatureSqr,
	*Crv1Deriv = CagdCrvDerive(Crv),
	*Crv2Deriv = CagdCrvDerive(Crv1Deriv);

    CagdCrvSplitScalar(Crv1Deriv, &Crv1W, &Crv1X, &Crv1Y, &Crv1Z);
    CagdCrvSplitScalar(Crv2Deriv, &Crv2W, &Crv2X, &Crv2Y, &Crv2Z);
    CagdCrvFree(Crv1Deriv);
    CagdCrvFree(Crv2Deriv);

    CTmp1 = CagdCrvMult(Crv1X, Crv2Y);
    CTmp2 = CagdCrvMult(Crv2X, Crv1Y);
    CTmp3 = CagdCrvSub(CTmp1, CTmp2);
    CagdCrvFree(CTmp1);
    CagdCrvFree(CTmp2);
    Numer = CagdCrvMult(CTmp3, CTmp3);
    CagdCrvFree(CTmp3);

    CTmp1 = CagdCrvMult(Crv1X, Crv1X);
    CTmp2 = CagdCrvMult(Crv1Y, Crv1Y);
    CTmp3 = CagdCrvAdd(CTmp1, CTmp2);
    CagdCrvFree(CTmp1);
    CagdCrvFree(CTmp2);
    CTmp1 = CagdCrvMult(CTmp3, CTmp3);
    Denom = CagdCrvMult(CTmp1, CTmp3);
    CagdCrvFree(CTmp1);
    CagdCrvFree(CTmp3);
    if (IsRational) {
	CTmp1 = CagdCrvMult(Crv1W, Crv1W);
	CTmp2 = CagdCrvMult(CTmp1, CTmp1);
	CTmp3 = CagdCrvMult(CTmp2, Numer);
	CagdCrvFree(CTmp1);
	CagdCrvFree(CTmp2);
	CagdCrvFree(Numer);
	Numer = CTmp3;

	CTmp1 = CagdCrvMult(Crv2W, Crv2W);
	CTmp2 = CagdCrvMult(CTmp1, Denom);
	CagdCrvFree(CTmp1);
	CagdCrvFree(Denom);
	Denom = CTmp2;
    }
    if (CAGD_IS_BSPLINE_CRV(Denom)) {
	CTmp1 = CagdMakePosCrvCtlPolyPos(Denom);
	CagdCrvFree(Denom);
	Denom = CTmp1;
    }

    CagdMakeCrvsCompatible(&Denom, &Numer, TRUE, TRUE);
    CurvatureSqr = CagdCrvMergeScalar(Denom, Numer, NULL, NULL);
    CagdCrvFree(Denom);
    CagdCrvFree(Numer);

    CagdCrvFree(Crv1X);
    CagdCrvFree(Crv1Y);
    CagdCrvFree(Crv2X);
    CagdCrvFree(Crv2Y);
    if (Crv1Z)
	CagdCrvFree(Crv1Z);
    if (Crv2Z)
	CagdCrvFree(Crv2Z);
    if (Crv1W)
	CagdCrvFree(Crv1W);
    if (Crv2W)
	CagdCrvFree(Crv2W);

    return CurvatureSqr;
}

/******************************************************************************
* Computes a vectior field curve representing the curvature of a curve, in    *
* the normal direction, that is kN.					      *
*                .   ..      .       .   ..     .			      *
*                C x C       C     ( C x C  ) x C     			      *
* kN = kB x N =  -----  x  ----- = --------------			      *
*                  .  3      .           .  4				      *
*                | C |     | C |       | C |				      *
******************************************************************************/
CagdCrvStruct *CagdCrvCurvatureNormal(CagdCrvStruct *Crv)
{
    CagdBType
        IsRational = CAGD_IS_RATIONAL_CRV(Crv);
    CagdCrvStruct *CrvW, *CrvX, *CrvY, *CrvZ,
	*CTmp, *Numer, *Denom, *CurvatureNormal,
	*Crv1Deriv = CagdCrvDerive(Crv),
	*Crv2Deriv = CagdCrvDerive(Crv1Deriv);

    CTmp = CagdCrvCrossProd(Crv1Deriv, Crv2Deriv);
    CagdCrvFree(Crv2Deriv);
    Numer = CagdCrvCrossProd(CTmp, Crv1Deriv);
    CagdCrvFree(CTmp);
    CagdCrvSplitScalar(Numer, &CrvW, &CrvX, &CrvY, &CrvZ);
    CagdCrvFree(Numer);

    CTmp = CagdCrvDotProd(Crv1Deriv, Crv1Deriv);
    CagdCrvFree(Crv1Deriv);
    Denom = CagdCrvMult(CTmp, CTmp);

    if (IsRational) {
	CagdCrvStruct *DenomCrvW, *DenomCrvX, *DenomCrvY, *DenomCrvZ;

	CagdCrvSplitScalar(Denom,
			   &DenomCrvW, &DenomCrvX, &DenomCrvY, &DenomCrvZ);
	CagdCrvFree(Denom);

	CTmp = CagdCrvMult(CrvW, DenomCrvX);
	CagdCrvFree(CrvW);
	CrvW = CTmp;

	CTmp = CagdCrvMult(CrvX, DenomCrvW);
	CagdCrvFree(CrvX);
	CrvX = CTmp;

	CTmp = CagdCrvMult(CrvY, DenomCrvW);
	CagdCrvFree(CrvY);
	CrvY = CTmp;

	CTmp = CagdCrvMult(CrvZ, DenomCrvW);
	CagdCrvFree(CrvZ);
	CrvZ = CTmp;

	CagdCrvFree(DenomCrvW);
	CagdCrvFree(DenomCrvX);

	CurvatureNormal = CagdCrvMult(CTmp, Numer);
	CagdCrvFree(CTmp);
    }
    else {
	CagdMakeCrvsCompatible(&Denom, &CrvX, TRUE, TRUE);
	CagdMakeCrvsCompatible(&Denom, &CrvY, TRUE, TRUE);
	CagdMakeCrvsCompatible(&Denom, &CrvZ, TRUE, TRUE);
	CrvW = Denom;
    }
    CurvatureNormal = CagdCrvMergeScalar(CrvW, CrvX, CrvY, CrvZ);
    CagdCrvFree(CrvX);
    CagdCrvFree(CrvY);
    CagdCrvFree(CrvZ);
    CagdCrvFree(CrvW);

    return CurvatureNormal;
}

/******************************************************************************
* Computes a scalar curve representing the curvature sign of a planar curve.  *
* The given curve is assumed to be planar and only its x and y coordinates    *
* are considered.							      *
*   Then the curvature sign is equal to					      *
*      .  ..    .  ..							      *
* s =  X  Y  -  Y  X							      *
******************************************************************************/
CagdCrvStruct *CagdCrvCurvatureSign(CagdCrvStruct *Crv)
{
    CagdBType
        IsRational = CAGD_IS_RATIONAL_CRV(Crv);
    CagdCrvStruct *Crv1W, *Crv1X, *Crv1Y, *Crv1Z,
		  *Crv2W, *Crv2X, *Crv2Y, *Crv2Z,
		  *CTmp1, *CTmp2, *Numer, *Denom, *CurvatureSign,
	*Crv1Deriv = CagdCrvDerive(Crv),
	*Crv2Deriv = CagdCrvDerive(Crv1Deriv);

    CagdCrvSplitScalar(Crv1Deriv, &Crv1W, &Crv1X, &Crv1Y, &Crv1Z);
    CagdCrvSplitScalar(Crv2Deriv, &Crv2W, &Crv2X, &Crv2Y, &Crv2Z);
    CagdCrvFree(Crv1Deriv);
    CagdCrvFree(Crv2Deriv);

    CTmp1 = CagdCrvMult(Crv1X, Crv2Y);
    CTmp2 = CagdCrvMult(Crv2X, Crv1Y);
    Numer = CagdCrvSub(CTmp1, CTmp2);
    CagdCrvFree(CTmp1);
    CagdCrvFree(CTmp2);

    if (IsRational) {
	Denom = CagdCrvMult(Crv1W, Crv2W);

	CagdMakeCrvsCompatible(&Denom, &Numer, TRUE, TRUE);
	CurvatureSign = CagdCrvMergeScalar(Denom, Numer, NULL, NULL);
	CagdCrvFree(Denom);
	CagdCrvFree(Numer);
    }
    else {
	CurvatureSign = Numer;
    }

    CagdCrvFree(Crv1X);
    CagdCrvFree(Crv1Y);
    CagdCrvFree(Crv2X);
    CagdCrvFree(Crv2Y);
    if (Crv1Z)
	CagdCrvFree(Crv1Z);
    if (Crv2Z)
	CagdCrvFree(Crv2Z);
    if (Crv1W)
	CagdCrvFree(Crv1W);
    if (Crv2W)
	CagdCrvFree(Crv2W);

    return CurvatureSign;
}

/******************************************************************************
* Given a curve that is positive, refine it until all its control points has  *
* positive coefficients. Always returns a Bspline curve.                      *
******************************************************************************/
CagdCrvStruct *CagdMakePosCrvCtlPolyPos(CagdCrvStruct *OrigCrv)
{
    int l;
    CagdCrvStruct
	*RefCrv = NULL;

    switch (OrigCrv -> GType) {
	case CAGD_CBEZIER_TYPE:
	    RefCrv = CnvrtBezier2BsplineCrv(OrigCrv);
	    break;
	case CAGD_CBSPLINE_TYPE:
	    RefCrv = CagdCrvCopy(OrigCrv);
	    break;
	case CAGD_CPOWER_TYPE:
	default:
	    FATAL_ERROR(CAGD_ERR_UNDEF_CRV);
	    break;
    }

    for (l = 0; l < MAX_POS_REF_ITERATION; l++) {
	int i, j,
	    Len = RefCrv -> Length,
	    Order = RefCrv -> Order;
	CagdRType
	    *KV = RefCrv -> KnotVector,
	    *Nodes = BspKnotNodes(KV, Len + Order, Order),
	    *Pts = RefCrv -> Points[1];

	for (i = j = 0; i < Len; i++) {
	    if (Pts[i] < 0)	    /* To refine at negative control points. */
	        Nodes[j++] = Nodes[i];
	}
	if (j == 0) {
	    IritFree(Nodes);
	    break;
	}
	else {
	    CagdCrvStruct
	        *NewCrv = CagdCrvRefineAtParams(RefCrv, FALSE, Nodes, j);

	    CagdCrvFree(RefCrv);
	    RefCrv = NewCrv;
	    IritFree(Nodes);
	}
    }

    return RefCrv;
}

/******************************************************************************
* Given a planar curve, find all its inflection points by find the zero set   *
* of the sign of the curvature function.				      *
******************************************************************************/
CagdPtStruct *CagdCrvInflectionPts(CagdCrvStruct *Crv, CagdRType Epsilon)
{
    CagdCrvStruct
	*CrvtrSign = CagdCrvCurvatureSign(Crv);
    CagdPtStruct
	*InflectionPts = CagdCrvZeroSet(CrvtrSign, 1, Epsilon);

    CagdCrvFree(CrvtrSign);

    return InflectionPts;    
}

/******************************************************************************
* Given a planar curve, find all its extreme curvatrue points by find the     *
* extreme set of the curvature function.				      *
******************************************************************************/
CagdPtStruct *CagdCrvExtremCrvtrPts(CagdCrvStruct *Crv, CagdRType Epsilon)
{
    if (CAGD_NUM_OF_PT_COORD(Crv -> PType) == 2) {
	CagdCrvStruct
	    *CrvtrSqrCrv = CagdCrvCurvatureSqr(Crv);
	CagdPtStruct
	    *ExtremCrvtrPts = CagdCrvExtremSet(CrvtrSqrCrv, 1, Epsilon);

	CagdCrvFree(CrvtrSqrCrv);

	return ExtremCrvtrPts;    
    }
    else if (CAGD_NUM_OF_PT_COORD(Crv -> PType) == 3) {
	CagdCrvStruct
	    *CrvtrNormalCrv = CagdCrvCurvatureNormal(Crv),
	    *CrvtrMag = CagdCrvDotProd(CrvtrNormalCrv, CrvtrNormalCrv);
	CagdPtStruct
	    *ExtremCrvtrPts = CagdCrvExtremSet(CrvtrMag, 1, Epsilon);

	CagdCrvFree(CrvtrNormalCrv);
	CagdCrvFree(CrvtrMag);

	return ExtremCrvtrPts;    
    }
    else {
	FATAL_ERROR(CAGD_ERR_ONLY_2D_OR_3D);
	return NULL;
    }
}