/// <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); }
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; } } }