public static int CreateIdentityEquation(Matrix <float> A, Vector <float> C, CorrespondenceProblem problem, int row)
{
int matRow = row;
int[] srcTriangles = problem.srcMesh.TriangleList;
int numTris = problem.srcMesh.NumberOfTriangles;
int triangleIndex = 0;
var sqrtWt = Mathf.Sqrt(problem.wtIdentity);
for (triangleIndex = 0; triangleIndex < numTris; triangleIndex++)
{
//Create Identity vector.
float[] cI = { 1, 0, 0,
0, 1, 0,
0, 0, 1 };
Vector <float> cIdentity = Vector <float> .Build.DenseOfArray(cI);
// T - I
for (int iden = 0; iden < 9; iden++)
{
cIdentity[iden] += problem.srcMesh.matricesForTriangle[triangleIndex].C[iden];
}
// get A matrix for triangle.
var a = problem.srcMesh.matricesForTriangle[triangleIndex].A;
// Add to linear system.
Utils.UtilityFunctions.AddToLinearEquationSystem(A, C, a, cIdentity, problem, triangleIndex, matRow, sqrtWt);
matRow += 9;
}
return(matRow);
}
public void PopulateVertexInformationList(CorrespondenceProblem problem)
{
int freeVertCount = 0;
int constrainedVertCount = 0;
var srcMesh = problem.srcMesh;
VertexInformation = new VertexInfo[srcMesh.VertexList.Length];
for (int i = 0; i < srcMesh.VertexList.Length; i++)
{
if (problem.vertexConstraintList.ContainsKey(i))
{
//constrained.
VertexInformation[i] = new VertexInfo();
VertexInformation[i].isConstrained = true;
VertexInformation[i].MatrixPosition = constrainedVertCount;
constrainedVertCount++;
}
else
{
//Free.
VertexInformation[i] = new VertexInfo();
VertexInformation[i].isConstrained = false;
VertexInformation[i].MatrixPosition = freeVertCount;
freeVertCount++;
}
}
numberConstrained = constrainedVertCount;
numberFree = freeVertCount;
}
public void ResolveCorrespondence()
{
Problem = new CorrespondenceProblem();
// Source & target mesh .
//Vector3[] srcVerts = new Vector3[] { new Vector3(1, 0, 0), new Vector3(1, 1, 0), new Vector3(2, 1, 0), new Vector3(2, 0, 0), new Vector3(3, 1, 0) };
//Vector3[] tgtVerts = new Vector3[] { new Vector3(1, 0, 0), new Vector3(1, 1, 0), new Vector3(2, 1, 0), new Vector3(2, 0, 0), new Vector3(3, 0.5f, 0) };
//int[] tris = new int[] {0,1,3,
// 1,2,3,
// 2,4,3};
var srcMesh = srcGO.GetComponent <MeshFilter>().sharedMesh;
var tgtMesh = tgtGO.GetComponent <MeshFilter>().sharedMesh;
Problem.srcMesh = new MeshData();
Problem.tgtMesh = new MeshData();
Problem.srcMesh.VertexList = srcMesh.vertices; //srcVerts; // = this.SourceMesh;
Problem.srcMesh.TriangleList = srcMesh.triangles; //tris;
Problem.srcMesh.NormalList = srcMesh.normals;
Problem.tgtMesh.VertexList = tgtMesh.vertices; //tgtVerts; //= this.TargetMesh;
Problem.tgtMesh.TriangleList = tgtMesh.triangles; //tris;
Problem.tgtMesh.NormalList = tgtMesh.normals;
// Vertex Constraint List
Problem.vertexConstraintList = new Dictionary <int, int>();
Problem.vertexConstraintList.Add(0, 0);
Problem.vertexConstraintList.Add(1, 1);
Problem.vertexConstraintList.Add(4, 4);
Problem.wtSmoothness = 1.0f;
Problem.wtIdentity = 0.01f;
Problem.wtClosestStart = 1.0f;
Problem.wtClosestEnd = 5000.0f;
Problem.wtClosestStep = 1250f;
// Create Adjacency list for source model.
Problem.triAdj = UtilityFunctions.CreateAdjacencyList(Problem.srcMesh.NumberOfVertices, Problem.srcMesh.TriangleList);
Problem.Solve();
}
public static int CreateSmoothnessEquation(Matrix <float> A, Vector <float> C, CorrespondenceProblem problem, int row)
{
int matRow = row;
int[] srcTriangles = problem.srcMesh.TriangleList;
int numTris = problem.srcMesh.NumberOfTriangles;
int triangleIndex = 0;
var sqrtWt = Mathf.Sqrt(problem.wtSmoothness);
for (triangleIndex = 0; triangleIndex < numTris; triangleIndex++)
{
var adjTriangles = problem.triAdj.adjacencyList[triangleIndex];
foreach (var adjTriangle in adjTriangles)
{
//Debug.Log("Triangle Pair #1 :" + triangleIndex);
Utils.UtilityFunctions.AddToLinearEquationSystem(A, C, problem, triangleIndex, matRow, sqrtWt);
//Debug.Log("Triangle Pair #2 :" + adjTriangle);
Utils.UtilityFunctions.AddToLinearEquationSystem(A, C, problem, adjTriangle, matRow, -sqrtWt);
matRow += 9;
}
}
return(matRow);
}
public static int CreateClosestPointEquation(Matrix <float> A, Vector <float> C, int[] correspondenceMap, CorrespondenceProblem problem, int row, float wtClosest)
{
for (int i = 0; i < problem.srcMesh.NumberOfVertices; i++)
{
var vInfo = problem.vertexInfo.VertexInformation;
if (vInfo[i].isConstrained == false)
{
int iVx = problem.MatrixPositionForFreeVertex(i, 0);
int iVy = problem.MatrixPositionForFreeVertex(i, 1);
int iVz = problem.MatrixPositionForFreeVertex(i, 2);
int tgtIdx = correspondenceMap[i];
A[row, iVx] = wtClosest;
C[row] = wtClosest * problem.tgtMesh.VertexList[tgtIdx].x;
row++;
A[row, iVy] = wtClosest;
C[row] = wtClosest * problem.tgtMesh.VertexList[tgtIdx].y;
row++;
A[row, iVz] = wtClosest;
C[row] = wtClosest * problem.tgtMesh.VertexList[tgtIdx].z;
row++;
}
}
return(row);
}