Пример #1
0
static void NURBScurveEval(int deg, double *cp, int ncp, double *knot, double *us, int nus, double *ep, double *left, double *right, double *N){
    /* Modification of  ALGORITHM A4.1, The NURBS Book, L.Piegl and W. Tiller */
    
    /* Evaluates a NURBS curve at given parameter values */
    
    /* NURBScurveEval( deg - degree of NURBS, cp - pointer to control points, ncp - number of control points, knot - pointer to knot sequence, us - pointer to parameter values, nus - number of parameter values, ep - pointer to evaluated points, left - pointer to array for function BasisFuns, right - pointer to array for function BasisFuns, N - pointer to array for function BasisFuns) */
    
    int i, jj, span, ind, fourncp;
    double wgh;
    
    fourncp=4*ncp;
    
    for (jj = 0; jj < nus; jj++){
        
        if(us[jj]<=knot[deg]){
            ep[jj*3] = cp[0]/cp[3];
            ep[jj*3+1] = cp[1]/cp[3];
            ep[jj*3+2] = cp[2]/cp[3];
        }
        else if(us[jj]>=knot[ncp]){
            ep[jj*3] = cp[fourncp-4]/cp[fourncp-1];
            ep[jj*3+1] = cp[fourncp-3]/cp[fourncp-1];
            ep[jj*3+2] = cp[fourncp-2]/cp[fourncp-1];
        }
        else{
            ep[jj*3]=0.0;
            ep[jj*3+1]=0.0;
            ep[jj*3+2]=0.0;
            wgh=0.0;
            
            span = FindSpan(ncp, deg, us[jj], knot);
            BasisFuns(span, us[jj], deg, knot, N, left, right);
            
            ind = span - deg;

            for (i = 0; i <= deg; i++){
                ep[jj*3] += N[i] * cp[(i+ind)*4];
                ep[jj*3+1] += N[i] * cp[(i+ind)*4+1];
                ep[jj*3+2] += N[i] * cp[(i+ind)*4+2];
                wgh += N[i] * cp[(i+ind)*4+3];
            }
            ep[jj*3]=ep[jj*3]/wgh;
            ep[jj*3+1]=ep[jj*3+1]/wgh;
            ep[jj*3+2]=ep[jj*3+2]/wgh;
            
        }
        
    }
    
}
void CurvePoint(int n, int p, double *U, double *P, double u, double *S)
{
	double *Nu;
	int k, uspan, uind;
	Nu = (double*)malloc(sizeof(double) * (ORD+1));

	uspan = FindSpan(n, p, u, U);
	BasisFuns(uspan, u, p, U, Nu);
	uind = uspan - p;

	*S = 0.0;
	for(k=0; k<=p; k++) 
	{
		*S += Nu[k] * P[uind+k];
	}
}
Пример #3
0
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
    /*	Return the NURBS basis function to matlab

    //
    // We expect the function to be called as [Nip] = NURBSBasis(i, p, xi, knot, weights)
    //
    // p = order of basis (0,1,2 etc)
    // knot = knot vector
    // i = basis function we want (1,2,3 ie. Hughes' notation)
    // xi = the coordinate we wish to evaluate at
    // weights = the weightings for the NURNS function (length(weights) = m - p -1) */

    if(nrhs != 5) mexErrMsgTxt("You fool! You haven't passed in 5 arguments to the function."
                                   "We expect it to be in the form [Nip] = NURBSBasis(i, p, xi, knot, weights)\n");

    int c, k, p, m, i, numKnot, numWeights;

    double *p_in, *m_in, *i_in;
    double *knot, *xi, *weight;

    double tol=100*DBL_EPSILON;

    for(c = 0; c < nrhs; c++)
    {
        switch(c)
        {
        case 0:
            i_in = mxGetPr(prhs[c]);
            i = (int)*i_in -1;
        /*mexPrintf("\n\ni=%d\n", i);*/

        case 1:
            p_in = mxGetPr(prhs[c]);
            p = (int)*p_in;
        /*mexPrintf("\n\np=%d\n", p);*/

        case 2:
            xi = mxGetPr(prhs[c]);
        /*mexPrintf("\nxi=%2.20f\n", *xi);											*/

        case 3:
            knot = mxGetPr(prhs[c]);
            numKnot = mxGetN(prhs[c]);
            m = numKnot - 1;
        /*mexPrintf("\nWe have a knot vector with %d components\n", numKnot);
        for(k = 0; k < numKnot; k++) mexPrintf("%2.2f\t", *(knot+k));
        mexPrintf("\n");*/

        case 4:
            weight = mxGetPr(prhs[c]);
            numWeights = mxGetM(prhs[c]);

        }
    }

    if(fabs(*xi-knot[numKnot-1]) < tol)
        *xi = knot[numKnot-1] - tol;


    /* and call the basis function routine*/

    double Rip, Nip, dRip_dxi;
    double w_interp, dw_interp_dxi ;

    int n = m - p -1;

    double *N = (double *)malloc(sizeof(double)*(p+1));
    double *dN = (double *)malloc(sizeof(double)*(p+1));
    double **ders = init2DArray(p+1, p+1);

    int span = FindSpan(n, p, *xi, knot);
    BasisFuns(span, *xi, p, knot, N);
    dersBasisFuns(span, *xi, p, p, knot, ders);

    w_interp = 0.0;
    dw_interp_dxi = 0.0;

    for(c = 0; c <= p; c++)
    {
        w_interp += N[c] * weight[span-p+c];
        dw_interp_dxi += ders[1][c] * weight[span-p+c];
    }

    dersOneBasisFuns(p, m, knot, i, *xi, p, dN);

    Nip = OneBasisFun(p, m, knot, i, *xi);

    Rip = Nip * weight[i] / w_interp;

    dRip_dxi = weight[i] * ( w_interp * dN[1] - dw_interp_dxi * Nip ) / (w_interp * w_interp);


    free2Darray(ders, (p+1));
    free(N);
    free(dN);

    plhs[0] = mxCreateDoubleScalar(Rip);
    plhs[1] = mxCreateDoubleScalar(dRip_dxi);

}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
	/*	Return the NURBS basis function to matlab
	
	//
	// We expect the function to be called as [interp interp_deriv] = NURBSinterpolation(xi, p, knot, points, weights)
	//
	//	xi = point where we want to interpolate
	//	knot = knot vector
	//	vector of points in format [pt1 pt2 pt3 pt4 .... ptn] */
	
	if(nrhs != 5) mexErrMsgTxt("You fool! You haven't passed in 3 arguments to the function."
								"We expect it to be in the form [interp interp_deriv] = NURBSinterpolation(xi, knot, points)\n");
			
	/* First get the inputs */
	
	double *xi = mxGetPr(prhs[0]);	

	double *p_in = (mxGetPr(prhs[1]));
	int p = (int) *p_in;
	
	double *knot = mxGetPr(prhs[2]);
	int numKnot = mxGetN(prhs[2]);
	int m = numKnot - 1;
	int n = m - p -1;
	
	double *points = mxGetPr(prhs[3]);
	int numPoints = mxGetN(prhs[3]);
	
	double *weight = mxGetPr(prhs[4]);
	int numWeights = mxGetN(prhs[4]);
	
	double tol=100*DBL_EPSILON;
	
	if(fabs(*xi-knot[numKnot-1]) < tol) 
		*xi = knot[numKnot-1] - tol;
	
	/* and evaluate the non-zero basis functions*/
	
	double *N = (double *)malloc(sizeof(double)*(p+1));
	double *NURBS = (double *)malloc(sizeof(double)*(p+1));
	double *NURBS_deriv = (double *)malloc(sizeof(double)*(p+1));
	double **ders = init2DArray(n+1, p+1);
	
	int span = FindSpan(n, p, *xi, knot); 
	BasisFuns(span, *xi, p, knot, N);
	dersBasisFuns(span, *xi, p, n, knot, ders);	
	
	/* and create NURBS approximation */
	int k, c;
	
	for(k = 0; k <=p; k++)
	{
		double w_interp = 0.0, dw_interp_dxi= 0.0;
		for(c = 0; c <= p; c++)
		{
			w_interp += N[c] * weight[span-p+c];
			dw_interp_dxi += ders[1][c] * weight[span-p+c];
		}
		NURBS[k] = N[k] * weight[span-p+k] / w_interp;
		NURBS_deriv[k] =  weight[span-p+k] * ( w_interp * ders[1][k] - dw_interp_dxi * N[k] ) / (w_interp * w_interp);
	}

	double interp = 0.0;
	double interp_deriv = 0.0;
	
	for(c = 0; c <= p; c++)
	{
		interp += NURBS[c] * points[span-p+c];
		interp_deriv += NURBS_deriv[c] * points[span-p+c];
	}
	
	free(N);
	free(NURBS);
	free(NURBS_deriv);
	free2Darray(ders, (n+1));
	
	plhs[0] = mxCreateDoubleScalar(interp); plhs[1] = mxCreateDoubleScalar(interp_deriv);


}
main()
{
	FILE *Bspline=fopen("Bspline.dat","wt");
	FILE *Bsplineslope=fopen("Bsplineslope.dat","wt");

	FILE *CP=fopen("CP.dat","wt");
	FILE *BPxy=fopen("BPxy.dat","wt");
	
	FILE *test=fopen("test.dat","wt");

	int k, i, j, spn, *pivot;

	double *BaseP_x, *BaseP_y, *End_BaseP_x, *End_BaseP_y, angle, arc, D0_x, D0_y, Dn_x, Dn_y;
	double *BP_sn, *BP_dests, **BP_N, **temp_BP_N, *N, plusminusone;

	double *x, *y, *u, **xdev, **ydev;	
	double *ksi, *eta, *rhs;

	double *U, *Nonuni_U, *End_Nonuni_U;
	double maxu, delu, **nders;
	double *nx, *ny, *kslope;

	BaseP_x = (double*)malloc(sizeof(double) * BP);
	BaseP_y = (double*)malloc(sizeof(double) * BP);
	End_BaseP_x = (double*)malloc(sizeof(double) * (BP+ENDD));
	End_BaseP_y = (double*)malloc(sizeof(double) * (BP+ENDD));
	BP_sn = (double*)malloc(sizeof(double) * BP);
	BP_dests = (double*)malloc(sizeof(double));

	N = (double*)malloc(sizeof(double) * (ORD+1));
	U = (double*)malloc(sizeof(double) * (VER_ARR+ENDD));
	Nonuni_U = (double*)malloc(sizeof(double) * VER_ARR);
	End_Nonuni_U = (double*)malloc(sizeof(double) * (VER_ARR+ENDD));
	pivot = (int*)malloc(sizeof(int) * (VER+ENDD));

	rhs = (double*)malloc(sizeof(double) * (VER+ENDD));
	ksi = (double*)malloc(sizeof(double) * (VER+ENDD));
	eta = (double*)malloc(sizeof(double) * (VER+ENDD));
	x = (double*)malloc(sizeof(double) * Num);
	y = (double*)malloc(sizeof(double) * Num);
	u = (double*)malloc(sizeof(double) * Num);
	nx = (double*)malloc(sizeof(double) * Num);
	ny = (double*)malloc(sizeof(double) * Num);
	kslope = (double*)malloc(sizeof(double) * Num);

	nders = (double**)malloc(sizeof(double*) * (ORD+1));
	nders[0] = (double*)malloc(sizeof(double) * (ORD+1) * (ORD+1));
	for(i=1; i<(ORD+1); i++)
	{
		nders[i] = nders[i-1] + (ORD+1);
	}

	xdev = (double**)malloc(sizeof(double*) * 2);
	xdev[0] = (double*)malloc(sizeof(double) * 2 * Num);
	for(i=1; i<2; i++)
	{
		xdev[i] = xdev[i-1] + Num;
	}

	ydev = (double**)malloc(sizeof(double*) * 2);
	ydev[0] = (double*)malloc(sizeof(double) * 2 * Num);
	for(i=1; i<2; i++)
	{
		ydev[i] = ydev[i-1] + Num;
	}
/*
	BP_N = (double**)malloc(sizeof(double*) * BP);
	BP_N[0] = (double*)malloc(sizeof(double) * BP * BP);
	for(i=1; i<BP; i++)
	{
		BP_N[i] = BP_N[i-1] + BP;
	}
	
	temp_BP_N = (double**)malloc(sizeof(double*) * BP);
	temp_BP_N[0] = (double*)malloc(sizeof(double) * BP * BP);
	for(i=1; i<BP; i++)
	{
		temp_BP_N[i] = temp_BP_N[i-1] + BP;
	}
*/	
	BP_N = (double**)malloc(sizeof(double*) * (BP+ENDD));
	BP_N[0] = (double*)malloc(sizeof(double) * (BP+ENDD) * (BP+ENDD));
	for(i=1; i<(BP+ENDD); i++)
	{
		BP_N[i] = BP_N[i-1] + (BP+ENDD);
	}
	
	temp_BP_N = (double**)malloc(sizeof(double*) * (BP+ENDD));
	temp_BP_N[0] = (double*)malloc(sizeof(double) * (BP+ENDD) * (BP+ENDD));
	for(i=1; i<(BP+ENDD); i++)
	{
		temp_BP_N[i] = temp_BP_N[i-1] + (BP+ENDD);
	}

	//...init datapoint	.... bow shape

	BaseP_x[0] = 0.000; 
	BaseP_x[1] = 0.200; 
	BaseP_x[2] = 1.000; 
	BaseP_x[3] = 1.500; 
	BaseP_x[4] = 2.500; 
	BaseP_x[5] = 3.100; 
	BaseP_x[6] = 2.500; 
//	BaseP_x[7] = 3.500; 

	BaseP_y[0] = 0.000;
	BaseP_y[1] = 0.500;
	BaseP_y[2] = 0.700;
	BaseP_y[3] = 0.250;
	BaseP_y[4] = 0.000;
	BaseP_y[5] = 1.150;
	BaseP_y[6] = 1.330;
//	BaseP_y[7] = 1.700; 

//init End BP	
	for(i=0;i<(BP+ENDD);i++)
	{
		End_BaseP_x[i] = End_BaseP_y[i] = 0.0;
	}

	End_BaseP_x[0] = BaseP_x[0];
	End_BaseP_y[0] = BaseP_y[0];

	End_BaseP_x[BP+ENDD-1] = BaseP_x[BP-1];
	End_BaseP_y[BP+ENDD-1] = BaseP_y[BP-1];

	for(i=ENDD;i<(BP+ENDD)-2;i++)
	{
		End_BaseP_x[i] = BaseP_x[i-ENDD/2];
		End_BaseP_y[i] = BaseP_y[i-ENDD/2];
	}

	for(i=0;i<BP;i++)
	{
		fprintf(BPxy, "%lf %lf\n", BaseP_x[i], BaseP_y[i]);
	}

	*BP_dests = 0.0;
	for(i=1;i<BP;i++)
	{
		cacu_BP_dests(BaseP_x[i], BaseP_x[i-1], BaseP_y[i], BaseP_y[i-1], BP_dests);
	}
	
//	D0 = Dn = *BP_dests * 0.0;
	
	End_BaseP_x[ENDD/2] = 0.000;
	End_BaseP_y[ENDD/2] = 10.000;//D0;
	End_BaseP_x[BP+ENDD-2] = -10.000;
	End_BaseP_y[BP+ENDD-2] = 0.000;//Dn;

	for(i=0;i<(BP+ENDD);i++)
	{
//		printf("%lf %lf\n", End_BaseP_x[i], End_BaseP_y[i]);
	}

	for(i=0;i<BP;i++)
	{
		if(i==0)
		{
			BP_sn[i] = 0.0;
		}
		else
		{
			cacu_BP_sn(BP_dests, BP_sn[i-1], BaseP_x[i], BaseP_x[i-1], BaseP_y[i], BaseP_y[i-1], &BP_sn[i]);
		}
//		printf("%lf\n", BP_sn[i]);
	}
	
//	create_Nonuni_Knot_Ver(BP, ORD, BP_sn, Nonuni_U);
	create_End_Nonuni_Knot_Ver(BP, ORD, BP_sn, End_Nonuni_U);

	for(i=0;i<VER_ARR;i++)
	{
//		printf("%lf\n", Nonuni_U[i]);
	}

	for(i=0;i<(VER_ARR+ENDD);i++)
	{
//		printf("%lf\n", End_Nonuni_U[i]);
	}
/*
	for(i=0;i<BP;i++)
	{
		for(j=0;j<BP;j++)
		{
			BP_N[i][j] = 0.0;
			temp_BP_N[i][j] = 0.0;
		}
	}
	
	for(j=0;j<BP;j++) 
	{
		spn = FindSpan(BP-1, ORD, BP_sn[j], Nonuni_U);
		BasisFuns(spn, BP_sn[j], ORD, Nonuni_U, N); 
		for(i=0;i<=ORD;i++) 
		{
			BP_N[j][spn-ORD+i] = N[i];
		}
	}

	for(i=0;i<BP;i++)
	{
		for(j=0;j<BP;j++)
		{
			fprintf(test, "%lf ", BP_N[i][j]);
		}
		fprintf(test, "\n");
	}
*/

	for(i=0;i<(BP+ENDD);i++)
	{
		for(j=0;j<(BP+ENDD);j++)
		{
			BP_N[i][j] = 0.0;
			temp_BP_N[i][j] = 0.0;
		}
	}
	
	for(j=1;j<(BP+ENDD)-1;j++) 
	{
		spn = FindSpan((BP+ENDD)-1, ORD, BP_sn[j-1], End_Nonuni_U);
		BasisFuns(spn, BP_sn[j-1], ORD, End_Nonuni_U, N); 
		for(i=0;i<=ORD;i++) 
		{
			BP_N[j][spn-ORD+i] = N[i];
		}
	}
	
	for(j=1;j<(BP+ENDD)-1;j++)
	{
		if(j==(0+ENDD/2) || j==((BP+ENDD)-2))
		{
			spn = FindSpan((BP+ENDD)-1, ORD, BP_sn[j-1], End_Nonuni_U);
			DersBasisFuns(spn, BP_sn[j-1], ORD, 1, End_Nonuni_U, nders);
			for(i=0;i<=ORD;i++) 
			{
				BP_N[j][spn-ORD+i] = nders[1][i];
			}
		}
	}
	
	BP_N[0][0] = BP_N[BP+ENDD-1][BP+ENDD-1]= 1.0;
/*
	for(i=0;i<(BP+ENDD);i++)
	{
		for(j=0;j<(BP+ENDD);j++)
		{
			fprintf(test, "%lf ", BP_N[i][j]);
		}
		fprintf(test, "\n");
	}
*/

	ludcmp(BP_N, temp_BP_N, (BP+ENDD), pivot, &plusminusone);  

	for(i=0;i<(BP+ENDD);i++) 
	{
		rhs[i] = End_BaseP_x[i];
	}
	lubksb(temp_BP_N, (BP+ENDD), pivot, rhs);
	for(i=0;i<(BP+ENDD);i++) 
	{
		ksi[i] = rhs[i];
	}

	for(i=0;i<(BP+ENDD);i++) 
	{
		rhs[i] = End_BaseP_y[i];
	}
	lubksb(temp_BP_N, (BP+ENDD), pivot, rhs);
	for(i=0;i<(BP+ENDD);i++) 
	{
		eta[i] = rhs[i];
	}
	
	for(i=0;i<(VER+ENDD);i++)
	{
		fprintf(CP, "%lf %lf\n", ksi[i], eta[i]);
	}

	create_Uni_Knot_Ver((VER+ENDD), ORD, U);

	maxu = (double)((VER+ENDD) - ORD);
	delu = maxu / (double)(Num-1);
	
	for(i=0;i<Num;i++)
	{
		u[i] = (double)i * delu;
		CurvePoint((VER+ENDD)-1, ORD, U, ksi, u[i], &x[i]);
		CurvePoint((VER+ENDD)-1, ORD, U, eta, u[i], &y[i]);
		fprintf(Bspline, "%lf %lf\n", x[i], y[i]);
	}

	for(k=0;k<2;k++)
	{
		for(i=0;i<Num;i++)
		{
			xdev[k][i] = 0.0;
			ydev[k][i] = 0.0;
			spn = FindSpan((VER+ENDD)-1, ORD, u[i], U);
			DersBasisFuns(spn, u[i], ORD, k+1, U, nders);
			for(j=0;j<=ORD;j++)
			{
				xdev[k][i] += nders[k+1][j] * ksi[spn-ORD+j];
				ydev[k][i] += nders[k+1][j] * eta[spn-ORD+j];
			}
		}
	}

	for(i=0;i<Num;i++)
	{
		fprintf(Bsplineslope, "zone t=\"Bsplineslope\"\n");
		cacu_nx(xdev[0][i], ydev[0][i], &nx[i]);
		cacu_ny(xdev[0][i], ydev[0][i], &ny[i]);
		cacu_kslope(xdev[0][i], ydev[0][i], xdev[1][i], ydev[1][i], &kslope[i]);
		fprintf(Bsplineslope, "%lf %lf\n", x[i], y[i]);
		fprintf(Bsplineslope, "%lf %lf\n", x[i]+nx[i]*kslope[i]*rad, y[i]+ny[i]*kslope[i]*rad);
	}
/*	*/
	return 0;
}
main()
{
	FILE *Bspline=fopen("Bspline.dat","wt");
	FILE *Bsplineslope=fopen("Bsplineslope.dat","wt");

	FILE *CP=fopen("CP.dat","wt");
	FILE *BPxy=fopen("BPxy.dat","wt");
	
	FILE *test=fopen("test.dat","wt");

	int k, i, j, spn, *pivot;

	double *BaseP_x, *BaseP_y, angle, arc;
	double *BP_sn, *BP_dests, *BP_un, **BP_N, **temp_BP_N, *N, plusminusone;

	double *x, *y, *u, **xdev, **ydev;	
	double *ksi, *eta, *rhs;

	double *U, *Nonuni_U;
	double maxu, delu, **nders;
	double *nx, *ny, *kslope;

	BaseP_x = (double*)malloc(sizeof(double) * BP);
	BaseP_y = (double*)malloc(sizeof(double) * BP);
	BP_sn = (double*)malloc(sizeof(double) * BP);
	BP_dests = (double*)malloc(sizeof(double));
	BP_un = (double*)malloc(sizeof(double) * BP);
	
	N = (double*)malloc(sizeof(double) * (ORD+1));
	U = (double*)malloc(sizeof(double) * VER_ARR);
	Nonuni_U = (double*)malloc(sizeof(double) * VER_ARR);
	pivot = (int*)malloc(sizeof(int) * VER);

	rhs = (double*)malloc(sizeof(double) * VER);
	ksi = (double*)malloc(sizeof(double) * VER);
	eta = (double*)malloc(sizeof(double) * VER);
	x = (double*)malloc(sizeof(double) * Num);
	y = (double*)malloc(sizeof(double) * Num);
	u = (double*)malloc(sizeof(double) * Num);
	nx = (double*)malloc(sizeof(double) * Num);
	ny = (double*)malloc(sizeof(double) * Num);
	kslope = (double*)malloc(sizeof(double) * Num);

	nders = (double**)malloc(sizeof(double*) * (ORD+1));
	nders[0] = (double*)malloc(sizeof(double) * (ORD+1) * (ORD+1));
	for(i=1; i<(ORD+1); i++)
	{
		nders[i] = nders[i-1] + (ORD+1);
	}

	xdev = (double**)malloc(sizeof(double*) * 2);
	xdev[0] = (double*)malloc(sizeof(double) * 2 * Num);
	for(i=1; i<2; i++)
	{
		xdev[i] = xdev[i-1] + Num;
	}

	ydev = (double**)malloc(sizeof(double*) * 2);
	ydev[0] = (double*)malloc(sizeof(double) * 2 * Num);
	for(i=1; i<2; i++)
	{
		ydev[i] = ydev[i-1] + Num;
	}

	BP_N = (double**)malloc(sizeof(double*) * BP);
	BP_N[0] = (double*)malloc(sizeof(double) * BP * BP);
	for(i=1; i<BP; i++)
	{
		BP_N[i] = BP_N[i-1] + BP;
	}
	
	temp_BP_N = (double**)malloc(sizeof(double*) * BP);
	temp_BP_N[0] = (double*)malloc(sizeof(double) * BP * BP);
	for(i=1; i<BP; i++)
	{
		temp_BP_N[i] = temp_BP_N[i-1] + BP;
	}

	//...init datapoint	.... bow shape

	BaseP_x[0] = 0.000; 
	BaseP_x[1] = 0.000; 
	BaseP_x[2] = 0.000; 
	BaseP_x[3] = 0.000; 
	BaseP_x[4] = 1.000; 
	BaseP_x[5] = 1.600; 
	BaseP_x[6] = 2.000; 
	BaseP_x[7] = 2.000; 
	BaseP_x[8] = 2.000; 
	BaseP_x[9] = 2.300; 
	BaseP_x[10] = 2.700; 
	BaseP_x[11] = 4.500; 
	BaseP_x[12] = 5.000; 

	BaseP_y[0] = 0.000;
	BaseP_y[1] = 0.300;
	BaseP_y[2] = 2.000;
	BaseP_y[3] = 2.300;
	BaseP_y[4] = 3.000;
	BaseP_y[5] = 3.000;
	BaseP_y[6] = 2.600;
	BaseP_y[7] = 2.300;
	BaseP_y[8] = 2.000;
	BaseP_y[9] = 1.800;
	BaseP_y[10] = 1.800;
	BaseP_y[11] = 1.800;
	BaseP_y[12] = 1.800;

/*

	BaseP_x[0] = 0.000; 
	BaseP_x[1] = 0.250; 
	BaseP_x[2] = 1.500; 
	BaseP_x[3] = 2.500; 
	BaseP_x[4] = 4.000; 
	BaseP_x[5] = 4.500; 
	BaseP_x[6] = 4.000; 

	BaseP_y[0] = 0.000;
	BaseP_y[1] = 1.000;
	BaseP_y[2] = 1.500;
	BaseP_y[3] = 0.500;
	BaseP_y[4] = 0.150;
	BaseP_y[5] = 2.000;
	BaseP_y[6] = 2.100;
*//*
	BaseP_x[0] = 0.000; 
	BaseP_x[1] = 3.037; 
	BaseP_x[2] = 5.941; 
	BaseP_x[3] = 7.769; 
	BaseP_x[4] = 8.406; 
	BaseP_x[5] = 8.948; 
	BaseP_x[6] = 9.075; 
	BaseP_x[7] = 8.789; 
	BaseP_x[8] = 7.705; 
	BaseP_x[9] = 5.941; 
	BaseP_x[10] = 3.037; 
	BaseP_x[11] = 0.000; 
//	BaseP_x[12] = 0.000;  

	BaseP_y[0] = 1.252;
	BaseP_y[1] = 2.340;
	BaseP_y[2] = 4.206;
	BaseP_y[3] = 6.000;
	BaseP_y[4] = 7.000;
	BaseP_y[5] = 8.000;
	BaseP_y[6] = 9.000;
	BaseP_y[7] = 10.000;
	BaseP_y[8] = 11.000;
	BaseP_y[9] = 11.198;
	BaseP_y[10] = 11.516;
	BaseP_y[11] = 11.947;
//	BaseP_y[12] = 12.300;

	
// 
	BaseP_x[0] = -4.000;
	BaseP_x[1] = 0.000;  
	BaseP_x[2] = 3.037; 
	BaseP_x[3] = 5.941;  
	BaseP_x[4] = 7.769;
	BaseP_x[5] = 8.406; 
	BaseP_x[6] = 8.948;  
	BaseP_x[7] = 9.075;
	BaseP_x[8] = 8.789;   
	BaseP_x[9] = 7.705;
	BaseP_x[10] = 5.941;
	BaseP_x[11] = 3.037;
	BaseP_x[12] = 0.000; 
	BaseP_x[13] = 0.000; 
	BaseP_x[14] = 0.034; 
	BaseP_x[15] = 0.524;  
	BaseP_x[16] = 1.267;  
	BaseP_x[17] = 3.037;
	BaseP_x[18] = 5.941; 
	

	BaseP_y[0] = 0.000;
	BaseP_y[1] = 1.252;  
	BaseP_y[2] = 2.340; 
	BaseP_y[3] = 4.206;  
	BaseP_y[4] = 6.000;
	BaseP_y[5] = 7.000; 
	BaseP_y[6] = 8.000;  
	BaseP_y[7] = 9.000;
	BaseP_y[8] = 10.000;   
	BaseP_y[9] = 11.000;
	BaseP_y[10] = 11.198;
	BaseP_y[11] = 11.516;
	BaseP_y[12] = 11.947; 
	BaseP_y[13] = 12.300; 
	BaseP_y[14] = 13.000; 
	BaseP_y[15] = 14.500;  
	BaseP_y[16] = 16.000;  
	BaseP_y[17] = 18.640;
	BaseP_y[18] = 23.013;
*/


	for(i=0;i<BP;i++)
	{
		fprintf(BPxy, "%lf %lf\n", BaseP_x[i], BaseP_y[i]);
	}

	// cylinder shape
/*
	angle = 0.0;
	for(i=0;i<BP;i++)
	{
		ang_to_rad(&angle, &arc);
		BaseP_x[i] = 1.0 * cos(arc);									
		BaseP_y[i] = 1.0 * sin(arc);		
		angle += 360.0 / (BP-1);
		fprintf(BPxy, "%lf %lf\n", BaseP_x[i], BaseP_y[i]);
	}
*/	

	*BP_dests = 0.0;
	for(i=1;i<BP;i++)
	{
		cacu_BP_dests(BaseP_x[i], BaseP_x[i-1], BaseP_y[i], BaseP_y[i-1], BP_dests);
	}


	for(i=0;i<BP;i++)
	{
		if(i==0)
		{
			BP_sn[i] = 0.0;
		}
		else
		{
			cacu_BP_sn(BP_dests, BP_sn[i-1], BaseP_x[i], BaseP_x[i-1], BaseP_y[i], BaseP_y[i-1], &BP_sn[i]);
		}
//		printf("%lf\n", BP_sn[i]);
	}
	
	create_Nonuni_Knot_Ver(BP, ORD, BP_sn, Nonuni_U);

//	for(i=0;i<VER_ARR;i++)
//	{
//		printf("%lf\n", Nonuni_U[i]);
//	}

	for(i=0;i<BP;i++)
	{
		for(j=0;j<BP;j++)
		{
			BP_N[i][j] = 0.0;
			temp_BP_N[i][j] = 0.0;
		}
	}
	
	for(j=0;j<BP;j++) 
	{
		spn = FindSpan(BP-1, ORD, BP_sn[j], Nonuni_U);
		BasisFuns(spn, BP_sn[j], ORD, Nonuni_U, N); 
		for(i=0;i<=ORD;i++) 
		{
			BP_N[j][spn-ORD+i] = N[i];
		}
	}

//	for(i=0;i<BP;i++)
//	{
//		for(j=0;j<BP;j++)
//		{
//			fprintf(test, "%lf ", BP_N[i][j]);
//		}
//		fprintf(test, "\n");
//	}


	ludcmp(BP_N, temp_BP_N, BP, pivot, &plusminusone);  
	
	for(i = 0; i < BP; i++) 
	{
		rhs[i] = BaseP_x[i];
	}
	lubksb(temp_BP_N, BP, pivot, rhs);
	for(i=0;i<BP;i++) 
	{
		ksi[i] = rhs[i];
	}

	for(i = 0; i < BP; i++) 
	{
		rhs[i] = BaseP_y[i];
	}
	lubksb(temp_BP_N, BP, pivot, rhs);
	for(i=0;i<BP;i++) 
	{
		eta[i] = rhs[i];
	}
	
	for(i=0;i<VER;i++)
	{
		fprintf(CP, "%lf %lf\n", ksi[i], eta[i]);
	}

	create_Uni_Knot_Ver(VER, ORD, U);

	maxu = (double)(VER - ORD);
	delu = maxu / (double)(Num-1);
	
	for(i=0;i<Num;i++)
	{
		u[i] = (double)i * delu;
		CurvePoint(VER-1, ORD, U, ksi, u[i], &x[i]);
		CurvePoint(VER-1, ORD, U, eta, u[i], &y[i]);
		fprintf(Bspline, "%lf %lf\n", x[i], y[i]);
	}

	for(k=0;k<2;k++)
	{
		for(i=0;i<Num;i++)
		{
			xdev[k][i] = 0.0;
			ydev[k][i] = 0.0;
			spn = FindSpan(VER-1, ORD, u[i], U);
			DersBasisFuns(spn, u[i], ORD, k+1, U, nders);
			for(j=0;j<=ORD;j++)
			{
				xdev[k][i] += nders[k+1][j] * ksi[spn-ORD+j];
				ydev[k][i] += nders[k+1][j] * eta[spn-ORD+j];
			}
		}
	}

	for(i=0;i<Num;i++)
	{
		fprintf(Bsplineslope, "zone t=\"Bsplineslope\"\n");
		cacu_nx(xdev[0][i], ydev[0][i], &nx[i]);
		cacu_ny(xdev[0][i], ydev[0][i], &ny[i]);
		cacu_kslope(xdev[0][i], ydev[0][i], xdev[1][i], ydev[1][i], &kslope[i]);
		fprintf(Bsplineslope, "%lf %lf\n", x[i], y[i]);
		fprintf(Bsplineslope, "%lf %lf\n", x[i]+nx[i]*kslope[i]*rad, y[i]+ny[i]*kslope[i]*rad);
	}
/*	*/
	return 0;
}
Пример #7
0
static void NURBSsurfaceEval(int degU, int degV, double *cp, int ncp, int kcp, double *knotU, double *knotV, double *us, int nus, double *ep, double *leftU, double *rightU, double *NU, double *leftV, double *rightV, double *NV){
    /* Modification of  ALGORITHM A4.3, The NURBS Book, L.Piegl and W. Tiller */
    
    /* Evaluates a NURBS surface at given parameter values */
    
    /* NURBSsurfaceEval( degU - degree of NURBS in u, degV - degree of NURBS in v, cp - pointer to control points, ncp - number of control points in u, kcp - number of control points in v, knotU - pointer to knot sequence in u, knotV - pointer to knot sequence in v, us - pointer to parameter values,  nus - number of parameter values, ep - pointer to evaluated points, leftU - pointer to array for function BasisFuns, rightU - pointer to array for function BasisFuns, NU - pointer to array for function BasisFuns, leftV - pointer to array for function BasisFuns, rightV - pointer to array for function BasisFuns, NV - pointer to array for function BasisFuns) */
    
    int i, j, jj, spanU, spanV, ind, ind2, fourncp, threejj, myint;
    double wgh, NN;
    
    fourncp=4*ncp;
    
    for (jj = 0; jj < nus; jj++){
        
        threejj=3*jj;
        
        if(us[2*jj]<=knotU[degU]){
            
            if(us[2*jj+1]<=knotV[degV]){
                ep[threejj] = cp[0]/cp[3];
                ep[threejj+1] = cp[1]/cp[3];
                ep[threejj+2] = cp[2]/cp[3];
            }
            else if(us[2*jj+1]>=knotV[kcp]){
                myint=fourncp*(kcp-1);
                ep[threejj] = cp[myint]/cp[myint+3];
                ep[threejj+1] = cp[myint+1]/cp[myint+3];
                ep[threejj+2] = cp[myint+2]/cp[myint+3];
            }
            else{
                ep[threejj]=0.0;
                ep[threejj+1]=0.0;
                ep[threejj+2]=0.0;
                wgh=0.0;
                
                spanV = FindSpan(kcp, degV, us[2*jj+1], knotV);
                BasisFuns(spanV, us[2*jj+1], degV, knotV, NV, leftV, rightV);
                
                ind = spanV - degV;
                
                for (i = 0; i <= degV; i++){
                    myint=(ind+i)*fourncp;
                    ep[threejj] += NV[i] * cp[myint];
                    ep[threejj+1] += NV[i] * cp[myint+1];
                    ep[threejj+2] += NV[i] * cp[myint+2];
                    wgh += NV[i] * cp[myint+3];
                }
                ep[threejj]=ep[threejj]/wgh;
                ep[threejj+1]=ep[threejj+1]/wgh;
                ep[threejj+2]=ep[threejj+2]/wgh;
            }
            
        }
        else if(us[2*jj]>=knotU[ncp]){
            
            if(us[2*jj+1]<=knotV[degV]){
                ep[threejj] = cp[fourncp-4]/cp[fourncp-1];
                ep[threejj+1] = cp[fourncp-3]/cp[fourncp-1];
                ep[threejj+2] = cp[fourncp-2]/cp[fourncp-1];
            }
            else if(us[2*jj+1]>=knotV[kcp]){
                myint=fourncp*kcp;
                ep[threejj] = cp[myint-4]/cp[myint-1];
                ep[threejj+1] = cp[myint-3]/cp[myint-1];
                ep[threejj+2] = cp[myint-2]/cp[myint-1];
            }
            else{
                ep[threejj]=0.0;
                ep[threejj+1]=0.0;
                ep[threejj+2]=0.0;
                wgh=0.0;
                
                spanV = FindSpan(kcp, degV, us[2*jj+1], knotV);
                BasisFuns(spanV, us[2*jj+1], degV, knotV, NV, leftV, rightV);
                
                ind = spanV - degV;
                myint=(ind+1)*fourncp;
                
                for (i = 0; i <= degV; i++){
                    ep[threejj] += NV[i] * cp[i*fourncp+myint-4];
                    ep[threejj+1] += NV[i] * cp[i*fourncp+myint-3];
                    ep[threejj+2] += NV[i] * cp[i*fourncp+myint-2];
                    wgh += NV[i] * cp[i*fourncp+myint-1];
                }
                ep[threejj]=ep[threejj]/wgh;
                ep[threejj+1]=ep[threejj+1]/wgh;
                ep[threejj+2]=ep[threejj+2]/wgh;
            }
            
        }
        else{
            
            ep[threejj]=0.0;
            ep[threejj+1]=0.0;
            ep[threejj+2]=0.0;
            wgh=0.0;
            
            if(us[2*jj+1]<=knotV[degV]){
                spanU = FindSpan(ncp, degU, us[2*jj], knotU);
                BasisFuns(spanU, us[2*jj], degU, knotU, NU, leftU, rightU);
                
                ind = spanU - degU;
                
                for (i = 0; i <= degU; i++){
                    ep[threejj] += NU[i] * cp[(ind+i)*4];
                    ep[threejj+1] += NU[i] * cp[(ind+i)*4+1];
                    ep[threejj+2] += NU[i] * cp[(ind+i)*4+2];
                    wgh += NU[i] * cp[(ind+i)*4+3];
                }
                ep[threejj]=ep[threejj]/wgh;
                ep[threejj+1]=ep[threejj+1]/wgh;
                ep[threejj+2]=ep[threejj+2]/wgh;
            }
            else if(us[2*jj+1]>=knotV[kcp]){
                spanU = FindSpan(ncp, degU, us[2*jj], knotU);
                BasisFuns(spanU, us[2*jj], degU, knotU, NU, leftU, rightU);
                
                ind = spanU - degU;
                myint=ind*4+(kcp-1)*fourncp;
                
                for (i = 0; i <= degU; i++){
                    ep[threejj] += NU[i] * cp[i*4+myint];
                    ep[threejj+1] += NU[i] * cp[i*4+myint+1];
                    ep[threejj+2] += NU[i] * cp[i*4+myint+2];
                    wgh += NU[i] * cp[i*4+myint+3];
                }
                ep[threejj]=ep[threejj]/wgh;
                ep[threejj+1]=ep[threejj+1]/wgh;
                ep[threejj+2]=ep[threejj+2]/wgh;
            }
            else{
                spanU = FindSpan(ncp, degU, us[2*jj], knotU);
                BasisFuns(spanU, us[2*jj], degU, knotU, NU, leftU, rightU);
                
                spanV = FindSpan(kcp, degV, us[2*jj+1], knotV);
                BasisFuns(spanV, us[2*jj+1], degV, knotV, NV, leftV, rightV);
                
                ind = spanU - degU;
                ind2 = spanV - degV;
                myint=ind2*fourncp+ind*4;
                
                for (i = 0; i <= degV; i++){
                    for (j = 0; j <= degU; j++){
                        NN=NV[i] * NU[j];
                        ep[threejj] += NN * cp[i*fourncp+j*4+myint];
                        ep[threejj+1] += NN * cp[i*fourncp+j*4+myint+1];
                        ep[threejj+2] += NN * cp[i*fourncp+j*4+myint+2];
                        wgh += NN * cp[i*fourncp+j*4+myint+3];
                    }
                }
                ep[threejj]=ep[threejj]/wgh;
                ep[threejj+1]=ep[threejj+1]/wgh;
                ep[threejj+2]=ep[threejj+2]/wgh;
            }
            
        }
        
    }
    
}
void main(void) {

  int nv, p, i;
  const double xmax = 180.0;
  ofstream res("res.out");
  car2d xy;

  p = 3;
  nv = 7;
  xy.setdimsize(nv, 2);

  double delx = xmax / double(nv-1);

  for(i = 0; i < nv; i++) {
    xy[i][0] = double(i)*delx;
    xy[i][1] = sin(toradian(xy[i][0]));
  }
/*
	xy[0][0] = -4.000;
	xy[1][0] = 0.000;  
	xy[2][0] = 3.037; 
	xy[3][0] = 5.941;  
	xy[4][0] = 7.769;
	xy[5][0] = 8.406; 
	xy[6][0] = 8.948;  
	xy[7][0] = 9.075;
	xy[8][0] = 8.789;   
	xy[9][0] = 7.705;
	xy[10][0] = 5.941;
	xy[11][0] = 3.037;
	xy[12][0] = 0.000; 
	xy[13][0] = 0.000; 
	xy[14][0] = 0.034; 
	xy[15][0] = 0.524;  
	xy[16][0] = 1.267;  
	xy[17][0] = 3.037;
	xy[18][0] = 5.941; 
	
	xy[0][1] = 0.000;
	xy[1][1] = 1.252;  
	xy[2][1] = 2.340; 
	xy[3][1] = 4.206;  
	xy[4][1] = 6.000;
	xy[5][1] = 7.000; 
	xy[6][1] = 8.000;  
	xy[7][1] = 9.000;
	xy[8][1] = 10.000;   
	xy[9][1] = 11.000;
	xy[10][1] = 11.198;
	xy[11][1] = 11.516;
	xy[12][1] = 11.947; 
	xy[13][1] = 12.300; 
	xy[14][1] = 13.000; 
	xy[15][1] = 14.500;  
	xy[16][1] = 16.000;  
	xy[17][1] = 18.640;
	xy[18][1] = 23.013;
*/
  res << "\nXY data\n" << xy << endl;
	
  // polygonal arc length
  cvecd s(nv);
  s[0] = 0.0;
  for(int j = 1; j < nv; j++) {
    s[j] = s[j-1] + sqrt(square(xy[j][0] - xy[j-1][0]) + 
                         square(xy[j][1] - xy[j-1][1]));

  }
  res << "\nPolygonal arc length\n"
      << s << endl;

  cvecd u(nv);
  for(j = 0; j < nv; j++) {
    u[j] = ( double(nv) - double(p)) / s[nv-1] * s[j];
}
  res << "parameter u\n" << u << endl;

  cvecd U(nv + p + 1);
  ComputeUniformKnotVector(nv-1, p, U);
  for(i=0;i<nv+p+1;i++)
  {
}
  res << "\nKnot vector\n" << U << endl;

  car2d A(nv, nv);
  cvecd N(p+1);
  for(j = 0; j < nv; j++) {
    int span = FindSpan(nv-1, p, u[j], U);
    BasisFuns(span, u[j], p, U, N); 
    for(int a=0; a<=p; a++) {
      A[j][span-p+a] = N[a];
   }
  }


  res << "\nCoefficient matrix\n" << A << endl;

  car2d ALUD(nv, nv);
  car2d P(nv, 2);
  cvecd rhs(nv);
  cveci pivot(nv);
  double plusminusone;
  // Xi  *P = Y(ui)
  // Etai*P = Y(ui) 
  //..solve simultaneous equation by LU Decomposition..
  ludcmp(A, ALUD, nv, pivot, plusminusone);  


  for(j = 0; j < 2; j++) {
    for(i = 0; i < nv; i++) rhs[i] = xy[i][j];
    lubksb(ALUD, nv, pivot, rhs);
    for(i = 0; i < nv; i++) P[i][j] = rhs[i];
  }

  res << "\nGCV\n" << P << endl;
  res.close();

  ofstream fig60("fig60.dat");
  fig60.setf(ios::showpoint | ios::right | ios::fixed);
  fig60.precision(6);
  fig60 << "variables = x, y\n"
        << "zone t = \"Input Points\", i = " << nv << endl;
  for(j = 0; j < nv; j++) 
    fig60 << setw(10) << xy[j][0] << ' '
          << setw(10) << xy[j][1] << endl;

  fig60 << "zone t = \"B-spline Control Polygon\", i = " << nv << endl;
  for(j = 0; j < nv; j++) 
    fig60 << setw(10) << P[j][0] << ' '
          << setw(10) << P[j][1] << endl;


  int nuout = 50;
  cvecd uout(nuout), point(2);
  double maxu = double(nv - p);
  double delu = maxu / double(nuout-1);
//  printf("%lf, %lf\n", maxu, double(nuout-1));
  for(i = 0; i < nuout; i++)
  {
   uout[i] = double(i) * delu;
 }

  fig60 << "zone t = \"Evaluated B-spline\", i = " << nuout << endl;
  for(i = 0; i < nuout; i++) {
    CurvePoint1P (
      nv-1, 
      p, 
      U, 
      P, 
      uout[i], 
      point, 
      2
    );
    fig60 << setw(10) << point[0] << ' '
          << setw(10) << point[1] << endl;
  }
}