/******************************************************************************
* TrivEval.c - tri-variate function handling routines - evaluation routines.  *
*******************************************************************************
* (C) Gershon Elber, Technion, Israel Institute of Technology                 *
*******************************************************************************
* Written by Gershon Elber, Sep. 94.					      *
******************************************************************************/

#include <string.h>
#include "triv_loc.h"

/*****************************************************************************
* DESCRIPTION:                                                               M
* Evaluates the given tensor product trivariate at a given point, by         M
* extracting an isoparamteric surface along w and evaluating (u,v) in it.    M
*									     V
*           +-----------------------+					     V
*       W  /                       /|					     V
*      /  /                       / |					     V
*     /  /	U -->		 /  |  Orientation of			     V
*       +-----------------------+   |  control tri-variate mesh.	     V
*   V | |P0                 Pi-1|   +					     V
*     v	|Pi                P2i-1|  /					     V
*	|			| /					     V
*	|Pn-i		    Pn-1|/      				     V
*	+-----------------------+					     V
*                                                                            *
* PARAMETERS:                                                                M
*   TV:       To evaluate at given (u, v, w) parametric location.            M
*   u, v, w:  Parametric location to evaluate TV at.                         M
*                                                                            *
* RETURN VALUE:                                                              M
*   CagdRType *:  A vector holding all the coefficients of all components    M
*                 of the trivariate's point type. If for example trivariate  M
*                 point type is P2, the W, X, and Y will be saved in the     M
*                 first three locations of the returned vector. The first    M
*                 location (index 0) of the returned vector is reserved for  M
*                 the rational coefficient W and XYZ always starts at second M
*                 location of the returned vector (index 1).                 M
*                                                                            *
* KEYWORDS:                                                                  M
*   TrivTVEval, evaluation, trivariates                                      M
*****************************************************************************/
CagdRType *TrivTVEval(TrivTVStruct *TV, CagdRType u, CagdRType v, CagdRType w)
{
    static CagdSrfStruct
	*IsoSubSrf = NULL;
    CagdRType *Pt, *WBasisFunc, UMin, UMax, VMin, VMax, WMin, WMax;
    CagdBType
	IsNotRational = !TRIV_IS_RATIONAL_TV(TV);
    int k, UIndexFirst, VIndexFirst, WIndexFirst,
	UOrder = TV -> UOrder,
	VOrder = TV -> VOrder,
	WOrder = TV -> WOrder,
        ULength = TV -> ULength,
        VLength = TV -> VLength,
        WLength = TV -> WLength,
	MaxCoord = CAGD_NUM_OF_PT_COORD(TV -> PType);

    /* The code below is optimized for Bspline trivariates. For Bezier    */
    /* trivariate we have to process the entire data any way.             */
    if (TRIV_IS_BEZIER_TV(TV))
        return TrivTVEval2(TV, u, v, w);

    TrivTVDomain(TV, &UMin, &UMax, &VMin, &VMax, &WMin, &WMax);
    if (u < UMin - IRIT_EPS || u > UMax + IRIT_EPS ||
        v < VMin - IRIT_EPS || v > VMax + IRIT_EPS ||
        w < WMin - IRIT_EPS || w > WMax + IRIT_EPS)
        TRIV_FATAL_ERROR(TRIV_ERR_WRONG_DOMAIN);

    if (u > UMax - IRIT_UEPS * 2)
	u = UMax - IRIT_UEPS * 2;
    else if (u < UMin)
        u = UMin;
    if (v > VMax - IRIT_UEPS * 2)
	v = VMax - IRIT_UEPS * 2;
    else if (v < VMin)
        v = VMin;
    if (w > WMax - IRIT_UEPS * 2)
	w = WMax - IRIT_UEPS * 2;
    else if (w < WMin)
        w = WMin;

    UIndexFirst = BspKnotLastIndexLE(TV -> UKnotVector, ULength + UOrder, u) -
								  (UOrder - 1);
    VIndexFirst = BspKnotLastIndexLE(TV -> VKnotVector, VLength + VOrder, v) -
								  (VOrder - 1);
    WBasisFunc = BspCrvCoxDeBoorBasis(TV -> WKnotVector, WOrder,
				      WLength, w, &WIndexFirst);

    if (IsoSubSrf != NULL &&
	(TV -> PType != IsoSubSrf -> PType ||
	 UOrder != IsoSubSrf -> UOrder ||
	 VOrder != IsoSubSrf -> VOrder)) {
	/* The cached surface is not the proper type - release it. */
	CagdSrfFree(IsoSubSrf);
	IsoSubSrf = NULL;
    }
    if (IsoSubSrf == NULL) {
        IsoSubSrf = BspSrfNew(UOrder, VOrder, UOrder, VOrder, TV -> PType);
    }
    CAGD_GEN_COPY(IsoSubSrf -> UKnotVector,
		  &TV -> UKnotVector[UIndexFirst],
		  sizeof(CagdRType) * UOrder * 2);
    CAGD_GEN_COPY(IsoSubSrf -> VKnotVector,
		  &TV -> VKnotVector[VIndexFirst],
		  sizeof(CagdRType) * VOrder * 2);

    for (k = 0; k < UOrder; k++, UIndexFirst++) {
	int n,
	    VIndexFirstTmp = VIndexFirst;

	for (n = 0; n < VOrder; n++, VIndexFirstTmp++) {
	    int l;

	    for (l = IsNotRational; l <= MaxCoord; l++) {
		int i;
		CagdRType
		    *TVP = &TV -> Points[l][TRIV_MESH_UVW(TV,
							  UIndexFirst,
							  VIndexFirstTmp,
							  WIndexFirst)],
		    *SrfP = &IsoSubSrf -> Points[l][CAGD_MESH_UV(IsoSubSrf,
								 k, n)];

		*SrfP = 0.0;
		for (i = 0; i < WOrder; i++) {
		    *SrfP += WBasisFunc[i] * *TVP;
		    TVP += TRIV_NEXT_W(TV);
		}
	    }
	}
    }

    Pt = BspSrfEvalAtParam(IsoSubSrf, u, v);

    return Pt;
}

/*****************************************************************************
* DESCRIPTION:                                                               M
*   This function is the same as TrivTVEval2 above. Cleaner, but much less   M
* efficient.							             M
*   Evaluates the given tensor product trivariate at a given point, by       M
* extracting an isoparamteric surface along w and evaluating (u, v) in it.   M
*                                                                            *
* PARAMETERS:                                                                M
*   TV:       To evaluate at given (u, v, w) parametric location.            M
*   u, v, w:  Parametric location to evaluate TV at.                         M
*                                                                            *
* RETURN VALUE:                                                              M
*   CagdRType *:  A vector holding all the coefficients of all components    M
*                 of the trivariate's point type. If for example trivariate  M
*                 point type is P2, the W, X, and Y will be saved in the     M
*                 first three locations of the returned vector. The first    M
*                 location (index 0) of the returned vector is reserved for  M
*                 the rational coefficient W and XYZ always starts at second M
*                 location of the returned vector (index 1).                 M
*                                                                            *
* KEYWORDS:                                                                  M
*   TrivTVEval2, evaluation, trivariates                                     M
*****************************************************************************/
CagdRType *TrivTVEval2(TrivTVStruct *TV, CagdRType u, CagdRType v, CagdRType w)
{
    CagdRType *Pt;
    CagdSrfStruct
	*IsoSrf = TrivSrfFromTV(TV, u, TRIV_CONST_U_DIR);

    if (!TrivParamsInDomain(TV, u, v, w)) {
	TRIV_FATAL_ERROR(TRIV_ERR_WRONG_DOMAIN);
	return NULL;
    }

    Pt = CagdSrfEval(IsoSrf, v, w);

    CagdSrfFree(IsoSrf);

    return Pt;
}

/*****************************************************************************
* DESCRIPTION:                                                               M
* Extract an isoparametric surface out of the given tensor product	     M
* trivariate.         							     M
*   Operations should favor the CONST_W_DIR, in which the extraction is	     M
* somewhat faster, if it is possible.					     M
*                                                                            *
* PARAMETERS:                                                                M
*   TV:       To extract an isoparametric surface from at parameter value t  M
*	      in direction Dir.						     M
*   t:        Parameter value at which to extract the isosurface.            M
*   Dir:      Direction of isosurface extraction. Either U or V or W.        M
*                                                                            *
* RETURN VALUE:                                                              M
*   CagdSrfStruct *:   A bivariate surface which is an isosurface of TV.     M
*                                                                            *
* KEYWORDS:                                                                  M
*   TrivSrfFromTV, trivariates                                               M
*****************************************************************************/
CagdSrfStruct *TrivSrfFromTV(TrivTVStruct *TV,
			     CagdRType t,
			     TrivTVDirType Dir)
{
    CagdSrfStruct
	*Srf = NULL;
    CagdBType
	IsNotRational = !TRIV_IS_RATIONAL_TV(TV);
    int i, j, k, SrfLen,
	MaxCoord = CAGD_NUM_OF_PT_COORD(TV -> PType);
    CagdRType *SrfP, *TVP;

    if (!TrivParamInDomain(TV, t, Dir)) {
	TRIV_FATAL_ERROR(TRIV_ERR_WRONG_DOMAIN);
	return NULL;
    }

    switch (Dir) {
	case TRIV_CONST_U_DIR:
	    if (TV -> GType == TRIV_TVBSPLINE_TYPE) {
		Srf = BspSrfNew(TV -> VLength, TV -> WLength,
				TV -> VOrder, TV -> WOrder,
				TV -> PType);
		CAGD_GEN_COPY(Srf -> UKnotVector, TV -> VKnotVector,
			      sizeof(CagdRType) * (TV -> VLength +
						   TV -> VOrder));
		CAGD_GEN_COPY(Srf -> VKnotVector, TV -> WKnotVector,
			      sizeof(CagdRType) * (TV -> WLength +
						   TV -> WOrder));
	    }
	    else {
		Srf = BzrSrfNew(TV -> VLength, TV -> WLength, TV -> PType);
	    }
	    SrfLen = Srf -> ULength * Srf -> VLength;

	    if (TV -> GType == TRIV_TVBSPLINE_TYPE) {
		for (i = IsNotRational; i <= MaxCoord; i++) {
		    SrfP = Srf -> Points[i];
		    TVP = TV -> Points[i];
		    for (j = 0; j < SrfLen; j++) {
			*SrfP++ = BspCrvEvalVecAtParam(TVP, TRIV_NEXT_U(TV),
						       TV -> UKnotVector,
						       TV -> UOrder,
						       TV -> ULength,
						       FALSE, t);
			TVP += TRIV_NEXT_V(TV);
		    }
		}
	    }
	    else {
		for (i = IsNotRational; i <= MaxCoord; i++) {
		    SrfP = Srf -> Points[i];
		    TVP = TV -> Points[i];
		    for (j = 0; j < SrfLen; j++) {
			*SrfP++ = BzrCrvEvalVecAtParam(TVP, TRIV_NEXT_U(TV),
						       TV -> ULength, t);
			TVP += TRIV_NEXT_V(TV);
		    }
		}
	    }
	    break;
	case TRIV_CONST_V_DIR:
	    if (TV -> GType == TRIV_TVBSPLINE_TYPE) {
		Srf = BspSrfNew(TV -> ULength, TV -> WLength,
				TV -> UOrder, TV -> WOrder,
				TV -> PType);
		CAGD_GEN_COPY(Srf -> UKnotVector, TV -> UKnotVector,
			      sizeof(CagdRType) * (TV -> ULength +
						   TV -> UOrder));
		CAGD_GEN_COPY(Srf -> VKnotVector, TV -> WKnotVector,
			      sizeof(CagdRType) * (TV -> WLength +
						   TV -> WOrder));
	    }
	    else {
		Srf = BzrSrfNew(TV -> ULength, TV -> WLength, TV -> PType);
	    }
	    SrfLen = Srf -> ULength * Srf -> VLength;

	    if (TV -> GType == TRIV_TVBSPLINE_TYPE) {
		for (k = 0, i = IsNotRational; i <= MaxCoord; i++) {
		    SrfP = Srf -> Points[i];
		    TVP = TV -> Points[i];
		    for (j = 0; j < SrfLen; j++) {
			*SrfP++ = BspCrvEvalVecAtParam(TVP, TRIV_NEXT_V(TV),
						       TV -> VKnotVector,
						       TV -> VOrder,
						       TV -> VLength,
						       FALSE, t);
			TVP += TRIV_NEXT_U(TV);
			if (++k == TV -> ULength) {
			    TVP += TRIV_NEXT_W(TV) - TRIV_NEXT_U(TV) * k;
			    k = 0;
			}
		    }
		}
	    }
	    else {
		for (k = 0, i = IsNotRational; i <= MaxCoord; i++) {
		    SrfP = Srf -> Points[i];
		    TVP = TV -> Points[i];
		    for (j = 0; j < SrfLen; j++) {
			*SrfP++ = BzrCrvEvalVecAtParam(TVP, TRIV_NEXT_V(TV),
						       TV -> VLength, t);
			TVP += TRIV_NEXT_U(TV);
			if (++k == TV -> ULength) {
			    TVP += TRIV_NEXT_W(TV) - TRIV_NEXT_U(TV) * k;
			    k = 0;
			}
		    }
		}
	    }
	    break;
	case TRIV_CONST_W_DIR:
	    if (TV -> GType == TRIV_TVBSPLINE_TYPE) {
		Srf = BspSrfNew(TV -> ULength, TV -> VLength,
				TV -> UOrder, TV -> VOrder,
				TV -> PType);
		CAGD_GEN_COPY(Srf -> UKnotVector, TV -> UKnotVector,
			      sizeof(CagdRType) * (TV -> ULength +
						   TV -> UOrder));
		CAGD_GEN_COPY(Srf -> VKnotVector, TV -> VKnotVector,
			      sizeof(CagdRType) * (TV -> VLength +
						   TV -> VOrder));
	    }
	    else {
		Srf = BzrSrfNew(TV -> ULength, TV -> VLength, TV -> PType);
	    }
	    SrfLen = Srf -> ULength * Srf -> VLength;

	    if (TV -> GType == TRIV_TVBSPLINE_TYPE) {
		for (i = IsNotRational; i <= MaxCoord; i++) {
		    SrfP = Srf -> Points[i];
		    TVP = TV -> Points[i];
		    for (j = 0; j < SrfLen; j++) {
			*SrfP++ = BspCrvEvalVecAtParam(TVP, TRIV_NEXT_W(TV),
						       TV -> WKnotVector,
						       TV -> WOrder,
						       TV -> WLength,
						       FALSE, t);
			TVP += TRIV_NEXT_U(TV);
		    }
		}
	    }
	    else {
		for (i = IsNotRational; i <= MaxCoord; i++) {
		    SrfP = Srf -> Points[i];
		    TVP = TV -> Points[i];
		    for (j = 0; j < SrfLen; j++) {
			*SrfP++ = BzrCrvEvalVecAtParam(TVP, TRIV_NEXT_W(TV),
						       TV -> WLength, t);
			TVP += TRIV_NEXT_U(TV);
		    }
		}
	    }
	    break;
	default:
	    TRIV_FATAL_ERROR(TRIV_ERR_WRONG_DOMAIN);
	    break;
    }
    return Srf;
}

/*****************************************************************************
* DESCRIPTION:                                                               M
* Extract a bivariate surface out of the given trivariate's mesh.	     M
*   The provided (zero based) Index specifies which Index to extract.        M
*                                                                            *
* PARAMETERS:                                                                M
*   TV:        Trivariate to extract a bivariate surface out of its mesh.    M
*   Index:     Index of row/column/level of TV's mesh in direction Dir.      M
*   Dir:       Direction of isosurface extraction. Either U or V or W.       M
*                                                                            *
* RETURN VALUE:                                                              M
*   CagdSrfStruct *:   A bivariate surface which was extracted from TV's     M
*                      Mesh. This surface is not necessarily on TV.          M
*                                                                            *
* KEYWORDS:                                                                  M
*   TrivSrfFromMesh, trivariates                                             M
*****************************************************************************/
CagdSrfStruct *TrivSrfFromMesh(TrivTVStruct *TV,
			       int Index,
			       TrivTVDirType Dir)
{
    CagdSrfStruct
	*Srf = NULL;
    CagdBType
	IsNotRational = !TRIV_IS_RATIONAL_TV(TV);
    int i, j, k, SrfLen,
	MaxCoord = CAGD_NUM_OF_PT_COORD(TV -> PType);
    CagdRType *SrfP, *TVP;

    switch (Dir) {
	case TRIV_CONST_U_DIR:
	    if (Index >= TV -> ULength || Index < 0)
		TRIV_FATAL_ERROR(TRIV_ERR_INDEX_NOT_IN_MESH);

	    if (TV -> GType == TRIV_TVBSPLINE_TYPE) {
		Srf = BspSrfNew(TV -> VLength, TV -> WLength,
				TV -> VOrder, TV -> WOrder,
				TV -> PType);
		CAGD_GEN_COPY(Srf -> UKnotVector, TV -> VKnotVector,
			      sizeof(CagdRType) * (TV -> VLength +
						   TV -> VOrder));
		CAGD_GEN_COPY(Srf -> VKnotVector, TV -> WKnotVector,
			      sizeof(CagdRType) * (TV -> WLength +
						   TV -> WOrder));
	    }
	    else {
		Srf = BzrSrfNew(TV -> VLength, TV -> WLength, TV -> PType);
	    }
	    SrfLen = Srf -> ULength * Srf -> VLength;

	    for (i = IsNotRational; i <= MaxCoord; i++) {
		SrfP = Srf -> Points[i];
		TVP = TV -> Points[i] + Index * TRIV_NEXT_U(TV);
		for (j = 0; j < SrfLen; j++) {
		    *SrfP++ = *TVP;
		    TVP += TRIV_NEXT_V(TV);
		}
	    }
	    break;
	case TRIV_CONST_V_DIR:
	    if (Index >= TV -> VLength || Index < 0)
		TRIV_FATAL_ERROR(TRIV_ERR_INDEX_NOT_IN_MESH);

	    if (TV -> GType == TRIV_TVBSPLINE_TYPE) {
		Srf = BspSrfNew(TV -> ULength, TV -> WLength,
				TV -> UOrder, TV -> WOrder,
				TV -> PType);
		CAGD_GEN_COPY(Srf -> UKnotVector, TV -> UKnotVector,
			      sizeof(CagdRType) * (TV -> ULength +
						   TV -> UOrder));
		CAGD_GEN_COPY(Srf -> VKnotVector, TV -> WKnotVector,
			      sizeof(CagdRType) * (TV -> WLength +
						   TV -> WOrder));
	    }
	    else {
		Srf = BzrSrfNew(TV -> ULength, TV -> WLength, TV -> PType);
	    }
	    SrfLen = Srf -> ULength * Srf -> VLength;

	    for (k = 0, i = IsNotRational; i <= MaxCoord; i++) {
		SrfP = Srf -> Points[i];
		TVP = TV -> Points[i]+ Index * TRIV_NEXT_V(TV);
		for (j = 0; j < SrfLen; j++) {
		    *SrfP++ = *TVP;
		    TVP += TRIV_NEXT_U(TV);
		    if (++k == TV -> ULength) {
			TVP += TRIV_NEXT_W(TV) - TRIV_NEXT_U(TV) * k;
			k = 0;
		    }
		}
	    }
	    break;
	case TRIV_CONST_W_DIR:
	    if (Index >= TV -> WLength || Index < 0)
		TRIV_FATAL_ERROR(TRIV_ERR_INDEX_NOT_IN_MESH);

	    if (TV -> GType == TRIV_TVBSPLINE_TYPE) {
		Srf = BspSrfNew(TV -> ULength, TV -> VLength,
				TV -> UOrder, TV -> VOrder,
				TV -> PType);
		CAGD_GEN_COPY(Srf -> UKnotVector, TV -> UKnotVector,
			      sizeof(CagdRType) * (TV -> ULength +
						   TV -> UOrder));
		CAGD_GEN_COPY(Srf -> VKnotVector, TV -> VKnotVector,
			      sizeof(CagdRType) * (TV -> VLength +
						   TV -> VOrder));
	    }
	    else {
		Srf = BzrSrfNew(TV -> ULength, TV -> VLength, TV -> PType);
	    }
	    SrfLen = Srf -> ULength * Srf -> VLength;

	    for (i = IsNotRational; i <= MaxCoord; i++) {
		SrfP = Srf -> Points[i];
		TVP = TV -> Points[i]+ Index * TRIV_NEXT_W(TV);
		for (j = 0; j < SrfLen; j++) {
		    *SrfP++ = *TVP;
		    TVP += TRIV_NEXT_U(TV);
		}
	    }
	    break;
	default:
	    TRIV_FATAL_ERROR(TRIV_ERR_WRONG_DOMAIN);
	    break;
    }
    return Srf;
}

/*****************************************************************************
* DESCRIPTION:                                                               M
* Substitute a bivariate surface into a given trivariate's mesh.	     M
*   The provided (zero based) Index specifies which Index to extract.        M
*                                                                            *
* PARAMETERS:                                                                M
*   Srf:       Surface to substitute into the trivariate TV.		     M
*   Index:     Index of row/column/level of TV's mesh in direction Dir.      M
*   Dir:       Direction of isosurface extraction. Either U or V or W.       M
*   TV:        Trivariate to substitute a bivariate surface into its mesh.   M
*                                                                            *
* RETURN VALUE:                                                              M
*   void	                                                             M
*                                                                            *
* KEYWORDS:                                                                  M
*   TrivSrfToMesh, trivariates                                               M
*****************************************************************************/
void TrivSrfToMesh(CagdSrfStruct *Srf,
		   int Index,
		   TrivTVDirType Dir,
		   TrivTVStruct *TV)
{
    CagdBType
	IsNotRational = !TRIV_IS_RATIONAL_TV(TV);
    int i, j, k,
	SrfLen = Srf -> ULength * Srf -> VLength,
	MaxCoord = CAGD_NUM_OF_PT_COORD(TV -> PType);
    CagdRType *SrfP, *TVP;

    switch (Dir) {
	case TRIV_CONST_U_DIR:
	    if (Index >= TV -> ULength || Index < 0)
		TRIV_FATAL_ERROR(TRIV_ERR_INDEX_NOT_IN_MESH);

	    for (i = IsNotRational; i <= MaxCoord; i++) {
		SrfP = Srf -> Points[i];
		TVP = TV -> Points[i] + Index * TRIV_NEXT_U(TV);
		for (j = 0; j < SrfLen; j++) {
		    *TVP = *SrfP++;
		    TVP += TRIV_NEXT_V(TV);
		}
	    }
	    break;
	case TRIV_CONST_V_DIR:
	    if (Index >= TV -> VLength || Index < 0)
		TRIV_FATAL_ERROR(TRIV_ERR_INDEX_NOT_IN_MESH);

	    for (k = 0, i = IsNotRational; i <= MaxCoord; i++) {
		SrfP = Srf -> Points[i];
		TVP = TV -> Points[i]+ Index * TRIV_NEXT_V(TV);
		for (j = 0; j < SrfLen; j++) {
		    *TVP = *SrfP++;
		    TVP += TRIV_NEXT_U(TV);
		    if (++k == TV -> ULength) {
			TVP += TRIV_NEXT_W(TV) - TRIV_NEXT_U(TV) * k;
			k = 0;
		    }
		}
	    }
	    break;
	case TRIV_CONST_W_DIR:
	    if (Index >= TV -> WLength || Index < 0)
		TRIV_FATAL_ERROR(TRIV_ERR_INDEX_NOT_IN_MESH);

	    for (i = IsNotRational; i <= MaxCoord; i++) {
		SrfP = Srf -> Points[i];
		TVP = TV -> Points[i]+ Index * TRIV_NEXT_W(TV);
		for (j = 0; j < SrfLen; j++) {
		    *TVP = *SrfP++;
		    TVP += TRIV_NEXT_U(TV);
		}
	    }
	    break;
	default:
	    TRIV_FATAL_ERROR(TRIV_ERR_WRONG_DOMAIN);
	    break;
    }
}