public void Solve()
{
Problem problem = new Problem(new ProblemInfo {
Mesh = LinearMesh, SolverType = Solver.SolverType
});
problem.Solve();
Q = problem.Q;
MatrixPortrait MP = new MatrixPortrait();
PB.Build(MP);
IMatrix A = new SymmetricSparseMatrix(Mesh.NodeCount, MP);
double[] B = new double[Mesh.NodeCount];
CurrentIter = 0;
Diff = 1.0;
QDiff = 1.0;
while (CurrentIter < MaxIters && Diff >= Eps && QDiff >= Delta)
{
// SLAE Solution
NSB.Build(A, B, Q);
prevQ = Q;
Q = Solver.Solve(A, B);
QDiff = VectorUtils.RelativeError(prevQ, Q);
A.Clear();
Array.Fill(B, 0.0);
if (DoOptimization)
{
// Optimization
double w = OptimizeQ(A, B);
// Calculating new Q
for (int i = 0; i < Q.Length; i++)
{
Q[i] = Q[i] * w + prevQ[i] * (1.0 - w);
}
}
// Calculate diff
NSB.Build(A, B, Q);
A.Multiply(Q, Aq);
Diff = VectorUtils.RelativeError(Aq, B);
CurrentIter++;
Console.WriteLine($"Iter: {CurrentIter}, Diff: {Diff}, QDiff: {QDiff}");
A.Clear();
Array.Fill(B, 0.0);
Array.Fill(Aq, 0.0);
}
}
public static void test_Matrices()
{
int N = 10;
DenseMatrix M1 = new DenseMatrix(N, N);
SymmetricSparseMatrix M2 = new SymmetricSparseMatrix();
for (int i = 0; i < N; ++i)
{
for (int j = i; j < N; ++j)
{
if (i == j)
{
M1.Set(i, i, 1);
M2.Set(i, i, 1);
}
else if (j % 2 != 0)
{
double d = Math.Sqrt(i + j);
M1.Set(i, j, d);
M1.Set(j, i, d);
M2.Set(i, j, d);
}
}
}
double[] X = new double[N], b1 = new double[N], b2 = new double[N];
for (int i = 0; i < N; ++i)
{
X[i] = (double)i / (double)N;
}
M1.Multiply(X, b1);
M2.Multiply(X, b2);
for (int i = 0; i < N; ++i)
{
Debug.Assert(MathUtil.EpsilonEqual(b1[i], b2[i]));
}
}
public void Initialize()
{
ToMeshV = new int[Mesh.MaxVertexID];
ToIndex = new int[Mesh.MaxVertexID];
N = 0;
foreach (int vid in Mesh.VertexIndices())
{
ToMeshV[N] = vid;
ToIndex[vid] = N;
N++;
}
Px = new double[N];
Py = new double[N];
Pz = new double[N];
nbr_counts = new int[N];
M = new SymmetricSparseMatrix();
for (int i = 0; i < N; ++i)
{
int vid = ToMeshV[i];
Vector3D v = Mesh.GetVertex(vid);
Px[i] = v.x; Py[i] = v.y; Pz[i] = v.z;
nbr_counts[i] = Mesh.GetVtxEdgeCount(vid);
}
// construct laplacian matrix
for (int i = 0; i < N; ++i)
{
int vid = ToMeshV[i];
int n = nbr_counts[i];
double sum_w = 0;
foreach (int nbrvid in Mesh.VtxVerticesItr(vid))
{
int j = ToIndex[nbrvid];
int n2 = nbr_counts[j];
// weight options
//double w = -1;
double w = -1.0 / Math.Sqrt(n + n2);
//double w = -1.0 / n;
M.Set(i, j, w);
sum_w += w;
}
sum_w = -sum_w;
M.Set(vid, vid, sum_w);
}
// compute laplacian vectors of initial mesh positions
MLx = new double[N];
MLy = new double[N];
MLz = new double[N];
M.Multiply(Px, MLx);
M.Multiply(Py, MLy);
M.Multiply(Pz, MLz);
// allocate memory for internal buffers
Preconditioner = new DiagonalMatrix(N);
WeightsM = new DiagonalMatrix(N);
Cx = new double[N]; Cy = new double[N]; Cz = new double[N];
Bx = new double[N]; By = new double[N]; Bz = new double[N];
Sx = new double[N]; Sy = new double[N]; Sz = new double[N];
UpdateForSolve();
}
// Result must be as large as Mesh.MaxVertexID
public bool Solve(Vector3D[] Result)
{
UpdateForSolve();
// use initial positions as initial solution.
Array.Copy(Px, Sx, N);
Array.Copy(Py, Sy, N);
Array.Copy(Pz, Sz, N);
Action <double[], double[]> CombinedMultiply = (X, B) => {
M.Multiply(X, B);
for (int i = 0; i < N; ++i)
{
B[i] += WeightsM[i, i] * X[i];
}
};
SparseSymmetricCG SolverX = new SparseSymmetricCG()
{
B = Bx, X = Sx,
MultiplyF = CombinedMultiply, PreconditionMultiplyF = Preconditioner.Multiply,
UseXAsInitialGuess = true
};
SparseSymmetricCG SolverY = new SparseSymmetricCG()
{
B = By, X = Sy,
MultiplyF = CombinedMultiply, PreconditionMultiplyF = Preconditioner.Multiply,
UseXAsInitialGuess = true
};
SparseSymmetricCG SolverZ = new SparseSymmetricCG()
{
B = Bz, X = Sz,
MultiplyF = CombinedMultiply, PreconditionMultiplyF = Preconditioner.Multiply,
UseXAsInitialGuess = true
};
SparseSymmetricCG[] solvers = new SparseSymmetricCG[3] {
SolverX, SolverY, SolverZ
};
bool[] ok = new bool[3];
int[] indices = new int[3] {
0, 1, 2
};
// preconditioned solve is slower =\
//Action<int> SolveF = (i) => { ok[i] = solvers[i].SolvePreconditioned(); };
Action <int> SolveF = (i) => { ok[i] = solvers[i].Solve(); };
gParallel.ForEach(indices, SolveF);
if (ok[0] == false || ok[1] == false || ok[2] == false)
{
return(false);
}
for (int i = 0; i < N; ++i)
{
int vid = ToMeshV[i];
Result[vid] = new Vector3D(Sx[i], Sy[i], Sz[i]);
}
// apply post-fixed constraints
if (HavePostFixedConstraints)
{
foreach (var constraint in SoftConstraints)
{
if (constraint.Value.PostFix)
{
int vid = constraint.Key;
Result[vid] = constraint.Value.Position;
}
}
}
return(true);
}
public void Initialize()
{
ToMeshV = new int[Mesh.MaxVertexID];
ToIndex = new int[Mesh.MaxVertexID];
N = 0;
foreach (int vid in Mesh.VertexIndices())
{
ToMeshV[N] = vid;
ToIndex[vid] = N;
N++;
}
Px = new double[N];
Py = new double[N];
Pz = new double[N];
nbr_counts = new int[N];
M = new SymmetricSparseMatrix();
for (int i = 0; i < N; ++i)
{
int vid = ToMeshV[i];
Vector3D v = Mesh.GetVertex(vid);
Px[i] = v.x; Py[i] = v.y; Pz[i] = v.z;
nbr_counts[i] = Mesh.GetVtxEdgeCount(vid);
}
// construct laplacian matrix
for (int i = 0; i < N; ++i)
{
int vid = ToMeshV[i];
int n = nbr_counts[i];
double sum_w = 0;
foreach (int nbrvid in Mesh.VtxVerticesItr(vid))
{
int j = ToIndex[nbrvid];
int n2 = nbr_counts[j];
// weight options
//double w = -1;
double w = -1.0 / Math.Sqrt(n + n2);
//double w = -1.0 / n;
M.Set(i, j, w);
sum_w += w;
}
sum_w = -sum_w;
M.Set(vid, vid, sum_w);
}
// transpose(L) * L, but matrix is symmetric...
if (UseSoftConstraintNormalEquations)
{
//M = M.Multiply(M);
M = M.Square(); // only works if M is symmetric
}
// construct packed version of M matrix
PackedM = new PackedSparseMatrix(M);
// compute laplacian vectors of initial mesh positions
MLx = new double[N];
MLy = new double[N];
MLz = new double[N];
M.Multiply(Px, MLx);
M.Multiply(Py, MLy);
M.Multiply(Pz, MLz);
// zero out...
for (int i = 0; i < Px.Length; ++i)
{
MLx[i] = 0;
MLy[i] = 0;
MLz[i] = 0;
}
// allocate memory for internal buffers
Preconditioner = new DiagonalMatrix(N);
WeightsM = new DiagonalMatrix(N);
Cx = new double[N]; Cy = new double[N]; Cz = new double[N];
Bx = new double[N]; By = new double[N]; Bz = new double[N];
Sx = new double[N]; Sy = new double[N]; Sz = new double[N];
UpdateForSolve();
}
public static void test_Matrices()
{
int N = 200;
//int N = 2500;
DenseMatrix M1 = new DenseMatrix(N, N);
SymmetricSparseMatrix M2 = new SymmetricSparseMatrix();
for (int i = 0; i < N; ++i)
{
for (int j = i; j < N; ++j)
{
if (i == j)
{
M1.Set(i, i, N);
M2.Set(i, i, N);
}
else if (j % 2 != 0)
{
double d = 1.0 / Math.Sqrt(i + j);
M1.Set(i, j, d);
M1.Set(j, i, d);
M2.Set(i, j, d);
}
}
}
double[] X = new double[N], b1 = new double[N], b2 = new double[N];
for (int i = 0; i < N; ++i)
{
X[i] = (double)i / (double)N;
}
M1.Multiply(X, b1);
M2.Multiply(X, b2);
for (int i = 0; i < N; ++i)
{
Debug.Assert(MathUtil.EpsilonEqual(b1[i], b2[i]));
}
Debug.Assert(M1.IsSymmetric());
Debug.Assert(M1.IsPositiveDefinite());
// test parallel cholesky decomposition
LocalProfiler p = new LocalProfiler();
p.Start("chol");
CholeskyDecomposition decompM = new CholeskyDecomposition(M1);
decompM.ComputeParallel();
p.Stop("chol");
//System.Console.WriteLine(p.AllTimes());
DenseMatrix LLT_M1 = decompM.L.Multiply(decompM.L.Transpose());
if (LLT_M1.EpsilonEquals(M1) == false)
{
System.Console.WriteLine("FAIL choleskyM1 did not reproduce input");
}
// test cholesky-decomp backsubstitution
Random r = new Random(31337);
double[] RealX = TestUtil.RandomScalars(N, r, new Interval1d(-10, 10));
double[] B = new double[N], SolvedX = new double[N], TmpY = new double[N];
M1.Multiply(RealX, B);
decompM.Solve(B, SolvedX, TmpY);
if (BufferUtil.DistanceSquared(RealX, SolvedX) > MathUtil.ZeroTolerance)
{
System.Console.WriteLine("FAIL choleskyM1 backsubstution did not reproduce input vector");
}
// test case from: https://people.cs.kuleuven.be/~karl.meerbergen/didactiek/h03g1a/ilu.pdf
//DenseMatrix tmp = new DenseMatrix(6, 6);
//tmp.Set(new double[] {
// 3,0,-1,-1,0,-1,
// 0,2,0,-1,0,0,
// -1,0,3,0,-1,0,
// -1,-1,0,2,0,-1,
// 0,0,-1,0,3,-1,
// -1,0,0,-1,-1,4});
//CholeskyDecomposition decompDense = new CholeskyDecomposition(tmp);
//decompDense.Compute();
//PackedSparseMatrix M1_sparse = PackedSparseMatrix.FromDense(tmp, true);
//M1_sparse.Sort();
//SparseCholeskyDecomposition decompM1_sparse = new SparseCholeskyDecomposition(M1_sparse);
//decompM1_sparse.ComputeIncomplete();
// cholesky decomposition known-result test
DenseMatrix MSym3x3 = new DenseMatrix(3, 3);
MSym3x3.Set(new double[] { 25, 15, -5, 15, 18, 0, -5, 0, 11 });
DenseMatrix MSym3x3_Chol = new DenseMatrix(3, 3);
MSym3x3_Chol.Set(new double[] { 5, 0, 0, 3, 3, 0, -1, 1, 3 });
CholeskyDecomposition decomp3x3 = new CholeskyDecomposition(MSym3x3);
decomp3x3.Compute();
if (decomp3x3.L.EpsilonEquals(MSym3x3_Chol) == false)
{
System.Console.WriteLine("FAIL cholesky3x3 incorrect result");
}
if (decomp3x3.L.Multiply(decomp3x3.L.Transpose()).EpsilonEquals(MSym3x3) == false)
{
System.Console.WriteLine("FAIL cholesky3x3 did not reproduce input");
}
// cholesky decomposition known-result test
DenseMatrix MSym4x4 = new DenseMatrix(4, 4);
MSym4x4.Set(new double[] {
18, 22, 54, 42, 22, 70, 86, 62,
54, 86, 174, 134, 42, 62, 134, 106
});
DenseMatrix MSym4x4_Chol = new DenseMatrix(4, 4);
MSym4x4_Chol.Set(new double[] {
4.24264, 0, 0, 0, 5.18545, 6.56591, 0, 0,
12.72792, 3.04604, 1.64974, 0, 9.89949, 1.62455, 1.84971, 1.39262
});
CholeskyDecomposition decomp4x4 = new CholeskyDecomposition(MSym4x4);
decomp4x4.Compute();
if (decomp4x4.L.EpsilonEquals(MSym4x4_Chol, 0.0001) == false)
{
System.Console.WriteLine("FAIL cholesky4x4 incorrect result");
}
if (decomp4x4.L.Multiply(decomp4x4.L.Transpose()).EpsilonEquals(MSym4x4) == false)
{
System.Console.WriteLine("FAIL cholesky4x4 did not reproduce input");
}
}
public static void test_SparseCG_Precond()
{
// A test case where Jacobi preconditioner (ie M = diag(A)) provides some improvement
// described in http://www.math.iit.edu/~fass/477577_Chapter_16.pdf
int N = 10000;
SymmetricSparseMatrix M = new SymmetricSparseMatrix();
double[] B = new double[N];
for (int i = 0; i < N; ++i)
{
for (int j = i; j < N; ++j)
{
if (i == j)
{
M.Set(i, i, 0.5 + Math.Sqrt(i));
}
else if (Math.Abs(i - j) == 1)
{
M.Set(i, j, 1);
}
else if (Math.Abs(i - j) == 100)
{
M.Set(i, j, 1);
}
}
B[i] = 1;
}
SparseSymmetricCG Solver = new SparseSymmetricCG()
{
B = B, MultiplyF = M.Multiply
};
Solver.Solve();
double[] BTest = new double[N];
M.Multiply(Solver.X, BTest);
double diff = BufferUtil.DistanceSquared(B, BTest);
if (diff > MathUtil.ZeroTolerance)
{
System.Console.WriteLine("test_SparseCG: initial solve failed!");
}
PackedSparseMatrix PackedM = new PackedSparseMatrix(M);
PackedM.Sort();
SparseSymmetricCG Solver_PackedM = new SparseSymmetricCG()
{
B = B, MultiplyF = PackedM.Multiply
};
Solver_PackedM.Solve();
PackedM.Multiply(Solver_PackedM.X, BTest);
double diff_packed = BufferUtil.DistanceSquared(B, BTest);
if (diff_packed > MathUtil.ZeroTolerance)
{
System.Console.WriteLine("test_SparseCG: Packed solve failed!");
}
#if false
SparseCholeskyDecomposition cholDecomp = new SparseCholeskyDecomposition(PackedM);
cholDecomp.ComputeIncomplete();
// factorization is filled with NaNs!! doing something wrong.
double[] TmpX = new double[N], Y = new double[N];
cholDecomp.Solve(BTest, TmpX, Y);
// note: can also try just lower-triangular matrix - this is (L+D), Gauss-Seidel preconditioner?
// see http://www.math.iit.edu/~fass/477577_Chapter_16.pdf
Action <double[], double[]> cholPrecond = (R, Z) => {
cholDecomp.Solve(R, Z, Y);
};
SymmetricSparseMatrix diagPrecond = new SymmetricSparseMatrix(N);
for (int k = 0; k < N; ++k)
{
diagPrecond[k, k] = 1.0 / M[k, k];
}
SparseSymmetricCG Solver_Precond = new SparseSymmetricCG()
{
B = B, MultiplyF = PackedM.Multiply, PreconditionMultiplyF = diagPrecond.Multiply
};
//SparseSymmetricCG Solver_Precond = new SparseSymmetricCG() { B = B, MultiplyF = PackedM.Multiply, PreconditionMultiplyF = cholPrecond };
Solver_Precond.SolvePreconditioned();
PackedM.Multiply(Solver_Precond.X, BTest);
double diff_precond = BufferUtil.DistanceSquared(B, BTest);
if (diff_precond > MathUtil.ZeroTolerance)
{
System.Console.WriteLine("test_SparseCG: cholesky-preconditioned solve failed!");
}
System.Console.WriteLine("Iterations regular {0} precond {1}", Solver_PackedM.Iterations, Solver_Precond.Iterations);
System.Console.WriteLine("Tol regular {0} precond {1}", diff_packed, diff_precond);
#endif
}
public static void test_SparseCG()
{
Random r = new Random(31337);
int N = 100;
var pts = TestUtil.RandomScalars(N, r, new Interval1d(1, 10));
SymmetricSparseMatrix M = new SymmetricSparseMatrix();
double[] B = new double[N];
for (int i = 0; i < N; ++i)
{
for (int j = i; j < N; ++j)
{
if (i == j)
{
M.Set(i, j, pts[i]);
}
else
{
M.Set(i, j, (double)(i + j) / 10.0);
}
}
B[i] = i + 1;
}
SparseSymmetricCG Solver = new SparseSymmetricCG()
{
B = B, MultiplyF = M.Multiply
};
Solver.Solve();
double[] BTest = new double[N];
M.Multiply(Solver.X, BTest);
double diff = BufferUtil.DistanceSquared(B, BTest);
if (diff > MathUtil.ZeroTolerance)
{
System.Console.WriteLine("test_SparseCG: initial solve failed!");
}
PackedSparseMatrix PackedM = new PackedSparseMatrix(M);
PackedM.Sort();
SparseSymmetricCG Solver_PackedM = new SparseSymmetricCG()
{
B = B, MultiplyF = PackedM.Multiply
};
Solver_PackedM.Solve();
PackedM.Multiply(Solver_PackedM.X, BTest);
double diff_packed = BufferUtil.DistanceSquared(B, BTest);
if (diff_packed > MathUtil.ZeroTolerance)
{
System.Console.WriteLine("test_SparseCG: Packed solve failed!");
}
#if false
SparseCholeskyDecomposition decomp = new SparseCholeskyDecomposition(PackedM);
decomp.ComputeIncomplete();
PackedSparseMatrix choleskyPrecond = decomp.L.Square();
SymmetricSparseMatrix diagPrecond = new SymmetricSparseMatrix(N);
for (int k = 0; k < N; ++k)
{
diagPrecond[k, k] = 1.0 / M[k, k];
}
SparseSymmetricCG Solver_Precond = new SparseSymmetricCG()
{
B = B, MultiplyF = PackedM.Multiply, PreconditionMultiplyF = diagPrecond.Multiply
};
Solver_Precond.SolvePreconditioned();
PackedM.Multiply(Solver_Precond.X, BTest);
double diff_precond = BufferUtil.DistanceSquared(B, BTest);
if (diff_precond > MathUtil.ZeroTolerance)
{
System.Console.WriteLine("test_SparseCG: cholesky-preconditioned solve failed!");
}
System.Console.WriteLine("Iterations regular {0} precond {1}", Solver_PackedM.Iterations, Solver_Precond.Iterations);
System.Console.WriteLine("Tol regular {0} precond {1}", diff_packed, diff_precond);
#endif
}
// [RMS] this only tests some basic cases...
public static void test_Laplacian()
{
// compact version
DMesh3 mesh = new DMesh3(TestUtil.MakeRemeshedCappedCylinder(1.0), true);
Debug.Assert(mesh.IsCompact);
AxisAlignedBox3d bounds = mesh.GetBounds();
TestUtil.WriteDebugMesh(mesh, "___CG_before.obj");
List <IMesh> result_meshes = new List <IMesh>();
// make uniform laplacian matrix
int N = mesh.VertexCount;
SymmetricSparseMatrix M = new SymmetricSparseMatrix();
//DenseMatrix M = new DenseMatrix(N, N);
double[] Px = new double[N], Py = new double[N], Pz = new double[N];
int[] nbr_counts = new int[N];
for (int vid = 0; vid < N; ++vid)
{
nbr_counts[vid] = mesh.GetVtxEdgeCount(vid);
}
int ti = MeshQueries.FindNearestTriangle_LinearSearch(mesh, new Vector3d(2, 5, 2));
int v_pin = mesh.GetTriangle(ti).a;
List <int> constraints = new List <int>()
{
v_pin
};
double consW = 10;
double consBottom = 10;
foreach (int vid in constraints)
{
result_meshes.Add(TestUtil.MakeMarker(mesh.GetVertex(vid), (vid == 0) ? 0.2f : 0.1f, Colorf.Red));
}
for (int vid = 0; vid < N; ++vid)
{
int n = nbr_counts[vid];
Vector3d v = mesh.GetVertex(vid), c = Vector3d.Zero;
Px[vid] = v.x; Py[vid] = v.y; Pz[vid] = v.z;
bool bottom = (v.y - bounds.Min.y) < 0.01f;
double sum_w = 0;
foreach (int nbrvid in mesh.VtxVerticesItr(vid))
{
int n2 = nbr_counts[nbrvid];
// weight options
//double w = -1;
double w = -1.0 / Math.Sqrt(n + n2);
//double w = -1.0 / n;
M.Set(vid, nbrvid, w);
c += w * mesh.GetVertex(nbrvid);
sum_w += w;
}
sum_w = -sum_w;
M.Set(vid, vid, sum_w);
// add soft constraints
if (constraints.Contains(vid))
{
M.Set(vid, vid, sum_w + consW);
}
else if (bottom)
{
M.Set(vid, vid, sum_w + consBottom);
}
}
// compute laplacians
double[] MLx = new double[N], MLy = new double[N], MLz = new double[N];
M.Multiply(Px, MLx);
M.Multiply(Py, MLy);
M.Multiply(Pz, MLz);
DiagonalMatrix Preconditioner = new DiagonalMatrix(N);
for (int i = 0; i < N; i++)
{
Preconditioner.Set(i, i, 1.0 / M[i, i]);
}
MLy[v_pin] += consW * 0.5f;
MLx[v_pin] += consW * 0.5f;
MLz[v_pin] += consW * 0.5f;
bool useXAsGuess = true;
// preconditioned
SparseSymmetricCG SolverX = new SparseSymmetricCG()
{
B = MLx, X = Px, MultiplyF = M.Multiply, PreconditionMultiplyF = Preconditioner.Multiply, UseXAsInitialGuess = useXAsGuess
};
// initial solution
SparseSymmetricCG SolverY = new SparseSymmetricCG()
{
B = MLy, X = Py, MultiplyF = M.Multiply, UseXAsInitialGuess = useXAsGuess
};
// neither of those
SparseSymmetricCG SolverZ = new SparseSymmetricCG()
{
B = MLz, MultiplyF = M.Multiply
};
bool bx = SolverX.Solve();
bool by = SolverY.Solve();
bool bz = SolverZ.Solve();
for (int vid = 0; vid < mesh.VertexCount; ++vid)
{
Vector3d newV = new Vector3d(SolverX.X[vid], SolverY.X[vid], SolverZ.X[vid]);
mesh.SetVertex(vid, newV);
}
result_meshes.Add(mesh);
TestUtil.WriteDebugMeshes(result_meshes, "___CG_result.obj");
}