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>３次ベジェ曲線</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">指定範囲開始点の長さ１の順方向ベクトル</param>
/// <param name="tHat2">指定範囲終了点の長さ１の逆方向ベクトル</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);
}