/******************************************************************************
* CagdSwep.c - Sweep srf operator out of given cross section and an axis.     *
*******************************************************************************
* Written by Gershon Elber, May. 91.					      *
******************************************************************************/

#include "cagd_loc.h"

#define MAX_AXIS_REFINE_LEVEL 50

static void TransformCrossSection(CagdRType **SPoints,
				  int Index,
				  CagdCrvStruct *CrossSection,
				  CagdRType *Position,
				  CagdRType Scale, 
				  CagdRType Weight,
				  CagdVecStruct *Tangent,
				  CagdVecStruct *Normal);
static void GenTransformMatrix(CagdMType Mat,
			       CagdRType *Trans,
			       CagdVecStruct *Normal,
			       CagdVecStruct *Tangent,
			       CagdRType Scale,
			       CagdRType Weight);

/*****************************************************************************
* DESCRIPTION:                                                               M
* Constructs a sweep surface using the following curves:                     M
* 1. CrossSection - defines the basic cross section of the sweep. Must be    M
*    in the XY plane.							     M
* 2. Axis - a 3D curve the CrossSection will be swept along such that the    M
*    Axis normal aligns with the Y axis of the cross section. If Axis is     M
*    linear (i.e. no normal), the normal is picked randomly or to fit the    M
*    non linear part of the Axis (if any).				     M
* 3. Scale - a scaling curve for the sweep, If NULL a scale of Scale is      M
*    used.								     M
* 4. Frame - a curve or a vector that specifies the orientation of the sweep M
*    by specifying the axes curve's binormal. If Frame is a vector, it is a  M
*    constant binormal. If Frame is a curve (FrameIsCrv = TRUE), it is       M
*    assumed to be a vector field binormal. If NULL, it is computed from     M
*    the Axis curve's pseudo Frenet frame, that minimizes rotation.	     M
*                                                                            M
*   This operation is only an approximation. See CagdSweepAxisRefine for a   M
* tool to refine the Axis curve and improve accuracy.                        M
*                                                                            *
* PARAMETERS:                                                                M
*   CrossSection:  Of the constructed sweep surface.                         M
*   Axis:          Of the constructed sweep surface.                         M
*   ScalingCrv:    Optional scale or profiel curve.                          M
*   Scale:         if no Scaling Crv, Scale is used to apply a fixed scale   M
*                  on the CrossSection curve.                                M
*   Frame:         An optional vector or a curve to specified the binormal   M
*                  orientation. Otherwise Frame must be NULL.		     M
*   FrameIsCrv:    If TRUE Frame is a curve, if FALSE a vector (if Frame is  M
*                  not NULL).					             M
*                                                                            *
* RETURN VALUE:                                                              M
*   CagdSrfStruct *:  Constructed sweep surface.                             M
*                                                                            *
* KEYWORDS:                                                                  M
*   CagdSweepSrf, sweep, surface constructors                                M
*****************************************************************************/
CagdSrfStruct *CagdSweepSrf(CagdCrvStruct *CrossSection,
			    CagdCrvStruct *Axis,
			    CagdCrvStruct *ScalingCrv,
			    CagdRType Scale,
			    VoidPtr Frame,
			    CagdBType FrameIsCrv)
{
    CagdSrfStruct
	*Srf = NULL;
    CagdVecStruct Normal, *Vec, TVec;
    CagdPointType
	SrfPType = CAGD_PT_E3_TYPE;
    CagdGeomType
	SrfGType = CAGD_SBSPLINE_TYPE;
    int i, j, k,
	ULength = CrossSection -> Length,
	VLength = Axis -> Length,
	UOrder = CrossSection -> Order,
	VOrder = Axis -> Order;
    CagdRType **Points,
	*AxisKV = NULL,
	*AxisNodes = NULL,
	*AxisNodePtr = NULL,
	*AxisWeights = NULL,
	*FrameVec = FrameIsCrv ? NULL : (CagdRType *) Frame;
    CagdCrvStruct
	*FrameCrv = FrameIsCrv ? (CagdCrvStruct *) Frame : NULL;

    switch (Axis -> GType) {
	case CAGD_CBEZIER_TYPE:
	    SrfGType = CAGD_SBEZIER_TYPE;
	    AxisKV = BspKnotUniformOpen(VLength, VOrder, NULL);
	    AxisNodes = AxisNodePtr = BspKnotNodes(AxisKV,
						   VLength + VOrder,
						   VOrder);
	    break;
	case CAGD_CBSPLINE_TYPE:
	    SrfGType = CAGD_SBSPLINE_TYPE;
	    AxisKV = Axis -> KnotVector;
	    AxisNodePtr = AxisNodes = BspKnotNodes(Axis -> KnotVector,
						   VLength + VOrder,
						   VOrder);
	    if (Axis -> Periodic) {
		/* Nodes will give us very skewed samples. Take middle of */
		/* every interior span as samples for axis positioning.   */
		for (i = 0; i < VLength; i++)
		    AxisNodes[i] = (AxisKV[i + VOrder - 1] +
				    AxisKV[i + VOrder]) / 2.0;
	    }
	    break;
	case CAGD_CPOWER_TYPE:
	    CAGD_FATAL_ERROR(CAGD_ERR_POWER_NO_SUPPORT);
	    break;
	default:
	    CAGD_FATAL_ERROR(CAGD_ERR_UNDEF_CRV);
	    break;
    }
    if (CAGD_IS_RATIONAL_CRV(Axis))
	AxisWeights = Axis -> Points[W];

    switch (CrossSection -> GType) {
	case CAGD_CBEZIER_TYPE:
	    break;
	case CAGD_CBSPLINE_TYPE:
	    SrfGType = CAGD_SBSPLINE_TYPE;
	    break;
	case CAGD_CPOWER_TYPE:
	    CAGD_FATAL_ERROR(CAGD_ERR_POWER_NO_SUPPORT);
	    break;
	default:
	    CAGD_FATAL_ERROR(CAGD_ERR_UNDEF_CRV);
	    break;
    }

    if (ScalingCrv) {
	int ScaleKVLen,
	    AxisKVLen = Axis -> Order + Axis -> Length;

	switch (ScalingCrv -> GType) {
	    case CAGD_CBEZIER_TYPE:
		ScalingCrv = CnvrtBezier2BsplineCrv(ScalingCrv);
	        break;
	    case CAGD_CBSPLINE_TYPE:
	        ScalingCrv = CagdCrvCopy(ScalingCrv);
		break;
	    case CAGD_CPOWER_TYPE:
		CAGD_FATAL_ERROR(CAGD_ERR_POWER_NO_SUPPORT);
		break;
	    default:
		CAGD_FATAL_ERROR(CAGD_ERR_UNDEF_CRV);
		break;
	}

	ScaleKVLen = ScalingCrv -> Order + ScalingCrv -> Length;

    	/* Affine trans. ScalingCrv KV to match Axis. */
	BspKnotAffineTrans(ScalingCrv -> KnotVector, ScaleKVLen,
			   AxisKV[0] - ScalingCrv -> KnotVector[0],
			   (AxisKV[AxisKVLen - 1] - AxisKV[0]) /
			   (ScalingCrv -> KnotVector[ScaleKVLen - 1] -
			    ScalingCrv -> KnotVector[0]));
    }

    if (FrameCrv) {
	int FrameKVLen,
	    AxisKVLen = Axis -> Order + Axis -> Length;

	switch (FrameCrv -> GType) {
	    case CAGD_CBEZIER_TYPE:
		FrameCrv = CnvrtBezier2BsplineCrv(FrameCrv);
	        break;
	    case CAGD_CBSPLINE_TYPE:
	        FrameCrv = CagdCrvCopy(FrameCrv);
		break;
	    case CAGD_CPOWER_TYPE:
		CAGD_FATAL_ERROR(CAGD_ERR_POWER_NO_SUPPORT);
		break;
	    default:
		CAGD_FATAL_ERROR(CAGD_ERR_UNDEF_CRV);
		break;
	}

	FrameKVLen = FrameCrv -> Order + FrameCrv -> Length;

    	/* Affine trans. FrameCrv KV to match Axis. */
	BspKnotAffineTrans(FrameCrv -> KnotVector, FrameKVLen,
			   AxisKV[0] - FrameCrv -> KnotVector[0],
			   (AxisKV[AxisKVLen - 1] - AxisKV[0]) /
			   (FrameCrv -> KnotVector[FrameKVLen - 1] -
			    FrameCrv -> KnotVector[0]));
    }

    if (CAGD_IS_RATIONAL_CRV(Axis) || CAGD_IS_RATIONAL_CRV(CrossSection))
	SrfPType = CAGD_PT_P3_TYPE;

    switch (SrfGType) {
	case CAGD_SBEZIER_TYPE:
	    Srf = BzrSrfNew(ULength, VLength, SrfPType);
	    break;
	case CAGD_SBSPLINE_TYPE:
	    Srf = BspPeriodicSrfNew(ULength, VLength, UOrder, VOrder,
				    CrossSection -> Periodic, Axis -> Periodic,
				    SrfPType);
	    if (CrossSection -> GType == CAGD_CBSPLINE_TYPE)
		CAGD_GEN_COPY(Srf -> UKnotVector, CrossSection -> KnotVector,
			      sizeof(CagdRType) *
				 (CAGD_CRV_PT_LST_LEN(CrossSection) + UOrder));
	    else
		BspKnotUniformOpen(ULength, UOrder, Srf -> UKnotVector);
	    if (Axis -> GType == CAGD_CBSPLINE_TYPE)
		CAGD_GEN_COPY(Srf -> VKnotVector, Axis -> KnotVector,
			      sizeof(CagdRType) *
			          (CAGD_CRV_PT_LST_LEN(Axis) + VOrder));
	    else
		BspKnotUniformOpen(VLength, VOrder, Srf -> VKnotVector);
	    break;
	default:
	    CAGD_FATAL_ERROR(CAGD_ERR_UNDEF_SRF);
	    break;
    }
    Points = Srf -> Points;

    /* Compute the first normal to be used to orient first cross section. */
    if (FrameVec == NULL && FrameCrv == NULL) {
	/* Compute the normal to axis curve and use it to align the +Y axis  */
	/* of cross section with that vector. If Axis curve has no normal    */
	/* (i.e. it is a linear segment), an arbitrary normal is picked.     */
	Vec = CagdCrvNormal(Axis, *AxisNodePtr);
	if (Vec != NULL) {
	    CAGD_COPY_VECTOR(Normal, *Vec);
	}
	else {
	    Vec = CagdCrvTangent(Axis, *AxisNodePtr);
	    Normal.Vec[0] = Normal.Vec[1] = Normal.Vec[2] = 0.0;
	    if (ABS(Vec -> Vec[0]) <= ABS(Vec -> Vec[1]) &&
		ABS(Vec -> Vec[0]) <= ABS(Vec -> Vec[2]))
		Normal.Vec[0] = 1.0;
	    else if (ABS(Vec -> Vec[1]) <= ABS(Vec -> Vec[0]) &&
		     ABS(Vec -> Vec[1]) <= ABS(Vec -> Vec[2]))
		Normal.Vec[1] = 1.0;
	    else
		Normal.Vec[2] = 1.0;

	    CROSS_PROD(TVec.Vec, Vec -> Vec, Normal.Vec);
	    CAGD_NORMALIZE_VECTOR(TVec);
	    CROSS_PROD(Normal.Vec, TVec.Vec, Vec -> Vec);
	}
    }

    /* For each ctl points of the axis, transform the cross section          */
    /* according to ctl point position, tangent to axis at the point and in  */
    /* such a way to minimize Normal change.				     */
    for (i = 0; i < VLength; i++, AxisNodePtr++) {
	CagdRType PosE3[3], ScaleE2[2],
	    *Scaling = ScalingCrv ? CagdCrvEval(ScalingCrv, *AxisNodePtr)
				  : NULL;
	CagdVecStruct
	    *Tangent = CagdCrvTangent(Axis, *AxisNodePtr);

	if (Scaling)
	    CagdCoerceToE2(ScaleE2, &Scaling, -1, ScalingCrv -> PType);
	else
	    ScaleE2[1] = Scale;

	/* If Normal is fully specified, get it now. */
	if (FrameVec != NULL) {
	    CROSS_PROD(Normal.Vec, FrameVec, Tangent -> Vec);
	    CAGD_NORMALIZE_VECTOR(Normal);
	}
	else if (FrameCrv != NULL) {
	    CagdRType Binormal[3],
		*FrameCrvVal = CagdCrvEval(FrameCrv, *AxisNodePtr);

	    CagdCoerceToE3(Binormal, &FrameCrvVal, -1, FrameCrv -> PType);

	    CROSS_PROD(Normal.Vec, Binormal, Tangent -> Vec);
	    CAGD_NORMALIZE_VECTOR(Normal);
	}

	if (Axis -> Periodic) {
	    CagdRType
		*R = CagdCrvEval(Axis, *AxisNodePtr);

	    CagdCoerceToE3(PosE3, &R, -1, Axis -> PType);
	}
	else
	    CagdCoerceToE3(PosE3, Axis -> Points, i, Axis -> PType);
	TransformCrossSection(Points, i * ULength, CrossSection,
			      PosE3, ScaleE2[1],
			      AxisWeights ? AxisWeights[i] : 1.0,
			      Tangent, &Normal);
    }

    /* Do fixups if axis is a rational curve (note surface is P3). */
    if (AxisWeights) {
	if (CAGD_IS_RATIONAL_CRV(CrossSection)) {
	    /* Need only scale by the Axis curve weights: */
	    for (j = 0, k = 0; j < VLength; j++)
		for (i = 0; i < ULength; i++, k++) {
		    Points[X][k] *= AxisWeights[j];
		    Points[Y][k] *= AxisWeights[j];
		    Points[Z][k] *= AxisWeights[j];
		    Points[W][k] *= AxisWeights[j];
		}
	}
	else {
	    /* Weights do not exists at the moment - need to copy them. */
	    for (j = 0, k = 0; j < VLength; j++)
		for (i = 0; i < ULength; i++, k++) {
		    Points[X][k] *= AxisWeights[j];
		    Points[Y][k] *= AxisWeights[j];
		    Points[Z][k] *= AxisWeights[j];
		    Points[W][k] = AxisWeights[j];
		}
	}
    }

    if (Axis -> GType == CAGD_CBEZIER_TYPE)
        IritFree((VoidPtr) AxisKV);
    if (ScalingCrv)
	CagdCrvFree(ScalingCrv);
    IritFree((VoidPtr) AxisNodes);

    return Srf;
}

/*****************************************************************************
* DESCRIPTION:                                                               *
* Transforms the CrossSection Points, to Position such that Tangent is       *
* perpendicular to the cross section (which is assumed to be on the XY       *
* plane). The +Y axis of the cross section is aligned with Normal direction  *
* to minimize twist along the sweep and been updated to new normal.	     *
*   Transformed cross section is place into srf Points, SPoints starting     *
* from index SIndex.							     *
*   All agrument vectors are assumed to be normalized to a unit length.	     *
*                                                                            *
* PARAMETERS:                                                                *
*   SPoints:        Final destination of the transformed points.             *
*   SIndex:         Index in SPOints where to start and place new points.    *
*   CrossSection:   To transform and place in SPoints.                       *
*   Position:       Translation factor.                                      *
*   Scale:          Scale factor.                                            *
*   Weight:         Weight factor if rational.                               *
*   Tangent:        Tangent direction to prescribe orientaion.               *
*   Normal:         Normal direction to prescribe orientaion.                *
*                                                                            *
* RETURN VALUE:                                                              *
*   void                                                                     *
*****************************************************************************/
static void TransformCrossSection(CagdRType **SPoints,
				  int SIndex,
				  CagdCrvStruct *CrossSection,
				  CagdRType *Position,
				  CagdRType Scale,
				  CagdRType Weight,
				  CagdVecStruct *Tangent,
				  CagdVecStruct *Normal)
{
    CagdBType
	IsNotRational = !CAGD_IS_RATIONAL_PT(CrossSection -> PType);
    CagdVecStruct TVec;
    CagdMType Mat;
    CagdCrvStruct
	*CSCopy = CagdCrvCopy(CrossSection);
    int i, j, MaxCoord,
	Len = CSCopy -> Length;
    CagdRType R,
	**CSPoints = CSCopy -> Points;

    /* Fix Normal to be perpendicular to the Tangent vector for a minimal */
    /* rotation. This ensures minimal twist in the resulting surface.	  */
    R = DOT_PROD(Normal -> Vec, Tangent -> Vec);
    TVec = *Tangent;
    CAGD_MULT_VECTOR(TVec, R);
    CAGD_SUB_VECTOR(*Normal, TVec);
    CAGD_NORMALIZE_VECTOR(*Normal);

    GenTransformMatrix(Mat, Position, Normal, Tangent, Scale, Weight);
    CagdCrvMatTransform(CSCopy, Mat);

    /* Max coord. may be modified by CagdCrvMatTransform to be 3D if was 2D! */
    MaxCoord = CAGD_NUM_OF_PT_COORD(CSCopy -> PType);

    for (i = 0; i < Len; i++)
	for (j = IsNotRational; j <= MaxCoord; j++)
	    SPoints[j][SIndex + i] = CSPoints[j][i];

    CagdCrvFree(CSCopy);
}

/*****************************************************************************
* DESCRIPTION:                                                               *
*   Routine to prepar a transformation martix to do the following (in this   *
* order): scale by Scale/Weight, rotate such that X axis is in Normal dir    *
* and Y is colinear with the BiNormal and then translate by Trans.	     *
*    Algorithm: given the Trans vector, it forms the 4th line of Mat. Dir is *
* used to form the second line (the first 3 lines set the rotation), and     *
* finally Scale is used to scale first 3 lines/columns to the needed scale:  *
*                |  Nx  Ny  Nz  0 |   A transformation which takes the coord *
*                |  Bx  By  Bz  0 |  system into T, N & B as required and    *
* [X  Y  Z  1] * |  Tx  Ty  Tz  0 |  then translate it to C. T, N, B are     *
*                |  Cx  Cy  Cz  1 |  scaled by Scale.			     *
* T is exactly Tangent (unit vec). N is set to be Normal and B their cross   *
* product.								     *
*   All agrument vectors are assumed to be normalized to a unit length.	     *
*                                                                            *
* PARAMETERS:                                                                *
*   Mat:        To place the newly computed transformation.                  *
*   Trans:      Translation factor.                                          *
*   Tangent:    Tangent direction to prescribe orientaion.                   *
*   Normal:     Normal direction to prescribe orientaion.                    *
*   Scale:      Scale factor.                                                *
*   Weight:     Weight factor.                                               *
*                                                                            *
* RETURN VALUE:                                                              *
*   void                                                                     *
*****************************************************************************/
static void GenTransformMatrix(CagdMType Mat,
			       CagdRType *Trans,
			       CagdVecStruct *Normal,
			       CagdVecStruct *Tangent,
			       CagdRType Scale,
			       CagdRType Weight)
{
    int i;
    CagdVecStruct B;
    CagdRType
	ScaleWeighted = Scale / Weight;

    CROSS_PROD(B.Vec, Tangent -> Vec, Normal -> Vec);

    for (i = 0; i < 3; i++) {
	Mat[0][i] = Normal -> Vec[i] * ScaleWeighted;
	Mat[1][i] = B.Vec[i] * Scale;
	Mat[2][i] = Tangent -> Vec[i] * ScaleWeighted;
	Mat[3][i] = Trans[i];
	Mat[i][3] = 0.0;
    }
    Mat[3][3] = 1.0;
}


/*****************************************************************************
* DESCRIPTION:                                                               M
*   Routine to refine the axis curve, according to the scaling curve to      M
* better approximate the requested sweep operation.			     M
*                                                                            *
* PARAMETERS:                                                                M
*   Axis:         Axis to be used in future sweep operation with the         M
*                 associated ScalingCrv.				     M
*   ScalingCrv:   To use as an estimate on refinement to apply to Axis.      M
*   RefLevel:     Some refinement control. Keep it low like 2 or 3.          M
*                                                                            *
* RETURN VALUE:                                                              M
*   CagdCrvStruct *:  Refined Axis curve.                                    M
*                                                                            *
* KEYWORDS:                                                                  M
*   CagdSweepAxisRefine, sweep, refinement                                   M
*****************************************************************************/
CagdCrvStruct *CagdSweepAxisRefine(CagdCrvStruct *Axis,
				   CagdCrvStruct *ScalingCrv,
				   int RefLevel)
{
    CagdBType NewScalingCrv;
    CagdCrvStruct
	*NewAxis = CagdCrvCopy(Axis);

    if (ScalingCrv == NULL || RefLevel < 1 || RefLevel > MAX_AXIS_REFINE_LEVEL)
	return CagdCrvCopy(Axis);

    if (CAGD_IS_BEZIER_CRV(Axis)) {
	CagdCrvFree(NewAxis);
	NewAxis = CnvrtBezier2BsplineCrv(Axis);
    }
    if (CAGD_IS_PERIODIC_CRV(NewAxis)) {
	CagdCrvStruct
	    *TCrv = CnvrtPeriodic2FloatCrv(NewAxis);

	CagdCrvFree(NewAxis);
	NewAxis = TCrv;
    }
    if (CAGD_IS_BSPLINE_CRV(NewAxis) && !BspCrvHasOpenEC(NewAxis)) {
	CagdCrvStruct
	    *TCrv = BspCrvOpenEnd(NewAxis);

	CagdCrvFree(NewAxis);
	NewAxis = TCrv;
    }

    if (CAGD_IS_BEZIER_CRV(ScalingCrv)) {
	NewScalingCrv = TRUE;
	ScalingCrv = CnvrtBezier2BsplineCrv(ScalingCrv);
    }
    else
	NewScalingCrv = FALSE;

    if (CAGD_IS_BSPLINE_CRV(ScalingCrv)) {
	int i, j,
	    SOrder = ScalingCrv -> Order,
	    SLength = ScalingCrv -> Length,
	    AOrder = NewAxis -> Order,
	    ALength = NewAxis -> Length;
	CagdRType
	    *AKV = NewAxis -> KnotVector,
	    *KV = BspKnotCopy(ScalingCrv -> KnotVector, SLength + SOrder),
	    *KVRef = (RealType *) IritMalloc(sizeof(CagdRType) * RefLevel *
						      (1 + SLength - SOrder));

	BspKnotAffineTrans(KV, SLength + SOrder,
			   AKV[AOrder - 1] - KV[SOrder - 1],
			   (AKV[ALength] - AKV[AOrder - 1]) /
			       (KV[SLength] - KV[SOrder - 1]));

	for (i = SOrder - 1, j = 0; i < SLength; i++) {
	    int k;
	    RealType
	        T1 = KV[i],
	        T2 = KV[i+1];

	    for (k = 0; k < RefLevel; k++)
		KVRef[j++] = (T1 * (RefLevel - k) + T2 * k) / RefLevel;
	}

	if (j > 1) {
	    /* Skip the first knot which is on the domain's boundary. */
	    Axis = CagdCrvRefineAtParams(NewAxis, FALSE, &KVRef[1], j - 1);
	}
	else
	    Axis = CagdCrvCopy(Axis);

	IritFree((VoidPtr) KV);
	IritFree((VoidPtr) KVRef);
    }
    else
        Axis = CagdCrvCopy(Axis);

    CagdCrvFree(NewAxis);
    if (NewScalingCrv)
	CagdCrvFree(ScalingCrv);

    return Axis;
}
