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

#include "cagd_loc.h"

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

/******************************************************************************
* Constructs a sweep surface using the following curves:                      *
* 1. CrossSection - defines the basic cross section of the sweep. Must be in  *
*    the XY plane.							      *
* 2. Axis - a 3D curve the CrossSection will be swept along such that the     *
*    Axis normal aligns with the Y axis of the cross section. If Axis is      *
*    linear (i.e. no normal), the normal is picked randomly or to fit the non *
*    linear part of the Axis (if any).					      *
* 3. Scale - a scaling curve for the sweep, If NULL a scale of Scale is used. *
* 4. Frame - a curve or a vector that specifies the orientation of the sweep  *
*    by specifying the axes curve's normal. If Frame is a vector, it is a     *
*    constant normal. If Frame is a curve (FrameIsCrv = TRUE), it is assumed  *
*    to be a vector field normal. If NULL, it is computed from axes curve's   *
*    pseudo Frenet frame, that minimizes rotation.			      *
******************************************************************************/
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);
	    break;
	case CAGD_CPOWER_TYPE:
	    FATAL_ERROR(CAGD_ERR_POWER_NO_SUPPORT);
	    break;
	default:
	    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:
	    FATAL_ERROR(CAGD_ERR_POWER_NO_SUPPORT);
	    break;
	default:
	    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:
		FATAL_ERROR(CAGD_ERR_POWER_NO_SUPPORT);
		break;
	    default:
		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:
		FATAL_ERROR(CAGD_ERR_POWER_NO_SUPPORT);
		break;
	    default:
		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 = BspSrfNew(ULength, VLength, UOrder, VOrder, SrfPType);
	    if (CrossSection -> GType == CAGD_CBSPLINE_TYPE)
		CAGD_GEN_COPY(Srf -> UKnotVector, CrossSection -> KnotVector,
			      sizeof(CagdRType) * (ULength + UOrder));
	    else
		BspKnotUniformOpen(ULength, UOrder, Srf -> UKnotVector);
	    if (Axis -> GType == CAGD_CBSPLINE_TYPE)
		CAGD_GEN_COPY(Srf -> VKnotVector, Axis -> KnotVector,
			      sizeof(CagdRType) * (VLength + VOrder));
	    else
		BspKnotUniformOpen(VLength, VOrder, Srf -> VKnotVector);
	    break;
	default:
	    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 (AxisWeights)
	    ScaleE2[1] /= AxisWeights[i];

	/* If Normal is fully specified, get it now. */
	if (FrameVec != NULL) {
	    PT_COPY(Normal.Vec, FrameVec);
	}
	else if (FrameCrv != NULL) {
	    CagdRType
		*FrameCrvVal = CagdCrvEval(FrameCrv, *AxisNodePtr);

	    CagdCoerceToE3(Normal.Vec, &FrameCrvVal, -1, FrameCrv -> PType);
	    CAGD_NORMALIZE_VECTOR(Normal);
	}

	CagdCoerceToE3(PosE3, Axis -> Points, i, Axis -> PType);
	TransformCrossSection(Points, i * ULength, CrossSection,
			      PosE3, ScaleE2[1], 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;
}

/******************************************************************************
* Transform 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.	      *
******************************************************************************/
static void TransformCrossSection(CagdRType **SPoints, int SIndex,
		      CagdCrvStruct *CrossSection, CagdRType *Position,
		      CagdRType Scale, CagdVecStruct *Tangent,
		      CagdVecStruct *Normal)
{
    CagdPointType
	PType = CrossSection -> PType;
    CagdBType
	IsNotRational = !CAGD_IS_RATIONAL_PT(PType);
    CagdVecStruct TVec;
    CagdMType Mat;
    CagdCrvStruct
	*CSCopy = CagdCrvCopy(CrossSection);
    int i, j,
	MaxCoord = CAGD_NUM_OF_PT_COORD(PType),
	Len = CSCopy -> Length;
    CagdRType R,
	**CSPoints = CSCopy -> Points;

    /* Fix the Normal to be perpendicular to the Tangent vector is 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);
    CagdCrvMatTransform(CSCopy, Mat);

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

    CagdCrvFree(CSCopy);
}

/******************************************************************************
*   Routine to preper transformation martix to do the following (in this      *
* order): scale by Scale, rotate such that the Z axis is in direction Dir     *
* and Y is colinear with the Normal 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:   *
*                |  Tx  Ty  Tz  0 |   A transformation which takes the coord  *
*                |  Bx  By  Bz  0 |  system into T, N & B as required and     *
* [X  Y  Z  1] * |  Nx  Ny  Nz  0 |  then translate it to C. T, N, B are      *
*                |  Cx  Cy  Cz  1 |  scaled by Scale.			      *
* N is exactly Dir (unit vec). T is set to be the Normal and B their cross    *
* product.								      *
*   All agrument vectors are assumed to be normalized to a unit length.	      *
******************************************************************************/
static void GenTransformMatrix(CagdMType Mat, CagdRType *Trans,
			       CagdVecStruct *Normal, CagdVecStruct *Dir,
			       CagdRType Scale)
{
    int i;
    CagdVecStruct B;

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

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