static element ComputeMaxError(vector[] d, int first, int last, cubicbezier bezCurve, element[] u, out int splitPoint) { var sp = (last - first + 1) / 2; element maxDist2 = 0; for (var i = first + 1; i < last; i++) { var dist2 = (bezCurve.Interpolate(u[i - first]) - d[i]).LengthSquare; if (maxDist2 <= dist2) { maxDist2 = dist2; sp = i; } } splitPoint = sp; return(maxDist2); }
/// <summary> /// Use Newton-Raphson iteration to find better root. /// </summary> /// <param name="Q">Current fitted curve</param> /// <param name="P">Digitized point</param> /// <param name="u">Parameter value for <see cref="P"/></param> /// <returns>パラメータ</returns> static element NewtonRaphsonRootFind(cubicbezier Q, vector P, element u) { /* Compute Q(u) */ var Q_u = Q.Interpolate(u); /* Generate control vertices for Q' */ var Q1 = new vector[3]; /* Q' and Q'' */ for (int i = 0; i <= 2; i++) { Q1[i] = (Q[i + 1] - Q[i]) * 3; } /* Generate control vertices for Q'' */ var Q2 = new vector[2]; for (int i = 0; i <= 1; i++) { Q2[i] = (Q1[i + 1] - Q1[i]) * 2; } /* Compute Q'(u) and Q''(u) */ var Q1_u = Interpolate2(u, Q1[0], Q1[1], Q1[2]); var Q2_u = Interpolate1(u, Q2[0], Q2[1]); /* Compute f(u)/f'(u) */ var Q_u_P = Q_u - P; var numerator = Q_u_P.Dot(Q1_u); var denominator = Q1_u.LengthSquare + Q_u_P.Dot(Q2_u); if (denominator == 0) { return(u); } /* u = u - f(u)/f'(u) */ return(u - numerator / denominator); }
/// <summary> /// 最小二乗法を用いて指定範囲のベジェコントロールポイントを探す /// </summary> /// <param name="d">頂点列</param> /// <param name="first">範囲開始インデックス</param> /// <param name="last">範囲終了インデックス</param> /// <param name="uPrime">指定範囲内のパラメータ</param> /// <param name="tHat1">範囲開始部分のベクトル</param> /// <param name="tHat2">範囲終了部分のベクトル</param> /// <returns>3次ベジェ曲線</returns> static cubicbezier GenerateBezier(vector[] d, int first, int last, element[] uPrime, vector tHat1, vector tHat2) { var nPts = last - first + 1; var tHat1LenDiv = tHat1.Length; var tHat2LenDiv = tHat2.Length; if (tHat1LenDiv != 0) { tHat1LenDiv = 1 / tHat1LenDiv; } if (tHat2LenDiv != 0) { tHat2LenDiv = 1 / tHat2LenDiv; } /* Compute the A's */ var A = new vector[nPts, 2]; /* Precomputed rhs for eqn */ for (int i = 0; i < nPts; i++) { var u = uPrime[i]; var ui = 1 - u; var b1 = 3 * u * ui * ui; var b2 = 3 * u * u * ui; A[i, 0] = tHat1 * (b1 * tHat1LenDiv); A[i, 1] = tHat2 * (b2 * tHat2LenDiv); } /* Create the C and X matrices */ var C = new element[2, 2]; /* Matrix C */ var X = new element[2]; /* Matrix X */ vector tmp; /* Utility variable */ for (int i = 0; i < nPts; i++) { C[0, 0] += A[i, 0].Dot(A[i, 0]); C[0, 1] += A[i, 0].Dot(A[i, 1]); C[1, 0] = C[0, 1]; C[1, 1] += A[i, 1].Dot(A[i, 1]); var df = d[first]; var dl = d[last]; tmp = d[first + i] - Interpolate3(uPrime[i], df, df, dl, dl); X[0] += A[i, 0].Dot(tmp); X[1] += A[i, 1].Dot(tmp); } /* Compute the determinants of C and X */ var det_C0_C1 = C[0, 0] * C[1, 1] - C[1, 0] * C[0, 1]; var det_C0_X = C[0, 0] * X[1] - C[1, 0] * X[0]; var det_X_C1 = X[0] * C[1, 1] - X[1] * C[0, 1]; /* Finally, derive alpha values */ var alpha_l = det_C0_C1 == 0 ? 0 : det_X_C1 / det_C0_C1; var alpha_r = det_C0_C1 == 0 ? 0 : det_C0_X / det_C0_C1; /* If alpha negative, use the Wu/Barsky heuristic (see text) */ /* (if alpha is 0, you get coincident control points that lead to * divide by zero in any subsequent NewtonRaphsonRootFind() call. */ var bezCurve = new cubicbezier(); var segLength = (d[last] - d[first]).Length; var epsilon = (element)1.0e-6 * segLength; if (alpha_l < epsilon || alpha_r < epsilon) { /* fall back on standard (probably inaccurate) formula, and subdivide further if needed. */ element dist = segLength / 3; bezCurve.P0 = d[first]; bezCurve.P3 = d[last]; bezCurve.P1 = bezCurve.P0 + tHat1 * (dist * tHat1LenDiv); bezCurve.P2 = bezCurve.P3 + tHat2 * (dist * tHat2LenDiv); return(bezCurve); } /* First and last control points of the Bezier curve are */ /* positioned exactly at the first and last data points */ /* Control points 1 and 2 are positioned an alpha distance out */ /* on the tangent vectors, left and right, respectively */ bezCurve.P0 = d[first]; bezCurve.P3 = d[last]; bezCurve.P1 = bezCurve.P0 + tHat1 * (alpha_l * tHat1LenDiv); bezCurve.P2 = bezCurve.P3 + tHat2 * (alpha_r * tHat2LenDiv); return(bezCurve); }
/// <summary> /// Given set of points and their parameterization, try to find a better parameterization. /// </summary> /// <param name="d">Array of digitized points</param> /// <param name="first">Indices defining region</param> /// <param name="last">Indices defining region</param> /// <param name="u">Current parameter values</param> /// <param name="bezCurve">Current fitted curve</param> /// <returns>パラメータ列</returns> static element[] Reparameterize(vector[] d, int first, int last, element[] u, cubicbezier bezCurve) { var uPrime = new element[last - first + 1]; /* New parameter values */ for (int i = first; i <= last; i++) { var j = i - first; uPrime[j] = NewtonRaphsonRootFind(bezCurve, d[i], u[j]); } return(uPrime); }
/// <summary> /// 指定された頂点列の指定範囲にベジェ曲線をフィットさせる /// </summary> /// <param name="d">フィット元頂点列</param> /// <param name="first"><see cref="d"/>内の範囲開始インデックス</param> /// <param name="last"><see cref="d"/>内の範囲終了インデックス</param> /// <param name="tHat1">指定範囲開始点の長さ1の順方向ベクトル</param> /// <param name="tHat2">指定範囲終了点の長さ1の逆方向ベクトル</param> /// <param name="error">フィット時許容誤差の二乗</param> /// <param name="result">ここにベジェ曲線列が追加される</param> static void FitCubic(vector[] d, int first, int last, vector tHat1, vector tHat2, element error, List <cubicbezier> result) { cubicbezier bezCurve; /*Control points of fitted Bezier curve*/ var nPts = last - first + 1; /* Number of points in subset */ /* Use heuristic if region only has two points in it */ if (nPts == 2) { bezCurve = new cubicbezier(); bezCurve.P0 = d[first]; bezCurve.P3 = d[last]; var dist = (d[last] - d[first]).Length / 3; bezCurve.P1 = bezCurve.P0 + tHat1.Relength(dist); bezCurve.P2 = bezCurve.P3 + tHat2.Relength(dist); result.Add(bezCurve); return; } /* Parameterize points, and attempt to fit curve */ var u = ChordLengthParameterize(d, first, last); bezCurve = GenerateBezier(d, first, last, u, tHat1, tHat2); /* Find max deviation of points to fitted curve */ int splitPoint; /* Point to split point set at */ var maxError = ComputeMaxError(d, first, last, bezCurve, u, out splitPoint); if (maxError < error) { result.Add(bezCurve); return; } /* If error not too large, try some reparameterization */ /* and iteration */ var iterationError = error * error; /*Error below which you try iterating */ if (maxError < iterationError) { var maxIterations = 4; /* Max times to try iterating */ for (var i = 0; i < maxIterations; i++) { var uPrime = Reparameterize(d, first, last, u, bezCurve); bezCurve = GenerateBezier(d, first, last, uPrime, tHat1, tHat2); maxError = ComputeMaxError(d, first, last, bezCurve, uPrime, out splitPoint); if (maxError < error) { result.Add(bezCurve); return; } u = uPrime; } } /* Fitting failed -- split at max error point and fit recursively */ var tHatCenter = ((d[splitPoint - 1] - d[splitPoint + 1]) * (element)0.5).Normalize(); FitCubic(d, first, splitPoint, tHat1, tHatCenter, error, result); tHatCenter = -tHatCenter; FitCubic(d, splitPoint, last, tHatCenter, tHat2, error, result); }