/*
Allocate memory for out before call
*/
void b_spline_function::Evaluate_W(double u, double* out)
{
std::vector<double> coeff;
auto span = FindSpan(m_P_degree, u, m_knot_vector);
BasisFunctions(span, u, m_P_degree, m_knot_vector, coeff);
double* Cw = new double(3);
memset(Cw, 0, 3 * sizeof(double));
memset(out, 0, 3 * sizeof(double));
double w = 0.0;
for (size_t i = 0; i <= m_P_degree; i++)
{
for (int j = 0; j < 3; j++)
{
// n-1 basic functions
Cw[j] = Cw[j] + coeff[i] * m_Control_Points[span - m_P_degree + i][j] * m_Weight[span - m_P_degree + i];
//std::cout << i << " " << j <<" "<< span << std::endl;
}
w = w + coeff[i] * m_Weight[span - m_P_degree + i];
}
for (int j = 0; j < 3; j++)
{
out[j] = Cw[j]/w;
}
}
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];
}
}
void b_spline_function::Evaluate(double u ,double * coor)
{
std::vector<double> coeff;
auto span = FindSpan(m_P_degree,u,m_knot_vector);
BasisFunctions(span,u,m_P_degree,m_knot_vector,coeff);
memset(coor,0,3*sizeof(double));
for (size_t i = 0; i <= m_P_degree; i++)
{
for (int j = 0; j < 3; j++)
{
// n-1 basic functions
coor[j] = coor[j] + coeff[i] * m_Control_Points[span - m_P_degree + i ][j];
//std::cout << i << " " << j <<" "<< span << std::endl;
}
}
}
int main(int arg, char* argv[])
{
int p = 2; // our basis order
double U[] = {0.0, 0.0, 0.0, 1.0, 2.0, 3.0, 4.0, 5.0,5.0, 5.0}; // knot vector
int m = sizeof(U)/sizeof(double) - 1; // number of knot points
int n = m - p - 1; // number of basis functions
double u;
int c, span;
/*printf("Number of knot points: %d\n", m);
//
// Test our knot span index code
//
for(c = 0; c < m+1; ++c)
{
span = FindSpan(n, p, U[c], U);
printf("For Xi=%2.1f, the knot span index is i=%d\n\n",U[c], span);
}*/
//
// Now let's test our routines which calculate the basis functions and derivatives
//
int numNonZeroBsFns = p + 1;
/*
double *N = (double*)malloc(sizeof(double) * numNonZeroBsFns);
double **ders = init2DArray(n+1, p+1);
for(c = 0; c < numNonZeroBsFns; ++c) N[c] = 0.0;
double point[] = {0.0, 1.0, 2,0, 3.0, 4.0, 5.0}; // some sample points to evaluate at
int k = sizeof(point) / sizeof(double); // number of points (for our loop)
int j,d;
for(j = 0; j < k; ++j)
{
span = FindSpan(n, p, point[j], U); // get the span
BasisFuns( span, point[j], p, U, N); // get the non-zero basis functions
dersBasisFuns(span, point[j], p, n, U, ders); // get the derivatives ders[k][j]
// print out basis functions
printf("The non-zero basis functions in the range %2.2f and %2.2f and u=%2.2f are:\n",U[span], U[span+1], point[j]);
for(c = 0; c < numNonZeroBsFns; ++c) printf("\n %2.2f\n", N[c]);
// print out derivatives
for(d = 0; d < (n+1); d++)
{
printf("For k=%d, derivatives are: \n", d);
for(c = 0; c < numNonZeroBsFns; c++) printf("%2.2f\t", ders[d][c]);
printf("\n");
}
printf("\n\n");
}
free(N);
free2Darray(ders, n+1);
*/
double *dN = (double*)malloc(sizeof(double) * (n+1));
double **ders = init2DArray(n+1, p+1);
double xi, Nip;
int i=2;
printf("xi\tN\tdNdxi\n");
for(xi=0.0; xi <= 5.0; xi+=0.1)
{
span = FindSpan(n, p, xi, U);
Nip = OneBasisFun(p, m, U, i, xi);
dersOneBasisFuns(p, m, U, i, xi, n, dN);
}
xi = 2.1;
span = FindSpan(n, p, xi, U);
dersBasisFuns(span, xi, p, n, U, ders); // get the derivatives ders[k][j]
printf("\n\n%2.2f\t%2.20f\t%2.20f\t%2.20f\n",xi, ders[1][0], ders[1][1], ders[1][2]);
free(dN);
free2Darray(ders, n+1);
return 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;
}
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;
}
}
