dFloat64 dBezierSpline::CalculateLength (dFloat64 tol) const
{
dBigVector stackPool[32][3];
int stack = 0;
dFloat64 length = 0.0f;
dFloat64 tol2 = tol * tol;
dFloat64 u0 = m_knotVector[m_degree];
dBigVector p0 (CurvePoint (u0));
for (int i = m_degree; i < (m_knotsCount - m_degree - 1); i ++) {
dFloat64 u1 = m_knotVector[i + 1];
dBigVector p1 (CurvePoint (u1));
stackPool[stack][0] = p0;
stackPool[stack][1] = p1;
stackPool[stack][2] = dBigVector (u0, u1, dFloat64(0.0f), dFloat64(0.0f));
stack ++;
while (stack) {
stack --;
dBigVector q0 (stackPool[stack][0]);
dBigVector q1 (stackPool[stack][1]);
dFloat64 t0 = stackPool[stack][2][0];
dFloat64 t1 = stackPool[stack][2][1];
dFloat64 t01 = (t1 + t0) * 0.5f;
dBigVector p01 ((q1 + q0).Scale (0.5f));
dBigVector q01 (CurvePoint (t01));
dBigVector err (q01 - p01);
dFloat64 err2 = err.DotProduct3(err);
if (err2 < tol2) {
dBigVector step (q1 - q0);
length += dSqrt (step.DotProduct3(step));
} else {
stackPool[stack][0] = q01;
stackPool[stack][1] = q1;
stackPool[stack][2] = dBigVector (t01, t1, dFloat64(0.0f), dFloat64(0.0f));
stack ++;
stackPool[stack][0] = q0;
stackPool[stack][1] = q01;
stackPool[stack][2] = dBigVector (t0, t01, dFloat64(0.0f), dFloat64(0.0f));
stack ++;
}
}
u0 = u1;
p0 = p1;
}
return length;
}
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;
}
dFloat64 dBezierSpline::FindClosestKnot (dBigVector& closestPoint, const dBigVector& point, int subdivitionSteps) const
{
int startSpan = m_degree;
dFloat64 bestU = 0.0f;
dFloat64 distance2 = 1.0e10f;
dBigVector closestControlPoint (m_controlPoints[0]);
subdivitionSteps = dMax (subdivitionSteps, 1);
dFloat64 scale = 1.0f / subdivitionSteps;
for (int span = m_degree; span < (m_knotsCount - m_degree - 1); span ++) {
dFloat64 param = 0.0f;
for (int i = 0; i < subdivitionSteps; i ++) {
dFloat64 u = m_knotVector[span] + (m_knotVector[span + 1] - m_knotVector[span]) * param;
param += scale;
dBigVector p (CurvePoint (u, span));
dBigVector dp (p - point);
dFloat64 dist2 = dp.DotProduct3(dp);
if (dist2 < distance2) {
bestU = u;
startSpan = span;
distance2 = dist2;
closestControlPoint = p;
}
}
}
dBigVector derivatives[32];
dFloat64 u0 = bestU;
bool stop = false;
for (int i = 0; (i < 20) && !stop; i ++) {
CurveAllDerivatives (u0, derivatives);
dBigVector dist (closestControlPoint - point);
dFloat64 num = derivatives[1].DotProduct3(dist);
dFloat64 den = derivatives[2].DotProduct3(dist) + derivatives[1].DotProduct3(derivatives[1]);
// if (dAbs (den) < 1.0e-6f)
// __debugbreak();
dFloat64 u1 = dClamp(u0 - num / den, dFloat64(0.0), dFloat64 (1.0));
if (u1 < m_knotVector[startSpan]) {
startSpan --;
dAssert (startSpan >= 0);
} else if (u1 >= m_knotVector[startSpan + 1]) {
startSpan ++;
dAssert (startSpan < (m_knotsCount - m_degree));
}
dAssert (startSpan >= m_degree);
dAssert (startSpan <= (m_knotsCount - m_degree - 1));
closestControlPoint = CurvePoint (u1, startSpan);
//dFloat64 xxx0 = num * num;
//dFloat64 xxx1 = dist % dist;
//dFloat64 xxx2 = derivatives[1] % derivatives[1];
//dFloat64 xxx3 = xxx1 * xxx2 * 1.0e-10;
stop |= (dAbs (u1 - u0) < 1.0e-10) || (num * num < ((dist.DotProduct3(dist)) * (derivatives[1].DotProduct3(derivatives[1])) * 1.0e-10));
u0 = u1;
}
closestPoint = closestControlPoint;
return u0;
}
dBigVector dBezierSpline::CurvePoint (dFloat64 u) const
{
u = dClamp (u, dFloat64 (0.0f), dFloat64 (1.0f));
int span = GetSpan(u);
return CurvePoint (u, span);
}