/// <summary>
/// The source band matrix.
/// </summary>
/// <param name="src">The source band matrix.</param>
/// <param name="srcIndex">The source band matrix start element index.</param>
/// <param name="dst">The destination band matrix.</param>
/// <param name="dstIndex">The destination band matrix start element index.</param>
/// <param name="length">Number of elements to copy from source band matrix.</param>
public static void Copy(BandMatrix[] src, int srcIndex, BandMatrix[] dst, int dstIndex, int length)
{
for (int i = 0; i < length; ++i)
{
dst[i + dstIndex].CopyFrom(ref src[i + srcIndex]);
}
}
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 void banbks11(BandMatrix[] m, int[] perm, double[] v, int n)
{
int i, k, l;
double tmp, x;
// forward substitution
l = 5;
for (k = 0; k < n; k++)
{
i = perm[k];
if (i != k)
{
tmp = v[k];
v[k] = v[i];
v[i] = tmp;
}
if (l < n)
l++;
for (i = k + 1; i < l; i++)
v[i] -= m[k].al[i - k - 1] * v[k];
}
// back substitution
l = 1;
for (i = n - 1; i >= 0; i--)
{
x = v[i];
for (k = 1; k < l; k++)
x -= m[i].a[k] * v[k + i];
v[i] = x / m[i].a[0];
if (l < 11)
l++;
}
}
public static void add_mat_line(BandMatrix[] m, double[] v, double[] derivs, double x, double y, int j, int jj, int jinc, int nmat)
{
int joff, k;
if (jj >= 0)
{
joff = (j + 5 - jj + nmat) % nmat;
if (nmat < 6)
{
joff = j + 5 - jj;
}
else if (nmat == 6)
{
joff = 2 + (j + 3 - jj + nmat) % nmat;
}
v[jj] += x;
for (k = 0; k < jinc; k++)
m[jj].a[joff + k] += y * derivs[k];
}
}
/// <summary>
/// Copy band matrix from source band matrix to current instance of band matrix.
/// </summary>
/// <param name="from">The source band matrix.</param>
private void CopyFrom(ref BandMatrix from)
{
Array.Copy(from.a, 0, a, 0, 11);
Array.Copy(from.al, 0, al, 0, 5);
}
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);
}
/// <summary>
/// Copy band matrix from source band matrix to current instance of band matrix.
/// </summary>
/// <param name="from">The source band matrix.</param>
private void CopyFrom(ref BandMatrix from)
{
Array.Copy(from.a, 0, a, 0, 11);
Array.Copy(from.al, 0, al, 0, 5);
}
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 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 void bandec11(BandMatrix[] m, int[] perm, int n)
{
int i, j, k, l, pivot;
double pivot_val, pivot_scale, tmp, x;
// pack top triangle to the left.
for (i = 0; i < 5; i++)
{
for (j = 0; j < i + 6; j++)
m[i].a[j] = m[i].a[j + 5 - i];
for (; j < 11; j++)
m[i].a[j] = 0.0;
}
l = 5;
for (k = 0; k < n; k++)
{
pivot = k;
pivot_val = m[k].a[0];
l = l < n ? l + 1 : n;
for (j = k + 1; j < l; j++)
if (Math.Abs(m[j].a[0]) > Math.Abs(pivot_val))
{
pivot_val = m[j].a[0];
pivot = j;
}
perm[k] = pivot;
if (pivot != k)
{
for (j = 0; j < 11; j++)
{
tmp = m[k].a[j];
m[k].a[j] = m[pivot].a[j];
m[pivot].a[j] = tmp;
}
}
if (Math.Abs(pivot_val) < 1e-12)
pivot_val = 1e-12;
pivot_scale = 1.0 / pivot_val;
for (i = k + 1; i < l; i++)
{
x = m[i].a[0] * pivot_scale;
m[k].al[i - k - 1] = x;
for (j = 1; j < 11; j++)
m[i].a[j - 1] = m[i].a[j] - x * m[k].a[j];
m[i].a[10] = 0.0;
}
}
}
