#define MAXDEPTH 20 // stacksize is a function of maxdepth but array sizes are integers only, not expressions :-( #define STACKSIZE 10000 // e=ldexp(1,-maxdepth) #define EPSILON exp2(-MAXDEPTH) // undefine 2D macro to make all calculations 3D #define CALC2D void ConvertToBezierForm(point p, point V[4], point w[6]) { int i, j, k, m, n, ub, lb; int row, column; // Table indices vector c[4]; // V(i)'s - P vector d[3]; // V(i+1) - V(i) float cdTable[12]; // Dot product of c, d float z[12] = { // Precomputed "z" for cubics 1.0, 0.6, 0.3, 0.1, 0.4, 0.6, 0.6, 0.4, 0.1, 0.3, 0.6, 1.0 }; //Determine the c's -- these are vectors created by subtracting // point P from each of the control points for (i = 0; i <= 3; i++) { #ifdef CALC2D c[i][0]=V[i][0]-p[0]; c[i][1]=V[i][1]-p[1]; #else c[i]=V[i]-p; #endif } // Determine the d's -- these are vectors created by subtracting // each control point from the next for (i = 0; i <= 3 - 1; i++) { #ifdef CALC2D d[i][0]=3*(V[i+1][0]-V[i][0]); d[i][1]=3*(V[i+1][1]-V[i][1]); #else d[i]=3*(V[i+1]-V[i]); #endif } // Create the c,d table -- this is a table of dot products of the // c's and d's for (row = 0; row <= 3 - 1; row++) { for (column = 0; column <= 3; column++) { #ifdef CALC2D cdTable[row*4+column] = d[row][0]*c[column][0]+d[row][1]*c[column][1]; #else cdTable[row*4+column] = dot(d[row], c[column]); #endif } } // Now, apply the z's to the dot products, on the skew diagonal // Also, set up the x-values, making these "points" for (i = 0; i <= 5; i++) { w[i][1] = 0.0; w[i][0] = (float)(i) / 5; } n = 3; m = 2; for (k = 0; k <= n + m; k++) { lb = max(0, k - m); ub = min(k, n); for (i = lb; i <= ub; i++) { j = k - i; w[i+j][1] += cdTable[j*4+i] * z[j*4+i]; } } } int CrossingCount(point w[], int wtop) { int i; int n_crossings = 0; // Number of zero-crossings int sign, old_sign; // Sign of coefficients sign = old_sign = (w[wtop][1] >= 0); for (i = 1; i <= 5; i++) { sign = (w[wtop+i][1] >= 0); if (sign != old_sign) n_crossings++; old_sign = sign; } return n_crossings; } float ComputeXIntercept(point w[], int wtop) { float XLK, YLK, XNM, YNM, XMK, YMK; float det, detInv; float S, X; XLK = 1.0; //YLK = 0.0; XNM = w[wtop+5][0] - w[wtop][0]; YNM = w[wtop+5][1] - w[wtop][1]; XMK = w[wtop][0]; YMK = w[wtop][1]; //det = XNM*YLK - YNM*XLK; //detInv = 1.0/det; //S = (XNM*YMK - YNM*XMK) * detInv; //X = XLK * S; return (XNM*YMK - YNM*XMK) / - YNM; } int ControlPolygonFlatEnough(point w[], int wtop) { int i; // Index variable float value; float max_distance_above; float max_distance_below; float error; // Precision of root float intercept_1, intercept_2, left_intercept, right_intercept; float a, b, c; // Coefficients of implicit // eqn for line from V[0]-V[deg] float det, dInv; float a1, b1, c1, a2, b2, c2; // Derive the implicit equation for line connecting first *' // and last control points a = w[wtop][1] - w[wtop+5][1]; b = w[wtop+5][0] - w[wtop][0]; c = w[wtop][0] * w[wtop+5][1] - w[wtop+5][0] * w[wtop][1]; max_distance_above = max_distance_below = 0.0; for (i = 1; i < 5; i++) { value = a * w[wtop+i][0] + b * w[wtop+i][1] + c; if (value > max_distance_above) { max_distance_above = value; } else if (value < max_distance_below) { max_distance_below = value; } } // Implicit equation for zero line // a1 = 0.0; //b1 = 1.0; //c1 = 0.0; // Implicit equation for "above" line a2 = a; b2 = b; c2 = c - max_distance_above; //det = a1 * b2 - a2 * b1; //dInv = 1.0/det; //intercept_1 = (b1 * c2 - b2 * c1) * dInv; intercept_1 = ( c2 ) / (-a2); // Implicit equation for "below" line a2 = a; b2 = b; c2 = c - max_distance_below; //det = a1 * b2 - a2 * b1; //dInv = 1.0/det; //intercept_2 = (b1 * c2 - b2 * c1) * dInv; intercept_2 = ( c2 ) / -a2; // Compute intercepts of bounding box left_intercept = min(intercept_1, intercept_2); right_intercept = max(intercept_1, intercept_2); error = right_intercept - left_intercept; return (error < EPSILON)? 1 : 0; } point Bezier(point V[], int degree, float t) { int i, j; // Index variables point Vtemp[36]; // Copy control points for (j =0; j <= degree; j++) { #ifdef CALC2D Vtemp[j][0] = V[j][0]; Vtemp[j][1] = V[j][1]; #else Vtemp[j] = V[j]; #endif } // Triangle computation for (i = 1; i <= degree; i++) { for (j =0 ; j <= degree - i; j++) { Vtemp[i*6+j][0] = (1.0 - t) * Vtemp[6*(i-1)+j][0] + t * Vtemp[6*(i-1)+j+1][0]; Vtemp[i*6+j][1] = (1.0 - t) * Vtemp[6*(i-1)+j][1] + t * Vtemp[6*(i-1)+j+1][1]; #ifndef CALC2D Vtemp[i*6+j][2] = (1.0 - t) * Vtemp[6*(i-1)+j][2] + t * Vtemp[6*(i-1)+j+1][2]; #endif } } return (Vtemp[6*degree]); } // this one is used for the 5th order interpolation only so should always be just 2d void DeCasteljau(point w[], float t, int wtop, int wend) { int i, j; // Index variables point Vtemp[36]; // Copy control points for (j =0; j <= 5; j++) { #ifdef CALC2D Vtemp[j][0] = w[wtop+j][0]; Vtemp[j][1] = w[wtop+j][1]; #else Vtemp[j] = w[wtop+j]; #endif } // Triangle computation int d=0; for (i = 1; i <= 5; i++) { for (j =0 ; j <= 5 - i; j++) { Vtemp[i*6+j][0] = (1.0 - t) * Vtemp[d+j][0] + t * Vtemp[d+j+1][0]; Vtemp[i*6+j][1] = (1.0 - t) * Vtemp[d+j][1] + t * Vtemp[d+j+1][1]; } d+=6; } for (j = 0; j <= 5; j++) { w[wend+j] = Vtemp[j*6]; } for (j = 0; j <= 5; j++) { w[wend+6+j] = Vtemp[6*(5-j)+j]; } } #define POP wtop+=6;wn-- int FindRoots(point w0[6], float t[5]){ point w[STACKSIZE]; int wtop=0; int wend=0; int wn=0; for(int k=0;k<6;k++){w[wend++]=w0[k] ;} wn++; int nc=0; while(wn>0){ //printf("[%d]",wn); int cc=CrossingCount(w,wtop); // w[:5] if(cc==0){ POP; continue; } if(cc==1){ if(wn>=MAXDEPTH){ t[nc++]= (w[wtop][0]+w[wtop+5][0])/2; //printf(" MAX "); POP; continue; } if(ControlPolygonFlatEnough(w,wtop)){ // w[:5] t[nc++]= ComputeXIntercept(w,wtop); //printf(" FLAT (%.1f | %.1f)",w[wtop],w[wtop+5]); POP; continue; } } // not flat or more than one crossing DeCasteljau(w,0.5,wtop,wend); //left=[wend:wend+5], right=[wend+6;wend+11] for(int k=0;k<6;k++){w[wtop+k]=w[wend+6+k];}; wn++; wend+=6; } return nc; } // note that might gain something by directly comparing to width and breaking out instead of trying to find the nearest point so this adaption from NearestPoint does just that int CloseEnough(point p, point V[4], float d, float Width2){ // Convert problem to 5th-degree Bezier form point w0[6]; ConvertToBezierForm(p, V, w0); //Find all possible roots of 5th-degree equation float t[5],dt; int n_solutions = FindRoots(w0,t); //printf("%d %.2f\n",n_solutions,t[0]); // check which root is closest, also check end points #ifdef CALC2D vector dv=vector(V[0][0]-p[0],V[0][1]-p[1],0); d=dv[0]*dv[0]+dv[1]*dv[1]; if(d < Width2) return 1; for(int n=0; n