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

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

#include "cagd_loc.h"

static void TransformCrossSection(CagdRType **SPoints, int Index,
		      CagdCrvStruct *CrossSection, CagdRType *Position,
		      CagdRType Scale, CagdVecStruct *Tangent,
		      CagdVecStruct *Normal);
static void GenTransformMatrix(CagdMatStruct *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. *
******************************************************************************/
CagdSrfStruct *CagdSweepSrf(CagdCrvStruct *CrossSection, CagdCrvStruct *Axis,
				    CagdCrvStruct *ScalingCrv, CagdRType Scale)
{
    CagdSrfStruct *Srf;
    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, *AxisNodes, *AxisNodePtr, *AxisKV,
	*AxisWeights = 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 (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)
		GEN_COPY(Srf -> UKnotVector, CrossSection -> KnotVector,
				sizeof(CagdRType) * (ULength + UOrder));
	    else
		BspKnotUniformOpen(ULength, UOrder, Srf -> UKnotVector);
	    if (Axis -> GType == CAGD_CBSPLINE_TYPE)
		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 normal to the axis curve and use it to align the +Y axis  */
    /* of the cross section with that vector. If the 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++) {
	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];

	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)
        CagdFree((VoidPtr) AxisKV);
    if (ScalingCrv)
	CagdCrvFree(ScalingCrv);
    CagdFree((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;
    CagdMatStruct 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(CagdMatStruct *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 -> Mat[0][i] = Normal -> Vec[i] * Scale;
	Mat -> Mat[1][i] = B.Vec[i] * Scale;
	Mat -> Mat[2][i] = Dir -> Vec[i] * Scale;
	Mat -> Mat[3][i] = Trans[i];
	Mat -> Mat[i][3] = 0.0;
    }
    Mat -> Mat[3][3] = 1.0;
}