public static Vector<double> Solve(List<double[]> Vertices, List<int[]> TrianglesEdges, List<int[]> Edges, List<int> IndiceOfFixedPoints, Point3D oRoot, UnitVector3D vDir1, UnitVector3D vDir2)
{
double[,] MatrixA = new double[Edges.Count*2, Vertices.Count * 2];
for (int i = 0; i < Edges.Count; i++) {
int[] CurTri = new int[] { 0, 0, 0 };
int indexDown0 = -1;
foreach (int[] t in TrianglesEdges) {
for (int j = 0; j < 3; j++) {
if (t[j] == i) { indexDown0 = j; break; }
}
if (indexDown0 != -1) { CurTri = t; break; }
}
int indexDown1 = indexDown0 - 1;
if (indexDown1 < 0) indexDown1 = 2;
int vi0 =0, vi1 = 0, vi2 = 0;
if (Edges[CurTri[indexDown0]][0]== Edges[CurTri[indexDown1]][0]) {
vi0 = Edges[CurTri[indexDown0]][1];
vi1= Edges[CurTri[indexDown0]][0];
vi2 = Edges[CurTri[indexDown1]][1];
} else if (Edges[CurTri[indexDown0]][0] == Edges[CurTri[indexDown1]][1]) {
vi0 = Edges[CurTri[indexDown0]][1];
vi1 = Edges[CurTri[indexDown0]][0];
vi2 = Edges[CurTri[indexDown1]][0];
} else if (Edges[CurTri[indexDown0]][1] == Edges[CurTri[indexDown1]][0]) {
vi0 = Edges[CurTri[indexDown0]][0];
vi2 = Edges[CurTri[indexDown0]][1];
vi1 = Edges[CurTri[indexDown1]][1];
} else if (Edges[CurTri[indexDown0]][1] == Edges[CurTri[indexDown1]][0]) {
vi0 = Edges[CurTri[indexDown0]][0];
vi1 = Edges[CurTri[indexDown0]][1];
vi2 = Edges[CurTri[indexDown1]][0];
}
Point3D vd0, vd1, vd2;
vd0 = new Point3D(Vertices[vi0]);
vd1 = new Point3D(Vertices[vi1]);
vd2 = new Point3D(Vertices[vi2]);
Line3D lU, lD;
lU = new Line3D(vd1, vd0);
lD = new Line3D(vd1, vd2);
double Ang = lU.Direction.AngleTo(lD.Direction).Radians;
double Len = lU.Length/ lD.Length ;
MatrixA[i * 2, vi0 * 2] = 1;
MatrixA[i * 2, vi0 * 2 + 1] = 0;
MatrixA[i * 2, vi2 * 2] = -Len * Math.Cos(Ang);
MatrixA[i * 2, vi2 * 2 + 1] = Len * Math.Sin(Ang);
MatrixA[i * 2, vi1 * 2] = Len * Math.Cos(Ang) - 1;
MatrixA[i * 2, vi1 * 2 + 1] = -Len * Math.Sin(Ang);
MatrixA[i * 2 + 1, vi0 * 2] = 0;
MatrixA[i * 2 + 1, vi0 * 2 + 1] = 1;
MatrixA[i * 2 + 1, vi2 * 2] = -Len * Math.Sin(Ang);
MatrixA[i * 2 + 1, vi2 * 2 + 1] = -Len * Math.Cos(Ang);
MatrixA[i * 2 + 1, vi1 * 2] = Len * Math.Sin(Ang);
MatrixA[i* 2 + 1, vi1 * 2 + 1] = Len * Math.Cos(Ang) - 1;
}
double[,] MatrixCa = new double[IndiceOfFixedPoints.Count * 2, Vertices.Count * 2];
double[] VectorR = new double[IndiceOfFixedPoints.Count * 2];
Plane oPlane = new Plane(vDir1.CrossProduct(vDir2), oRoot);
for (int i = 0; i < IndiceOfFixedPoints.Count; i++) {
MatrixCa[i * 2, IndiceOfFixedPoints[i] * 2] = 1;
VectorR[i * 2] = new Point3D(Vertices[IndiceOfFixedPoints[i]]).ProjectOn(oPlane).X;
MatrixCa[i * 2 + 1, IndiceOfFixedPoints[i] * 2 + 1] = 1;
VectorR[i * 2 + 1] = new Point3D(Vertices[IndiceOfFixedPoints[i]]).ProjectOn(oPlane).Y;
}
Matrix<double> Ca = Matrix<double>.Build.DenseOfArray(MatrixCa);
Console.WriteLine(Ca);
Vector<double> R = Vector<double>.Build.DenseOfArray(VectorR);
Console.WriteLine(R);
Matrix<double> A = Matrix<double>.Build.DenseOfArray(MatrixA);
Console.WriteLine(A);
double Penalty = 100;
Matrix<double> Ak = A.Transpose() * A + Penalty * Ca.Transpose() * Ca;
Console.WriteLine(Ak);
Vector<double> X = Ak.Solve(Penalty * Ca.Transpose() * R);
X = Ak.Solve(Penalty * Ca.Transpose() * R);
Console.WriteLine(X);
bool valid = true;
if (!valid) {
var solver = new MathNet.Numerics.LinearAlgebra.Double.Solvers.BiCgStab();
var preconditioner = new MathNet.Numerics.LinearAlgebra.Double.Solvers.MILU0Preconditioner();
var iterator = new MathNet.Numerics.LinearAlgebra.Solvers.Iterator<double>(
new MathNet.Numerics.LinearAlgebra.Solvers.ResidualStopCriterion<double>(1.0e-8),
new MathNet.Numerics.LinearAlgebra.Solvers.IterationCountStopCriterion<double>(1000));
var x = Ak.SolveIterative(Penalty * Ca.Transpose() * R, solver, iterator);
var status = iterator.Status;
Console.WriteLine(X);
}
return X;
}
public static Vector <double> Solve(List <double[]> Vertices, List <int[]> TrianglesEdges, List <int[]> Edges, List <int> IndiceOfFixedPoints, Point3D oRoot, UnitVector3D vDir1, UnitVector3D vDir2)
{
double[,] MatrixA = new double[Edges.Count * 2, Vertices.Count * 2];
for (int i = 0; i < Edges.Count; i++)
{
int[] CurTri = new int[] { 0, 0, 0 };
int indexDown0 = -1;
foreach (int[] t in TrianglesEdges)
{
for (int j = 0; j < 3; j++)
{
if (t[j] == i)
{
indexDown0 = j; break;
}
}
if (indexDown0 != -1)
{
CurTri = t; break;
}
}
int indexDown1 = indexDown0 - 1;
if (indexDown1 < 0)
{
indexDown1 = 2;
}
int vi0 = 0, vi1 = 0, vi2 = 0;
if (Edges[CurTri[indexDown0]][0] == Edges[CurTri[indexDown1]][0])
{
vi0 = Edges[CurTri[indexDown0]][1];
vi1 = Edges[CurTri[indexDown0]][0];
vi2 = Edges[CurTri[indexDown1]][1];
}
else if (Edges[CurTri[indexDown0]][0] == Edges[CurTri[indexDown1]][1])
{
vi0 = Edges[CurTri[indexDown0]][1];
vi1 = Edges[CurTri[indexDown0]][0];
vi2 = Edges[CurTri[indexDown1]][0];
}
else if (Edges[CurTri[indexDown0]][1] == Edges[CurTri[indexDown1]][0])
{
vi0 = Edges[CurTri[indexDown0]][0];
vi2 = Edges[CurTri[indexDown0]][1];
vi1 = Edges[CurTri[indexDown1]][1];
}
else if (Edges[CurTri[indexDown0]][1] == Edges[CurTri[indexDown1]][0])
{
vi0 = Edges[CurTri[indexDown0]][0];
vi1 = Edges[CurTri[indexDown0]][1];
vi2 = Edges[CurTri[indexDown1]][0];
}
Point3D vd0, vd1, vd2;
vd0 = new Point3D(Vertices[vi0]);
vd1 = new Point3D(Vertices[vi1]);
vd2 = new Point3D(Vertices[vi2]);
Line3D lU, lD;
lU = new Line3D(vd1, vd0);
lD = new Line3D(vd1, vd2);
double Ang = lU.Direction.AngleTo(lD.Direction).Radians;
double Len = lU.Length / lD.Length;
MatrixA[i * 2, vi0 * 2] = 1;
MatrixA[i * 2, vi0 * 2 + 1] = 0;
MatrixA[i * 2, vi2 * 2] = -Len *Math.Cos(Ang);
MatrixA[i * 2, vi2 * 2 + 1] = Len * Math.Sin(Ang);
MatrixA[i * 2, vi1 * 2] = Len * Math.Cos(Ang) - 1;
MatrixA[i * 2, vi1 * 2 + 1] = -Len *Math.Sin(Ang);
MatrixA[i * 2 + 1, vi0 * 2] = 0;
MatrixA[i * 2 + 1, vi0 * 2 + 1] = 1;
MatrixA[i * 2 + 1, vi2 * 2] = -Len *Math.Sin(Ang);
MatrixA[i * 2 + 1, vi2 * 2 + 1] = -Len *Math.Cos(Ang);
MatrixA[i * 2 + 1, vi1 * 2] = Len * Math.Sin(Ang);
MatrixA[i * 2 + 1, vi1 * 2 + 1] = Len * Math.Cos(Ang) - 1;
}
double[,] MatrixCa = new double[IndiceOfFixedPoints.Count * 2, Vertices.Count * 2];
double[] VectorR = new double[IndiceOfFixedPoints.Count * 2];
Plane oPlane = new Plane(vDir1.CrossProduct(vDir2), oRoot);
for (int i = 0; i < IndiceOfFixedPoints.Count; i++)
{
MatrixCa[i * 2, IndiceOfFixedPoints[i] * 2] = 1;
VectorR[i * 2] = new Point3D(Vertices[IndiceOfFixedPoints[i]]).ProjectOn(oPlane).X;
MatrixCa[i * 2 + 1, IndiceOfFixedPoints[i] * 2 + 1] = 1;
VectorR[i * 2 + 1] = new Point3D(Vertices[IndiceOfFixedPoints[i]]).ProjectOn(oPlane).Y;
}
Matrix <double> Ca = Matrix <double> .Build.DenseOfArray(MatrixCa);
Console.WriteLine(Ca);
Vector <double> R = Vector <double> .Build.DenseOfArray(VectorR);
Console.WriteLine(R);
Matrix <double> A = Matrix <double> .Build.DenseOfArray(MatrixA);
Console.WriteLine(A);
double Penalty = 100;
Matrix <double> Ak = A.Transpose() * A + Penalty * Ca.Transpose() * Ca;
Console.WriteLine(Ak);
Vector <double> X = Ak.Solve(Penalty * Ca.Transpose() * R);
X = Ak.Solve(Penalty * Ca.Transpose() * R);
Console.WriteLine(X);
bool valid = true;
if (!valid)
{
var solver = new MathNet.Numerics.LinearAlgebra.Double.Solvers.BiCgStab();
var preconditioner = new MathNet.Numerics.LinearAlgebra.Double.Solvers.MILU0Preconditioner();
var iterator = new MathNet.Numerics.LinearAlgebra.Solvers.Iterator <double>(
new MathNet.Numerics.LinearAlgebra.Solvers.ResidualStopCriterion <double>(1.0e-8),
new MathNet.Numerics.LinearAlgebra.Solvers.IterationCountStopCriterion <double>(1000));
var x = Ak.SolveIterative(Penalty * Ca.Transpose() * R, solver, iterator);
var status = iterator.Status;
Console.WriteLine(X);
}
return(X);
}