/******************************************************************************
* CBsp-Aux.c - Bspline curve auxilary routines.				      *
*******************************************************************************
* Written by Gershon Elber, Aug. 90.					      *
******************************************************************************/

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

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

/******************************************************************************
* Given a bspline curve - subdivide it into two at the given parametric value.*
* Returns pointer to first curve in a list of two curves (subdivided ones).   *
* The subdivision is achieved by inserting (order-1) knot at the given param. *
* value t and spliting the control polygon and knot vector at that point.     *
******************************************************************************/
CagdCrvStruct *BspCrvSubdivAtParam(CagdCrvStruct *Crv, CagdRType t)
{
    CagdBType
	IsNotRational = !CAGD_IS_RATIONAL_CRV(Crv);
    int j,
	k = Crv -> Order,
	Len = Crv -> Length,
        KVLen = k + Len,
	Index1 = BspKnotLastIndexL(Crv -> KnotVector, KVLen, t),
	Index2 = BspKnotFirstIndexG(Crv -> KnotVector, KVLen, t),
	MaxCoord = CAGD_NUM_OF_PT_COORD(Crv -> PType);
    CagdCrvStruct
	*RefinedCrv = BspCrvKnotInsertNSame(Crv, t, k - 1),
	*LCrv = BspCrvNew(Index1 + 1, k, Crv -> PType),
	*RCrv = BspCrvNew(Len - Index2 + k, k, Crv -> PType);

    /* Copy Curve into LCrv. */
    for (j = IsNotRational; j <= MaxCoord; j++)
	GEN_COPY(LCrv -> Points[j],
		 RefinedCrv -> Points[j],
                 sizeof(CagdRType) * (Index1 + 1));
    GEN_COPY(LCrv -> KnotVector,
	     RefinedCrv -> KnotVector,
	     sizeof(CagdRType) * (Index1 + k));
    /* Close the knot vector with multiplicity Order: */
    LCrv -> KnotVector[Index1 + k] = LCrv -> KnotVector[Index1 + k - 1];

    /* Copy Curve into RCrv. */
    for (j = IsNotRational; j <= MaxCoord; j++)
	GEN_COPY(RCrv -> Points[j],
		 &RefinedCrv -> Points[j][Index1],
		 sizeof(CagdRType) * (Len - Index2 + k));
    GEN_COPY(&RCrv -> KnotVector[1],
	     &RefinedCrv -> KnotVector[Index1 + 1],
	     sizeof(CagdRType) * (Len - Index2 + k + k - 1));
    /* Make sure knot vector starts with multiplicity Order: */
    RCrv -> KnotVector[0] = RCrv -> KnotVector[1];

    LCrv -> Pnext = RCrv;

    CagdCrvFree(RefinedCrv);

    return LCrv;
}

/******************************************************************************
* Return a new curve, identical to the original but with one degree higher    *
******************************************************************************/
CagdCrvStruct *BspCrvDegreeRaise(CagdCrvStruct *Crv)
{
    CagdBType
	IsNotRational = !CAGD_IS_RATIONAL_CRV(Crv);
    int i, i2, j, RaisedLen,
	Order = Crv -> Order,
	Length = Crv -> Length,
	MaxCoord = CAGD_NUM_OF_PT_COORD(Crv -> PType);
    CagdCrvStruct *RaisedCrv;

    if (Order > 2) {
	FATAL_ERROR(CAGD_ERR_NOT_IMPLEMENTED);
	return NULL;
    }

    /* If curve is linear, degree raising means basically to increase the     */
    /* knot multiplicity of each segment by one and add a middle point for    */
    /* each such segment.						      */
    RaisedLen = Length * 2 - 1;
    RaisedCrv = BspCrvNew(RaisedLen, Order + 1, Crv -> PType);

    /* Update the control polygon; */
    for (j = IsNotRational; j <= MaxCoord; j++)		      /* First point. */
	RaisedCrv -> Points[j][0] = Crv -> Points[j][0];

    for (i = 1, i2 = 1; i < Length; i++, i2 += 2)
	for (j = IsNotRational; j <= MaxCoord; j++) {
	    RaisedCrv -> Points[j][i2] =
		Crv -> Points[j][i-1] * 0.5 + Crv -> Points[j][i] * 0.5;
	    RaisedCrv -> Points[j][i2 + 1] = Crv -> Points[j][i];
	}

    /* Update the knot vector. */
    for (i = 0; i < 3; i++)
	RaisedCrv -> KnotVector[i] = Crv -> KnotVector[0];

    for (i = 2, j = 3; i < Length; i++, j += 2)
	RaisedCrv -> KnotVector[j] = RaisedCrv -> KnotVector[j + 1] = 
	    Crv -> KnotVector[i];

    for (i = j; i < j + 3; i++)
	RaisedCrv -> KnotVector[i] = Crv -> KnotVector[Length];

    return RaisedCrv;
}

/******************************************************************************
* Return a normalized vector, equal to the tangent to Crv at parameter t.     *
* Algorithm: insert (order - 1) knots and return control polygon tangent.     *
******************************************************************************/
CagdVecStruct *BspCrvTangent(CagdCrvStruct *Crv, CagdRType t)
{
    static CagdVecStruct P2;
    CagdVecStruct P1;
    CagdRType TMin, TMax;
    int k = Crv -> Order,
	Len = Crv -> Length,
	Index = BspKnotLastIndexL(Crv -> KnotVector, k + Len, t);
    CagdPointType
	PType = Crv -> PType;
    CagdCrvStruct *RefinedCrv;

    if (!BspKnotParamInDomain(Crv -> KnotVector, Len, k, t))
	FATAL_ERROR(CAGD_ERR_T_NOT_IN_CRV);

    BspCrvDomain(Crv, &TMin, &TMax);

    if (APX_EQ(t, TMin)) {
	/* Use Crv starting tangent direction. */
	CagdCoerceToE3(P1.Vec, Crv -> Points, 0, PType);
	CagdCoerceToE3(P2.Vec, Crv -> Points, 1, PType);
    }
    else if (APX_EQ(t, TMax)) {
	/* Use Crv ending tangent direction. */
	CagdCoerceToE3(P1.Vec, Crv -> Points, Len - 2, PType);
	CagdCoerceToE3(P2.Vec, Crv -> Points, Len - 1, PType);
    }
    else {
	RefinedCrv = BspCrvKnotInsertNSame(Crv, t, k - 1);

	CagdCoerceToE3(P1.Vec, RefinedCrv -> Points, Index, PType);
	CagdCoerceToE3(P2.Vec, RefinedCrv -> Points, Index + 1, PType);

	CagdCrvFree(RefinedCrv);
    }

    CAGD_SUB_VECTOR(P2, P1);

    CAGD_NORMALIZE_VECTOR(P2);			    /* Normalize the vector. */

    return &P2;
}

/******************************************************************************
* Return a normalized vector, equal to the binormal to Crv at parameter t.    *
* Algorithm: insert (order - 1) knots and using 3 consecutive control points  *
* at the refined location (p1, p2, p3), compute to binormal to be the cross   *
* product of the two vectors (p1 - p2) and (p2 - p3).			      *
******************************************************************************/
CagdVecStruct *BspCrvBiNormal(CagdCrvStruct *Crv, CagdRType t)
{
    static CagdVecStruct P3;
    CagdVecStruct P1, P2;
    CagdRType TMin, TMax;
    int k = Crv -> Order,
	Len = Crv -> Length,
	Index = BspKnotLastIndexL(Crv -> KnotVector, k + Len, t);
    CagdPointType
	PType = Crv -> PType;
    CagdCrvStruct *RefinedCrv;

    if (!BspKnotParamInDomain(Crv -> KnotVector, Len, k, t))
	FATAL_ERROR(CAGD_ERR_T_NOT_IN_CRV);

    /* Can not compute for linear curves. */
    if (k <= 2) return NULL;

    BspCrvDomain(Crv, &TMin, &TMax);

    if (APX_EQ(t, TMin)) {
	/* Use Crv starting tangent direction. */
	CagdCoerceToE3(P1.Vec, Crv -> Points, 0, PType);
	CagdCoerceToE3(P2.Vec, Crv -> Points, 1, PType);
	CagdCoerceToE3(P3.Vec, Crv -> Points, 2, PType);
    }
    else if (APX_EQ(t, TMax)) {
	/* Use Crv ending tangent direction. */
	CagdCoerceToE3(P1.Vec, Crv -> Points, Len - 3, PType);
	CagdCoerceToE3(P2.Vec, Crv -> Points, Len - 2, PType);
	CagdCoerceToE3(P3.Vec, Crv -> Points, Len - 1, PType);
    }
    else {
	RefinedCrv = BspCrvKnotInsertNSame(Crv, t, k - 1);

	CagdCoerceToE3(P1.Vec, RefinedCrv -> Points, Index, PType);
	CagdCoerceToE3(P2.Vec, RefinedCrv -> Points, Index + 1, PType);
	CagdCoerceToE3(P3.Vec, RefinedCrv -> Points, Index + 2, PType);

	CagdCrvFree(RefinedCrv);
    }

    CAGD_SUB_VECTOR(P1, P2);
    CAGD_SUB_VECTOR(P2, P3);

    CROSS_PROD(P3.Vec, P1.Vec, P2.Vec);

    if ((t = CAGD_LEN_VECTOR(P3)) < EPSILON)
	return NULL;
    else
	CAGD_DIV_VECTOR(P3, t);			    /* Normalize the vector. */

    return &P3;
}

/******************************************************************************
* Return a normalized vector, equal to the normal to Crv at parameter t.      *
* Algorithm: returns the cross product of the curve tangent and binormal.     *
******************************************************************************/
CagdVecStruct *BspCrvNormal(CagdCrvStruct *Crv, CagdRType t)
{
    static CagdVecStruct N, *T, *B;

    T = BspCrvTangent(Crv, t);
    B = BspCrvBiNormal(Crv, t);

    if (T == NULL || B == NULL) return NULL;

    CROSS_PROD(N.Vec, T -> Vec, B -> Vec);

    CAGD_NORMALIZE_VECTOR(N);			    /* Normalize the vector. */

    return &N;
}

/******************************************************************************
* Return a new curve, equal to the derived curve. If the original curve is    *
* rational, NULL is return (curve must be non-rational for this one).	      *
* Let old control polygon be P(i), i = 0 to k-1, and Q(i) be new one then:    *
* Q(i) = (k - 1) * P(i+1) - P(i), i = 0 to k-2.				      *
******************************************************************************/
CagdCrvStruct *BspCrvDerive(CagdCrvStruct *Crv)
{
    CagdBType
	IsNotRational = !CAGD_IS_RATIONAL_CRV(Crv);
    int i, j,
	k = Crv -> Order,
	Len = Crv -> Length,
	MaxCoord = CAGD_NUM_OF_PT_COORD(Crv -> PType);
    CagdCrvStruct *DerivedCrv;

    if (!IsNotRational || k < 3)
	FATAL_ERROR(CAGD_ERR_RAT_LIN_NO_SUPPORT);

    DerivedCrv = BspCrvNew(Len - 1, k - 1, Crv -> PType);

    for (i = 0; i < Len - 1; i++)
	for (j = IsNotRational; j <= MaxCoord; j++)
	    DerivedCrv -> Points[j][i] =
		(k - 1) * (Crv -> Points[j][i+1] - Crv -> Points[j][i]);

    GEN_COPY(DerivedCrv -> KnotVector,
    	     &Crv -> KnotVector[1],
    	     sizeof(CagdRType) * (k + Len - 2));

    return DerivedCrv;
}