public static SparseMatrixQuaternion operator -(SparseMatrixQuaternion left, SparseMatrixQuaternion right)
{
SparseMatrixQuaternion result = new SparseMatrixQuaternion(right.rowCount, right.columnCount);
foreach (KeyValuePair <Pair, Quaternion> item in left.mapData)
{
Pair pair = item.Key;
Quaternion value = item.Value;
result.mapData.Add(pair, value);
}
foreach (KeyValuePair <Pair, Quaternion> item in right.mapData)
{
Pair pair = item.Key;
Quaternion value = item.Value;
if (result.mapData.ContainsKey(pair))
{
Quaternion temp = result.mapData[pair] -= item.Value;
if (temp.Equals(Quaternion.Zero))
{
result.mapData.Remove(pair);
}
}
else
{
result.mapData.Add(pair, value);
}
}
return(result);
}
public void FactorizationLU(ref SparseMatrixQuaternion A)
{
CholmodInfo cholmodA = CholmodConverter.qConverter(ref A,
CholmodInfo.CholmodMatrixStorage.CCS);
m = A.RowCount;
n = A.ColumnCount;
fixed(int *Index = cholmodA.rowIndex, Pt = cholmodA.colIndex)
fixed(double *val = cholmodA.values)
{
solver = CreateSolverLUUMFPACK_CCS(cholmodA.RowCount,
cholmodA.ColumnCount,
cholmodA.nnz,
Index,
Pt,
val
);
}
if (solver == null)
{
throw new Exception("Create Solver Fail");
}
}
SparseMatrixQuaternion BuildCotLaplace()
{
SparseMatrixQuaternion Laplace = new SparseMatrixQuaternion(mesh.Vertices.Count, mesh.Vertices.Count);
foreach (TriMesh.Face face in mesh.Faces)
{
foreach (TriMesh.HalfEdge hf in face.Halfedges)
{
Vector3D a = hf.FromVertex.Traits.Position;
Vector3D b = hf.Next.FromVertex.Traits.Position;
Vector3D c = hf.Next.Next.FromVertex.Traits.Position;
int aIndex = hf.FromVertex.Index;
int bIndex = hf.Next.FromVertex.Index;
int cIndex = hf.Next.Next.FromVertex.Index;
Vector3D u1 = b - a;
Vector3D u2 = c - a;
double cotAlpha = u1.Dot(u2) / u1.Cross(u2).Length();
Laplace[bIndex, cIndex] -= cotAlpha / 2;
Laplace[cIndex, bIndex] -= cotAlpha / 2;
Laplace[cIndex, cIndex] += cotAlpha / 2;
Laplace[bIndex, bIndex] += cotAlpha / 2;
}
}
return(Laplace);
}
public static CholmodInfo qConverter(ref SparseMatrixQuaternion A, CholmodInfo.CholmodMatrixStorage storage)
{
CholmodInfo info = new CholmodInfo();
info.MatrixType = CholmodInfo.CholmodMatrixType.Sparse;
info.MatrixStorageType = storage;
info.MatrixStorageMethod = CholmodInfo.CholmodMatrixStoageMethod.Normal;
info.MatrixItemType = CholmodInfo.CholmodMatrixItemType.Quaternion;
info.RowCount = 4 * A.RowCount;
info.ColumnCount = 4 * A.ColumnCount;
switch (storage)
{
case CholmodInfo.CholmodMatrixStorage.CRS:
A.ToCRS(out info.colIndex, out info.rowIndex, out info.values, out info.nnz);
break;
case CholmodInfo.CholmodMatrixStorage.CCS:
A.ToCCS(out info.colIndex, out info.rowIndex, out info.values, out info.nnz);
break;
case CholmodInfo.CholmodMatrixStorage.Triplet:
A.ToTriplet(out info.colIndex, out info.rowIndex, out info.values, out info.nnz);
break;
default:
A.ToTriplet(out info.colIndex, out info.rowIndex, out info.values, out info.nnz);
break;
}
return(info);
}
public static SparseMatrixQuaternion Copy(ref SparseMatrixQuaternion B)
{
Dictionary <Pair, Quaternion> newData = new Dictionary <Pair, Quaternion>(B.mapData);
SparseMatrixQuaternion newMatrix = new SparseMatrixQuaternion(B.rowCount, B.columnCount);
newMatrix.mapData = newData;
return(newMatrix);
}
public SparseMatrixQuaternion SubMatrix(int rowStart, int rowEnd, int columnStart, int columnEnd)
{
if (rowEnd < rowStart || columnEnd < columnStart)
{
throw new ArgumentOutOfRangeException();
}
int rows = rowEnd - rowStart + 1;
int columns = columnEnd - columnStart + 1;
SparseMatrixQuaternion subMatrix = new SparseMatrixQuaternion(rows, columns);
int subMatrixEntiries = rows * columns;
if (subMatrixEntiries < mapData.Count)
{
for (int i = 0; i < rows; i++)
{
int rowInx = i + rowStart;
for (int j = 0; j < columns; j++)
{
int columnInx = j + columnStart;
Pair pair = new Pair(rowInx, columnInx);
if (!mapData.ContainsKey(pair))
{
continue;
}
Pair subPair = new Pair(i, j);
subMatrix.mapData[subPair] = mapData[pair];
}
}
}
else if (subMatrixEntiries >= mapData.Count)
{
foreach (KeyValuePair <Pair, Quaternion> item in mapData)
{
Pair pair = item.Key;
Quaternion value = item.Value;
int m = pair.Key;
int n = pair.Value;
if ((m >= rowStart && m <= rowEnd) && (n >= columnStart && m <= columnEnd))
{
int i = m - rowStart;
int j = n - columnStart;
subMatrix.mapData[new Pair(i, j)] = value;
}
}
}
return(subMatrix);
}
public static SparseMatrixQuaternion Identity(int N)
{
SparseMatrixQuaternion identity = new SparseMatrixQuaternion(N, N);
for (int i = 0; i < N; i++)
{
identity.mapData.Add(new Pair(i, i), Quaternion.Identity);
}
return(identity);
}
public SparseMatrixQuaternion Transpose()
{
SparseMatrixQuaternion trMatrix = new SparseMatrixQuaternion(this.columnCount, this.rowCount);
foreach (KeyValuePair <Pair, Quaternion> item in mapData)
{
Pair pair = item.Key;
Quaternion value = item.Value;
trMatrix.mapData[new Pair(pair.Value, pair.Key)] = value;
}
return(trMatrix);
}
public static SparseMatrixQuaternion operator *(double left, SparseMatrixQuaternion right)
{
SparseMatrixQuaternion result = new SparseMatrixQuaternion(right.rowCount, right.columnCount);
foreach (KeyValuePair <Pair, Quaternion> item in right.mapData)
{
Pair pair = item.Key;
Quaternion value = left * item.Value;
result.mapData.Add(pair, value);
}
return(result);
}
public static SparseMatrixQuaternion operator *(double left, SparseMatrixQuaternion right)
{
SparseMatrixQuaternion result = new SparseMatrixQuaternion(right.rowCount, right.columnCount);
foreach (KeyValuePair<Pair, Quaternion> item in right.mapData)
{
Pair pair = item.Key;
Quaternion value = left * item.Value;
result.mapData.Add(pair, value);
}
return result;
}
public void UpdateDeformation(String imagePath, double scale)
{
TGAImage image = new TGAImage(imagePath);
//double[] rho = IOHuiZhao.Instance.ReadVectorFromMatlab("A.vector");
double[] rho = SetCurvatureChange(image, 10);
SparseMatrixQuaternion E = BuildEigenValueProblem(rho);
DenseMatrixQuaternion lamda = LinearSystemDEC.Instance.SolveEigen(ref E);
SparseMatrixQuaternion Laplace = BuildCotLaplace();
DenseMatrixQuaternion omega = BuildOmega(lamda);
LinearSystemGenericByLib.Instance.FactorizationLU(ref Laplace);
DenseMatrixQuaternion newV = LinearSystemGenericByLib.Instance.SolveByFactorizedLU(ref omega);
NormalizeSolution(ref newV);
UpdatePosition(ref newV);
}
public DenseMatrixQuaternion SolveEigen(ref SparseMatrixQuaternion A)
{
LinearSystemGenericByLib.Instance.FactorizationLU(ref A);
DenseMatrixQuaternion b = new DenseMatrixQuaternion(A.ColumnCount, 1);
b.Fill(Quaternion.Identity);
for (int i = 0; i < 20; i++)
{
b.Normalize();
DenseMatrixQuaternion x = LinearSystemGenericByLib.Instance.SolveByFactorizedLU(ref b);
b = null;
b = x;
}
GC.Collect();
b.Normalize();
LinearSystemGenericByLib.Instance.FreeSolverLU();
return(b);
}
SparseMatrixQuaternion BuildEigenValueProblem(double[] rho)
{
int nV = mesh.Vertices.Count;
SparseMatrixQuaternion E = new SparseMatrixQuaternion(nV, nV);
int[] Indices = new int[3];
foreach (TriMesh.Face face in mesh.Faces)
{
double A = TriMeshUtil.ComputeAreaFace(face);
double a = -1 / (4 * A);
double b = rho[face.Index] / 6;
double c = A * rho[face.Index] * rho[face.Index] / 9;
Indices[0] = face.GetVertex(0).Index;
Indices[1] = face.GetVertex(1).Index;
Indices[2] = face.GetVertex(2).Index;
Quaternion[] e = new Quaternion[3];
for (int i = 0; i < 3; i++)
{
Vector3D eV = mesh.Vertices[Indices[(i + 2) % 3]].Traits.Position -
mesh.Vertices[Indices[(i + 1) % 3]].Traits.Position;
e[i] = new Quaternion(eV, 0);
}
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
Quaternion q = a * e[i] * e[j] + b * (e[j] - e[i]) + c;
E[Indices[i], Indices[j]] += q;
}
}
}
return E;
}
SparseMatrixQuaternion BuildEigenValueProblem(double[] rho)
{
int nV = mesh.Vertices.Count;
SparseMatrixQuaternion E = new SparseMatrixQuaternion(nV, nV);
int[] Indices = new int[3];
foreach (TriMesh.Face face in mesh.Faces)
{
double A = TriMeshUtil.ComputeAreaFace(face);
double a = -1 / (4 * A);
double b = rho[face.Index] / 6;
double c = A * rho[face.Index] * rho[face.Index] / 9;
Indices[0] = face.GetVertex(0).Index;
Indices[1] = face.GetVertex(1).Index;
Indices[2] = face.GetVertex(2).Index;
Quaternion[] e = new Quaternion[3];
for (int i = 0; i < 3; i++)
{
Vector3D eV = mesh.Vertices[Indices[(i + 2) % 3]].Traits.Position -
mesh.Vertices[Indices[(i + 1) % 3]].Traits.Position;
e[i] = new Quaternion(eV, 0);
}
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
Quaternion q = a * e[i] * e[j] + b * (e[j] - e[i]) + c;
E[Indices[i], Indices[j]] += q;
}
}
}
return(E);
}
public static SparseMatrixQuaternion Copy(ref SparseMatrixQuaternion B)
{
Dictionary<Pair, Quaternion> newData = new Dictionary<Pair, Quaternion>(B.mapData);
SparseMatrixQuaternion newMatrix = new SparseMatrixQuaternion(B.rowCount, B.columnCount);
newMatrix.mapData = newData;
return newMatrix;
}
public static SparseMatrixQuaternion Identity(int N)
{
SparseMatrixQuaternion identity = new SparseMatrixQuaternion(N, N);
for (int i = 0; i < N; i++)
{
identity.mapData.Add(new Pair(i, i), Quaternion.Identity);
}
return identity;
}
public SparseMatrixQuaternion(SparseMatrixQuaternion matrix)
{
mapData = new Dictionary<Pair, Quaternion>(matrix.mapData);
this.rowCount = matrix.rowCount;
this.columnCount = matrix.columnCount;
}
public SparseMatrixQuaternion SubMatrix(int rowStart, int rowEnd, int columnStart, int columnEnd)
{
if (rowEnd < rowStart || columnEnd < columnStart)
{
throw new ArgumentOutOfRangeException();
}
int rows = rowEnd - rowStart + 1;
int columns = columnEnd - columnStart + 1;
SparseMatrixQuaternion subMatrix = new SparseMatrixQuaternion(rows, columns);
int subMatrixEntiries = rows * columns;
if (subMatrixEntiries < mapData.Count)
{
for (int i = 0; i < rows; i++)
{
int rowInx = i + rowStart;
for (int j = 0; j < columns; j++)
{
int columnInx = j + columnStart;
Pair pair = new Pair(rowInx, columnInx);
if (!mapData.ContainsKey(pair))
{
continue;
}
Pair subPair = new Pair(i, j);
subMatrix.mapData[subPair] = mapData[pair];
}
}
}
else if (subMatrixEntiries >= mapData.Count)
{
foreach (KeyValuePair<Pair, Quaternion> item in mapData)
{
Pair pair = item.Key;
Quaternion value = item.Value;
int m = pair.Key;
int n = pair.Value;
if ((m >= rowStart && m <= rowEnd) && (n >= columnStart && m <= columnEnd))
{
int i = m - rowStart;
int j = n - columnStart;
subMatrix.mapData[new Pair(i, j)] = value;
}
}
}
return subMatrix;
}
public SparseMatrixQuaternion Transpose()
{
SparseMatrixQuaternion trMatrix = new SparseMatrixQuaternion(this.columnCount, this.rowCount);
foreach (KeyValuePair<Pair, Quaternion> item in mapData)
{
Pair pair = item.Key;
Quaternion value = item.Value;
trMatrix.mapData[new Pair(pair.Value, pair.Key)] = value;
}
return trMatrix;
}
public static SparseMatrixQuaternion operator -(SparseMatrixQuaternion left, SparseMatrixQuaternion right)
{
SparseMatrixQuaternion result = new SparseMatrixQuaternion(right.rowCount, right.columnCount);
foreach (KeyValuePair<Pair, Quaternion> item in left.mapData)
{
Pair pair = item.Key;
Quaternion value = item.Value;
result.mapData.Add(pair, value);
}
foreach (KeyValuePair<Pair, Quaternion> item in right.mapData)
{
Pair pair = item.Key;
Quaternion value = item.Value;
if (result.mapData.ContainsKey(pair))
{
Quaternion temp = result.mapData[pair] -= item.Value;
if (temp.Equals(Quaternion.Zero))
{
result.mapData.Remove(pair);
}
}
else
{
result.mapData.Add(pair, value);
}
}
return result;
}
public SparseMatrixQuaternion(SparseMatrixQuaternion matrix)
{
mapData = new Dictionary <Pair, Quaternion>(matrix.mapData);
this.rowCount = matrix.rowCount;
this.columnCount = matrix.columnCount;
}
public static SparseMatrixQuaternion operator *(SparseMatrixQuaternion left, SparseMatrixQuaternion right)
{
//Make sure matrix dimensions are equal
if (left.columnCount != right.rowCount)
{
throw new Exception("The dimension of two matrix must be equal");
}
SparseMatrixQuaternion result = new SparseMatrixQuaternion(left.rowCount, right.columnCount);
int leftNNZ = left.mapData.Count;
int rightNNZ = right.mapData.Count;
#region Left < Right
//We use right as stardand sight
//if (leftNNZ < rightNNZ)
//{
//Connection nonezero for each row of matrix a
List <KeyValuePair <int, Quaternion> >[] bRows = new List <KeyValuePair <int, Quaternion> > [right.rowCount];
for (int i = 0; i < bRows.Length; i++)
{
bRows[i] = new List <KeyValuePair <int, Quaternion> >();
}
foreach (KeyValuePair <Pair, Quaternion> item in right.mapData)
{
Pair pair = item.Key;
Quaternion value = item.Value;
bRows[pair.Key].Add(new KeyValuePair <int, Quaternion>(pair.Value, value));
}
//Compute C = A*B
foreach (KeyValuePair <Pair, Quaternion> item in left.mapData)
{
Pair pair = item.Key;
int mA = pair.Key;
int nA = pair.Value;
Quaternion value = item.Value;
List <KeyValuePair <int, Quaternion> > bRow = bRows[nA];
for (int i = 0; i < bRow.Count; i++)
{
int k = bRow[i].Key;
Pair pair2 = new Pair(mA, k);
if (result.mapData.ContainsKey(pair2))
{
result.mapData[pair2] += value * bRow[i].Value;
}
else
{
result.mapData.Add(pair2, value * bRow[i].Value);
}
}
}
//}
#endregion
#region Right < Left
//else if (leftNNZ > rightNNZ)
//{
// //Connection nonezero for each row of matrix a
// List<KeyValuePair<int, double>>[] aCols = new List<KeyValuePair<int, double>>[left.columnCount];
// for (int i = 0; i < aCols.Length; i++)
// {
// aCols[i] = new List<KeyValuePair<int, double>>();
// }
// foreach (KeyValuePair<Pair, double> item in left.mapData)
// {
// Pair pair = item.Key;
// double value = item.Value;
// aCols[pair.Value].Add(new KeyValuePair<int, double>(pair.Key, value));
// }
// //Compute C = A*B
// foreach (KeyValuePair<Pair, double> item in right.mapData)
// {
// Pair pair = item.Key;
// int mA = pair.Key;
// int nA = pair.Value;
// double value = item.Value;
// List<KeyValuePair<int, double>> aCol = aCols[mA];
// for (int i = 0; i < aCol.Count; i++)
// {
// int k = aCol[i].Key;
// Pair pair2 = new Pair(k, nA);
// if (result.mapData.ContainsKey(pair2))
// {
// result.mapData[pair2] += value * aCol[i].Value;
// }
// else
// {
// result.mapData.Add(pair2, value * aCol[i].Value);
// }
// }
// }
//}
#endregion
return(result);
}
public static CholmodInfo qConverter(ref SparseMatrixQuaternion A, CholmodInfo.CholmodMatrixStorage storage)
{
CholmodInfo info = new CholmodInfo();
info.MatrixType = CholmodInfo.CholmodMatrixType.Sparse;
info.MatrixStorageType = storage;
info.MatrixStorageMethod = CholmodInfo.CholmodMatrixStoageMethod.Normal;
info.MatrixItemType = CholmodInfo.CholmodMatrixItemType.Quaternion;
info.RowCount = 4 * A.RowCount;
info.ColumnCount = 4 * A.ColumnCount;
switch (storage)
{
case CholmodInfo.CholmodMatrixStorage.CRS:
A.ToCRS(out info.colIndex, out info.rowIndex, out info.values, out info.nnz);
break;
case CholmodInfo.CholmodMatrixStorage.CCS:
A.ToCCS(out info.colIndex, out info.rowIndex, out info.values, out info.nnz);
break;
case CholmodInfo.CholmodMatrixStorage.Triplet:
A.ToTriplet(out info.colIndex, out info.rowIndex, out info.values, out info.nnz);
break;
default:
A.ToTriplet(out info.colIndex, out info.rowIndex, out info.values, out info.nnz);
break;
}
return info;
}
SparseMatrixQuaternion BuildCotLaplace()
{
SparseMatrixQuaternion Laplace = new SparseMatrixQuaternion(mesh.Vertices.Count, mesh.Vertices.Count);
foreach (TriMesh.Face face in mesh.Faces)
{
foreach (TriMesh.HalfEdge hf in face.Halfedges)
{
Vector3D a = hf.FromVertex.Traits.Position;
Vector3D b = hf.Next.FromVertex.Traits.Position;
Vector3D c = hf.Next.Next.FromVertex.Traits.Position;
int aIndex = hf.FromVertex.Index;
int bIndex = hf.Next.FromVertex.Index;
int cIndex = hf.Next.Next.FromVertex.Index;
Vector3D u1 = b - a;
Vector3D u2 = c - a;
double cotAlpha = u1.Dot(u2) / u1.Cross(u2).Length();
Laplace[bIndex, cIndex] -= cotAlpha / 2;
Laplace[cIndex, bIndex] -= cotAlpha / 2;
Laplace[cIndex, cIndex] += cotAlpha / 2;
Laplace[bIndex, bIndex] += cotAlpha / 2;
}
}
return Laplace;
}
public static SparseMatrixQuaternion operator *(SparseMatrixQuaternion left, SparseMatrixQuaternion right)
{
//Make sure matrix dimensions are equal
if (left.columnCount != right.rowCount)
{
throw new Exception("The dimension of two matrix must be equal");
}
SparseMatrixQuaternion result = new SparseMatrixQuaternion(left.rowCount, right.columnCount);
int leftNNZ = left.mapData.Count;
int rightNNZ = right.mapData.Count;
#region Left < Right
//We use right as stardand sight
//if (leftNNZ < rightNNZ)
//{
//Connection nonezero for each row of matrix a
List<KeyValuePair<int, Quaternion>>[] bRows = new List<KeyValuePair<int, Quaternion>>[right.rowCount];
for (int i = 0; i < bRows.Length; i++)
{
bRows[i] = new List<KeyValuePair<int, Quaternion>>();
}
foreach (KeyValuePair<Pair, Quaternion> item in right.mapData)
{
Pair pair = item.Key;
Quaternion value = item.Value;
bRows[pair.Key].Add(new KeyValuePair<int, Quaternion>(pair.Value, value));
}
//Compute C = A*B
foreach (KeyValuePair<Pair, Quaternion> item in left.mapData)
{
Pair pair = item.Key;
int mA = pair.Key;
int nA = pair.Value;
Quaternion value = item.Value;
List<KeyValuePair<int, Quaternion>> bRow = bRows[nA];
for (int i = 0; i < bRow.Count; i++)
{
int k = bRow[i].Key;
Pair pair2 = new Pair(mA, k);
if (result.mapData.ContainsKey(pair2))
{
result.mapData[pair2] += value * bRow[i].Value;
}
else
{
result.mapData.Add(pair2, value * bRow[i].Value);
}
}
}
//}
#endregion
#region Right < Left
//else if (leftNNZ > rightNNZ)
//{
// //Connection nonezero for each row of matrix a
// List<KeyValuePair<int, double>>[] aCols = new List<KeyValuePair<int, double>>[left.columnCount];
// for (int i = 0; i < aCols.Length; i++)
// {
// aCols[i] = new List<KeyValuePair<int, double>>();
// }
// foreach (KeyValuePair<Pair, double> item in left.mapData)
// {
// Pair pair = item.Key;
// double value = item.Value;
// aCols[pair.Value].Add(new KeyValuePair<int, double>(pair.Key, value));
// }
// //Compute C = A*B
// foreach (KeyValuePair<Pair, double> item in right.mapData)
// {
// Pair pair = item.Key;
// int mA = pair.Key;
// int nA = pair.Value;
// double value = item.Value;
// List<KeyValuePair<int, double>> aCol = aCols[mA];
// for (int i = 0; i < aCol.Count; i++)
// {
// int k = aCol[i].Key;
// Pair pair2 = new Pair(k, nA);
// if (result.mapData.ContainsKey(pair2))
// {
// result.mapData[pair2] += value * aCol[i].Value;
// }
// else
// {
// result.mapData.Add(pair2, value * aCol[i].Value);
// }
// }
// }
//}
#endregion
return result;
}
public void FactorizationLU(ref SparseMatrixQuaternion A)
{
CholmodInfo cholmodA = CholmodConverter.qConverter(ref A,
CholmodInfo.CholmodMatrixStorage.CCS);
m = A.RowCount;
n = A.ColumnCount;
fixed (int* Index = cholmodA.rowIndex, Pt = cholmodA.colIndex)
fixed (double* val = cholmodA.values)
{
solver = CreateSolverLUUMFPACK_CCS(cholmodA.RowCount,
cholmodA.ColumnCount,
cholmodA.nnz,
Index,
Pt,
val
);
}
if (solver == null) throw new Exception("Create Solver Fail");
}