public static void compute_pderivs(ref SpiroSegment s, double[][] ends, double[][][] derivs, int jinc)
{
double recip_d = 2e6;
double delta = 1.0 / recip_d;
double[] try_ks = new double[4];
double[][] try_ends = { new double[4], new double[4] };
int i, j, k;
compute_ends(s.ks, ends, s.seg_ch);
for (i = 0; i < jinc; i++)
{
for (j = 0; j < 4; j++)
{
try_ks[j] = s.ks[j];
}
try_ks[i] += delta;
compute_ends(try_ks, try_ends, s.seg_ch);
for (k = 0; k < 2; k++)
{
for (j = 0; j < 4; j++)
{
derivs[j][k][i] = recip_d * (try_ends[k][j] - ends[k][j]);
}
}
}
}
public static SpiroSegment[]? setup_path(SpiroControlPoint[] src, int n)
{
int i, ilast, n_seg;
double dx, dy;
SpiroSegment[] r;
#if CHECK_INPUT_FINITENESS
// Verify that input values are within realistic limits
for (i = 0; i < n; i++)
{
if (IsFinite(src[i].X) == 0 || IsFinite(src[i].Y) == 0)
{
return(null);
}
}
#endif
n_seg = src[0].Type == SpiroPointType.OpenContour ? n - 1 : n;
r = new SpiroSegment[n_seg + 1];
for (int j = 0; j < n_seg + 1; j++)
{
r[j].ks = new double[4];
}
if (r == null)
{
return(null);
}
for (i = 0; i < n_seg; i++)
{
r[i].X = src[i].X;
r[i].Y = src[i].Y;
r[i].Type = src[i].Type;
r[i].ks[0] = 0.0;
r[i].ks[1] = 0.0;
r[i].ks[2] = 0.0;
r[i].ks[3] = 0.0;
}
r[n_seg].X = src[n_seg % n].X;
r[n_seg].Y = src[n_seg % n].Y;
r[n_seg].Type = src[n_seg % n].Type;
for (i = 0; i < n_seg; i++)
{
dx = r[i + 1].X - r[i].X;
dy = r[i + 1].Y - r[i].Y;
#if !CHECK_INPUT_FINITENESS
r[i].seg_ch = hypot(dx, dy);
#else
if (IsFinite(dx) == 0 || IsFinite(dy) == 0 || IsFinite((r[i].seg_ch = hypot(dx, dy))) == 0)
{
return(null);
}
#endif
r[i].seg_th = Math.Atan2(dy, dx);
}
ilast = n_seg - 1;
for (i = 0; i < n_seg; i++)
{
if (r[i].Type == SpiroPointType.OpenContour || r[i].Type == SpiroPointType.EndOpenContour || r[i].Type == SpiroPointType.Corner)
{
r[i].bend_th = 0.0;
}
else
{
r[i].bend_th = mod_2pi(r[i].seg_th - r[ilast].seg_th);
}
ilast = i;
}
return(r);
}
public static void spiro_to_bpath(SpiroSegment[] s, int n, IBezierContext bc)
{
int i, nsegs;
double x0, y0, x1, y1;
if (s == null || n <= 0 || bc == null)
return;
nsegs = s[n - 1].Type == SpiroPointType.EndOpenContour ? n - 1 : n;
for (i = 0; i < nsegs; i++)
{
x0 = s[i].X; x1 = s[i + 1].X;
y0 = s[i].Y; y1 = s[i + 1].Y;
if (i == 0)
bc.MoveTo(x0, y0, s[0].Type == SpiroPointType.OpenContour ? true : false);
bc.MarkKnot(i, get_knot_th(s, i), s[i].X, s[i].Y, s[i].Type);
spiro_seg_to_bpath(s[i].ks, x0, y0, x1, y1, bc, 0);
}
if (nsegs == n - 1)
bc.MarkKnot(n - 1, get_knot_th(s, n - 1), s[n - 1].X, s[n - 1].Y, s[n - 1].Type);
}
public static int solve_spiro(SpiroSegment[] s, int nseg)
{
int i, converged, nmat, n_alloc;
BandMatrix[] m;
double[] v;
int[] perm;
double norm;
nmat = count_vec(s, nseg);
n_alloc = nmat;
if (nmat == 0)
return 1; // just means no convergence problems
if (s[0].Type != SpiroPointType.OpenContour && s[0].Type != SpiroPointType.Corner)
n_alloc *= 3;
if (n_alloc < 5)
n_alloc = 5;
m = new BandMatrix[n_alloc];
for (int n = 0; n < n_alloc; n++)
{
m[n].a = new double[11];
m[n].al = new double[5];
}
v = new double[n_alloc];
perm = new int[n_alloc];
i = converged = 0; // not solved (yet)
if (m != null && v != null && perm != null)
{
while (i++ < 60)
{
norm = spiro_iter(s, m, perm, v, nseg, nmat);
if (check_finiteness(s, nseg) == false)
break;
if (norm < 1e-12)
{
converged = 1;
break;
}
}
}
return converged;
}
public static double spiro_iter(SpiroSegment[] s, BandMatrix[] m, int[] perm, double[] v, int n, int nmat)
{
bool cyclic;
int i, j, jthl, jthr, jk0l, jk0r, jk1l, jk1r, jk2l, jk2r, jinc, jj, k, n_invert;
SpiroPointType ty0, ty1;
double dk, norm, th;
double[][] ends = { new double[4], new double[4] };
double[][][] derivs =
{
new double[2][] { new double[4], new double[4] },
new double[2][] { new double[4], new double[4] },
new double[2][] { new double[4], new double[4] },
new double[2][] { new double[4], new double[4] },
};
cyclic = s[0].Type != SpiroPointType.OpenContour && s[0].Type != SpiroPointType.Corner;
for (i = 0; i < nmat; i++)
{
v[i] = 0.0;
for (j = 0; j < 11; j++)
m[i].a[j] = 0.0;
for (j = 0; j < 5; j++)
m[i].al[j] = 0.0;
}
j = 0;
if (s[0].Type == SpiroPointType.G4)
jj = nmat - 2;
else if (s[0].Type == SpiroPointType.G2)
jj = nmat - 1;
else
jj = 0;
for (i = 0; i < n; i++)
{
ty0 = s[i].Type;
ty1 = s[i + 1].Type;
jinc = compute_jinc(ty0, ty1);
th = s[i].bend_th;
jthl = jk0l = jk1l = jk2l = -1;
jthr = jk0r = jk1r = jk2r = -1;
compute_pderivs(ref s[i], ends, derivs, jinc);
// constraints crossing left
if (ty0 == SpiroPointType.G4 || ty0 == SpiroPointType.G2 || ty0 == SpiroPointType.Left || ty0 == SpiroPointType.Right)
{
jthl = jj++;
jj %= nmat;
jk0l = jj++;
if (ty0 == SpiroPointType.G4)
{
jj %= nmat;
jk1l = jj++;
jk2l = jj++;
}
}
// constraints on left
if ((ty0 == SpiroPointType.Left || ty0 == SpiroPointType.Corner || ty0 == SpiroPointType.OpenContour || ty0 == SpiroPointType.G2) && jinc == 4)
{
if (ty0 != SpiroPointType.G2)
jk1l = jj++;
jk2l = jj++;
}
// constraints on right
if ((ty1 == SpiroPointType.Right || ty1 == SpiroPointType.Corner || ty1 == SpiroPointType.EndOpenContour || ty1 == SpiroPointType.G2) && jinc == 4)
{
if (ty1 != SpiroPointType.G2)
jk1r = jj++;
jk2r = jj++;
}
// constraints crossing right
if (ty1 == SpiroPointType.G4 || ty1 == SpiroPointType.G2 || ty1 == SpiroPointType.Left || ty1 == SpiroPointType.Right)
{
jthr = jj;
jk0r = (jj + 1) % nmat;
if (ty1 == SpiroPointType.G4)
{
jk1r = (jj + 2) % nmat;
jk2r = (jj + 3) % nmat;
}
}
add_mat_line(m, v, derivs[0][0], th - ends[0][0], 1, j, jthl, jinc, nmat);
add_mat_line(m, v, derivs[1][0], ends[0][1], -1, j, jk0l, jinc, nmat);
add_mat_line(m, v, derivs[2][0], ends[0][2], -1, j, jk1l, jinc, nmat);
add_mat_line(m, v, derivs[3][0], ends[0][3], -1, j, jk2l, jinc, nmat);
add_mat_line(m, v, derivs[0][1], -ends[1][0], 1, j, jthr, jinc, nmat);
add_mat_line(m, v, derivs[1][1], -ends[1][1], 1, j, jk0r, jinc, nmat);
add_mat_line(m, v, derivs[2][1], -ends[1][2], 1, j, jk1r, jinc, nmat);
add_mat_line(m, v, derivs[3][1], -ends[1][3], 1, j, jk2r, jinc, nmat);
if (jthl >= 0)
v[jthl] = mod_2pi(v[jthl]);
if (jthr >= 0)
v[jthr] = mod_2pi(v[jthr]);
j += jinc;
}
if (cyclic)
{
BandMatrix.Copy(m, 0, m, nmat, nmat);
BandMatrix.Copy(m, 0, m, 2 * nmat, nmat);
Array.Copy(v, 0, v, nmat, nmat);
Array.Copy(v, 0, v, 2 * nmat, nmat);
n_invert = 3 * nmat;
j = nmat;
}
else
{
n_invert = nmat;
j = 0;
}
bandec11(m, perm, n_invert);
banbks11(m, perm, v, n_invert);
norm = 0.0;
for (i = 0; i < n; i++)
{
jinc = compute_jinc(s[i].Type, s[i + 1].Type);
for (k = 0; k < jinc; k++)
{
dk = v[j++];
s[i].ks[k] += dk;
norm += dk * dk;
}
s[i].ks[0] = 2.0 * mod_2pi(s[i].ks[0] / 2.0);
}
return norm;
}
public static SpiroSegment[] setup_path(SpiroControlPoint[] src, int n)
{
int i, ilast, n_seg;
double dx, dy;
SpiroSegment[] r;
#if CHECK_INPUT_FINITENESS
// Verify that input values are within realistic limits
for (i = 0; i < n; i++)
{
if (IsFinite(src[i].X) == 0 || IsFinite(src[i].Y) == 0)
{
return null;
}
}
#endif
n_seg = src[0].Type == SpiroPointType.OpenContour ? n - 1 : n;
r = new SpiroSegment[n_seg + 1];
for (int j = 0; j < n_seg + 1; j++)
r[j].ks = new double[4];
if (r == null)
return null;
for (i = 0; i < n_seg; i++)
{
r[i].X = src[i].X;
r[i].Y = src[i].Y;
r[i].Type = src[i].Type;
r[i].ks[0] = 0.0;
r[i].ks[1] = 0.0;
r[i].ks[2] = 0.0;
r[i].ks[3] = 0.0;
}
r[n_seg].X = src[n_seg % n].X;
r[n_seg].Y = src[n_seg % n].Y;
r[n_seg].Type = src[n_seg % n].Type;
for (i = 0; i < n_seg; i++)
{
dx = r[i + 1].X - r[i].X;
dy = r[i + 1].Y - r[i].Y;
#if !CHECK_INPUT_FINITENESS
r[i].seg_ch = hypot(dx, dy);
#else
if (IsFinite(dx) == 0 || IsFinite(dy) == 0 || IsFinite((r[i].seg_ch = hypot(dx, dy))) == 0)
{
return null;
}
#endif
r[i].seg_th = Math.Atan2(dy, dx);
}
ilast = n_seg - 1;
for (i = 0; i < n_seg; i++)
{
if (r[i].Type == SpiroPointType.OpenContour || r[i].Type == SpiroPointType.EndOpenContour || r[i].Type == SpiroPointType.Corner)
r[i].bend_th = 0.0;
else
r[i].bend_th = mod_2pi(r[i].seg_th - r[ilast].seg_th);
ilast = i;
}
return r;
}
public static double get_knot_th(SpiroSegment[] s, int i)
{
double[][] ends = { new double[4], new double[4] };
if (i == 0)
{
compute_ends(s[i].ks, ends, s[i].seg_ch);
return s[i].seg_th - ends[0][0];
}
else
{
compute_ends(s[i - 1].ks, ends, s[i - 1].seg_ch);
return s[i - 1].seg_th + ends[1][0];
}
}
public static int count_vec(SpiroSegment[] s, int nseg)
{
int i, n;
n = 0;
for (i = 0; i < nseg; i++)
n += compute_jinc(s[i].Type, s[i + 1].Type);
return n;
}
public static void compute_pderivs(ref SpiroSegment s, double[][] ends, double[][][] derivs, int jinc)
{
double recip_d = 2e6;
double delta = 1.0 / recip_d;
double[] try_ks = new double[4];
double[][] try_ends = { new double[4], new double[4] };
int i, j, k;
compute_ends(s.ks, ends, s.seg_ch);
for (i = 0; i < jinc; i++)
{
for (j = 0; j < 4; j++)
try_ks[j] = s.ks[j];
try_ks[i] += delta;
compute_ends(try_ks, try_ends, s.seg_ch);
for (k = 0; k < 2; k++)
for (j = 0; j < 4; j++)
derivs[j][k][i] = recip_d * (try_ends[k][j] - ends[k][j]);
}
}
public static bool check_finiteness(SpiroSegment[] segs, int num_segs)
{
// Check if all values are "finite", return true, else return fail=false
int i, j;
for (i = 0; i < num_segs; ++i)
for (j = 0; j < 4; ++j)
if (IsFinite(segs[i].ks[j]) == 0)
return false;
return true;
}
