public static DenseMatrixQuaternion operator *(DenseMatrixQuaternion 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");
}
DenseMatrixQuaternion resultMatrix = new DenseMatrixQuaternion(left.RowCount, right.ColumnCount);
Quaternion[,] result = resultMatrix.datas;
for (int i = 0; i < left.rowCount; i++)
{
foreach (KeyValuePair <Pair, Quaternion> item in right.Datas)
{
Pair pair = item.Key;
Quaternion value = item.Value;
int m = pair.Key;
int n = pair.Value;
//M: mutiply index N: vector store index
result[i, n] += left[i, m] * value;
}
}
return(resultMatrix);
}
DenseMatrixQuaternion ReadLambda(String path)
{
FileStream fs = new FileStream(path, FileMode.Open);
StreamReader sr = new StreamReader(fs);
String line = null;
DenseMatrixQuaternion qt = new DenseMatrixQuaternion(mesh.Vertices.Count, 1);
char[] splitChar = new char[] { ' ' };
int row = 0;
while ((line = sr.ReadLine()) != null)
{
String[] token = line.Split(splitChar);
double w = double.Parse(token[0]);
double x = double.Parse(token[1]);
double y = double.Parse(token[2]);
double z = double.Parse(token[3]);
Quaternion q = new Quaternion(x, y, z, w);
qt[row, 0] = q;
row++;
}
return(qt);
}
public Quaternion Dot(DenseMatrixQuaternion right)
{
DenseMatrixQuaternion x = this.Transpose() * right;
Quaternion result = x[0, 0];
return(result);
}
void UpdatePosition(ref DenseMatrixQuaternion newVertices)
{
for (int i = 0; i < newVertices.RowCount; i++)
{
Quaternion v = newVertices[i, 0];
Vector3D newPosition = new Vector3D(v.x, v.y, v.z);
mesh.Vertices[i].Traits.Position = newPosition;
}
}
public static DenseMatrixQuaternion Copy(ref DenseMatrixQuaternion B)
{
DenseMatrixQuaternion newMatrix = new DenseMatrixQuaternion();
newMatrix.rowCount = B.rowCount;
newMatrix.columnCount = B.columnCount;
Array.Copy(B.datas, newMatrix.datas, B.datas.Length);
return(newMatrix);
}
public static DenseMatrixQuaternion Identity(int N)
{
DenseMatrixQuaternion identity = new DenseMatrixQuaternion(N, N);
for (int i = 0; i < N; i++)
{
identity.datas[i, i] = Quaternion.Identity;
}
return identity;
}
public static DenseMatrixQuaternion Identity(int N)
{
DenseMatrixQuaternion identity = new DenseMatrixQuaternion(N, N);
for (int i = 0; i < N; i++)
{
identity.datas[i, i] = Quaternion.Identity;
}
return(identity);
}
DenseMatrixQuaternion BuildOmega(DenseMatrixQuaternion lamda)
{
DenseMatrixQuaternion Omega = new DenseMatrixQuaternion(mesh.Vertices.Count, 1);
TriMesh.Vertex[] index = new TriMesh.Vertex[3];
foreach (TriMesh.Face face in mesh.Faces)
{
index[0] = face.GetVertex(2);
index[1] = face.GetVertex(0);
index[2] = face.GetVertex(1);
for (int i = 0; i < 3; i++)
{
Quaternion f0 = new Quaternion(index[(i + 0) % 3].Traits.Position, 0);
Quaternion f1 = new Quaternion(index[(i + 1) % 3].Traits.Position, 0);
Quaternion f2 = new Quaternion(index[(i + 2) % 3].Traits.Position, 0);
//Setting Orientation Swap it
TriMesh.Vertex aI = index[(i + 1) % 3];
TriMesh.Vertex bI = index[(i + 2) % 3];
if (aI.Index > bI.Index)
{
aI = index[(i + 2) % 3];
bI = index[(i + 1) % 3];
}
Quaternion aLamda = lamda[aI.Index, 0];
Quaternion bLamda = lamda[bI.Index, 0];
Quaternion e = new Quaternion(bI.Traits.Position, 0) - new Quaternion(aI.Traits.Position, 0);
Quaternion conj = aLamda.Conjugate();
Quaternion bconj = bLamda.Conjugate();
Quaternion partA = conj * e * aLamda;
Quaternion eTilde = (1f / 3f) * partA +
(1f / 6f) * aLamda.Conjugate() * e * bLamda +
(1f / 6f) * bLamda.Conjugate() * e * aLamda +
(1f / 3f) * bLamda.Conjugate() * e * bLamda;
Vector3D u1 = index[(i + 1) % 3].Traits.Position - index[(i + 0) % 3].Traits.Position;
Vector3D u2 = index[(i + 2) % 3].Traits.Position - index[(i + 0) % 3].Traits.Position;
double cotAlpha = u1.Dot(u2) / u1.Cross(u2).Length();
Quaternion q = cotAlpha * eTilde / 2;
Omega[aI.Index, 0] -= q;
Omega[bI.Index, 0] += q;
}
}
RemoveMean(ref Omega);
return(Omega);
}
public double F_Norm()
{
DenseMatrixQuaternion tr = this.Transpose() * this;
double sum = 0;
for (int i = 0; i < tr.RowCount; i++)
{
sum += tr[i, i].Length;
}
return(Math.Sqrt(sum));
}
public static DenseMatrixQuaternion operator *(Quaternion left, DenseMatrixQuaternion right)
{
DenseMatrixQuaternion result = new DenseMatrixQuaternion(right.rowCount, right.columnCount);
for (int i = 0; i < right.rowCount; i++)
{
for (int j = 0; j < right.columnCount; j++)
{
result.datas[i, j] = right.datas[i, j] * left;
}
}
return(result);
}
public DenseMatrixQuaternion Transpose()
{
DenseMatrixQuaternion tr = new DenseMatrixQuaternion(this.columnCount, this.rowCount);
for (int i = 0; i < rowCount; i++)
{
for (int j = 0; j < columnCount; j++)
{
tr.datas[j, i] = datas[i, j];
}
}
return(tr);
}
public DenseMatrixQuaternion Transpose()
{
DenseMatrixQuaternion tr = new DenseMatrixQuaternion(this.columnCount, this.rowCount);
for (int i = 0; i < rowCount; i++)
{
for (int j = 0; j < columnCount; j++)
{
tr.datas[j, i] = datas[i, j];
}
}
return tr;
}
public DenseMatrixQuaternion SolveByFactorizedLU(ref DenseMatrixQuaternion b)
{
if (b.ColumnCount > 1)
{
return(null);
}
double[][] rhs = new double[4][]; //Arranage By Column
for (int i = 0; i < 4; i++)
{
rhs[i] = new double[4 * b.RowCount];
}
double[][] x = new double[4][];
for (int i = 0; i < 4; i++)
{
x[i] = new double[4 * this.n];
}
for (int i = 0; i < b.RowCount; i++)
{
Quaternion item = b[i, 0];
Matrix4D mat = item.ToMatrix();
rhs[0][4 * i] = mat[0, 0]; rhs[1][4 * i] = mat[0, 1]; rhs[2][4 * i] = mat[0, 2]; rhs[3][4 * i] = mat[0, 3];
rhs[0][4 * i + 1] = mat[1, 0]; rhs[1][4 * i + 1] = mat[1, 1]; rhs[2][4 * i + 1] = mat[1, 2]; rhs[3][4 * i + 1] = mat[1, 3];
rhs[0][4 * i + 2] = mat[2, 0]; rhs[1][4 * i + 2] = mat[2, 1]; rhs[2][4 * i + 2] = mat[2, 2]; rhs[3][4 * i + 2] = mat[2, 3];
rhs[0][4 * i + 3] = mat[3, 0]; rhs[1][4 * i + 3] = mat[3, 1]; rhs[2][4 * i + 3] = mat[3, 2]; rhs[3][4 * i + 3] = mat[3, 3];
}
DenseMatrixQuaternion result = new DenseMatrixQuaternion(n, 1);
//Solve
for (int i = 0; i < 4; i++)
{
SolveLU(ref rhs[i], ref x[i]);
}
for (int i = 0; i < this.n; i++)
{
result[i, 0] = new Quaternion(x[0][4 * i + 1], x[0][4 * i + 2], x[0][4 * i + 3], x[0][4 * i]);
}
x = null;
GC.Collect();
return(result);
}
void RemoveMean(ref DenseMatrixQuaternion x)
{
Quaternion mean = Quaternion.Zero;
for (int i = 0; i < x.RowCount; i++)
{
mean += x[i, 0];
}
mean /= x.RowCount;
for (int i = 0; i < x.RowCount; i++)
{
x[i, 0] -= mean;
}
}
public static DenseMatrixQuaternion operator /(DenseMatrixQuaternion left, Quaternion right)
{
//Make sure matrix dimensions are equal
DenseMatrixQuaternion matrix = new DenseMatrixQuaternion(left.RowCount, left.ColumnCount);
// Quaternion rightInv = right.Invert();
for (int i = 0; i < left.RowCount; i++)
{
for (int j = 0; j < left.columnCount; j++)
{
// matrix.datas[i, j] = left.datas[i, j] * rightInv;
}
}
return(matrix);
}
void NormalizeSolution(ref DenseMatrixQuaternion newVertices)
{
RemoveMean(ref newVertices);
// Infinite Norm
double r = 0;
for (int i = 0; i < newVertices.RowCount; i++)
{
r = Math.Max(r, newVertices[i, 0].LengthSquared);
}
r = Math.Sqrt(r);
for (int i = 0; i < newVertices.RowCount; i++)
{
newVertices[i, 0] /= r;
}
}
public static DenseMatrixQuaternion operator +(DenseMatrixQuaternion left, DenseMatrixQuaternion right)
{
//Make sure matrix dimensions are equal
if (left.columnCount != right.columnCount)
{
throw new Exception("The dimension of two matrix must be equal");
}
DenseMatrixQuaternion matrix = new DenseMatrixQuaternion(left.RowCount, left.ColumnCount);
for (int i = 0; i < left.RowCount; i++)
{
for (int j = 0; j < left.columnCount; j++)
{
matrix.datas[i, j] = left.datas[i, j] + right.datas[i, j];
}
}
return(matrix);
}
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);
}
public static DenseMatrixQuaternion operator *(DenseMatrixQuaternion left, DenseMatrixQuaternion right)
{
//Make sure matrix dimensions are equal
if (left.columnCount != right.rowCount)
{
throw new Exception("The dimension of two matrix must be equal");
}
DenseMatrixQuaternion result = new DenseMatrixQuaternion(left.rowCount, right.columnCount);
for (int i = 0; i < left.rowCount; i++)
{
for (int j = 0; j < right.columnCount; j++)
{
for (int z = 0; z < left.columnCount; z++)
{
result.datas[i, j] += left.datas[i, z] * right.datas[z, j];
}
}
}
return(result);
}
public static DenseMatrixQuaternion operator +(DenseMatrixQuaternion left, DenseMatrixQuaternion right)
{
//Make sure matrix dimensions are equal
if (left.columnCount != right.columnCount)
{
throw new Exception("The dimension of two matrix must be equal");
}
DenseMatrixQuaternion matrix = new DenseMatrixQuaternion(left.RowCount, left.ColumnCount);
for (int i = 0; i < left.RowCount; i++)
{
for (int j = 0; j < left.columnCount; j++)
{
matrix.datas[i, j] = left.datas[i, j] + right.datas[i, j];
}
}
return matrix;
}
public static DenseMatrixQuaternion operator *(DenseMatrixQuaternion left, DenseMatrixQuaternion right)
{
//Make sure matrix dimensions are equal
if (left.columnCount != right.rowCount)
{
throw new Exception("The dimension of two matrix must be equal");
}
DenseMatrixQuaternion result = new DenseMatrixQuaternion(left.rowCount, right.columnCount);
for (int i = 0; i < left.rowCount; i++)
{
for (int j = 0; j < right.columnCount; j++)
{
for (int z = 0; z < left.columnCount; z++)
{
result.datas[i, j] += left.datas[i, z] * right.datas[z, j];
}
}
}
return result;
}
public static DenseMatrixQuaternion operator /(DenseMatrixQuaternion left, Quaternion right)
{
//Make sure matrix dimensions are equal
DenseMatrixQuaternion matrix = new DenseMatrixQuaternion(left.RowCount, left.ColumnCount);
// Quaternion rightInv = right.Invert();
for (int i = 0; i < left.RowCount; i++)
{
for (int j = 0; j < left.columnCount; j++)
{
// matrix.datas[i, j] = left.datas[i, j] * rightInv;
}
}
return matrix;
}
public static DenseMatrixQuaternion operator *(DenseMatrixQuaternion 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");
}
DenseMatrixQuaternion resultMatrix = new DenseMatrixQuaternion(left.RowCount, right.ColumnCount);
Quaternion[,] result = resultMatrix.datas;
for (int i = 0; i < left.rowCount; i++)
{
foreach (KeyValuePair<Pair, Quaternion> item in right.Datas)
{
Pair pair = item.Key;
Quaternion value = item.Value;
int m = pair.Key;
int n = pair.Value;
//M: mutiply index N: vector store index
result[i, n] += left[i, m] * value;
}
}
return resultMatrix;
}
void NormalizeSolution(ref DenseMatrixQuaternion newVertices)
{
RemoveMean(ref newVertices);
// Infinite Norm
double r = 0;
for (int i = 0; i < newVertices.RowCount; i++)
{
r = Math.Max(r, newVertices[i, 0].LengthSquared);
}
r = Math.Sqrt(r);
for (int i = 0; i < newVertices.RowCount; i++)
{
newVertices[i, 0] /= r;
}
}
void UpdatePosition(ref DenseMatrixQuaternion newVertices)
{
for (int i = 0; i < newVertices.RowCount; i++)
{
Quaternion v = newVertices[i, 0];
Vector3D newPosition = new Vector3D(v.x, v.y, v.z);
mesh.Vertices[i].Traits.Position = newPosition;
}
}
DenseMatrixQuaternion ReadLambda(String path)
{
FileStream fs = new FileStream(path, FileMode.Open);
StreamReader sr = new StreamReader(fs);
String line = null;
DenseMatrixQuaternion qt = new DenseMatrixQuaternion(mesh.Vertices.Count, 1);
char[] splitChar = new char[] { ' ' };
int row = 0;
while ((line = sr.ReadLine()) != null)
{
String[] token = line.Split(splitChar);
double w = double.Parse(token[0]);
double x = double.Parse(token[1]);
double y = double.Parse(token[2]);
double z = double.Parse(token[3]);
Quaternion q = new Quaternion(x, y, z, w);
qt[row, 0] = q;
row++;
}
return qt;
}
void RemoveMean(ref DenseMatrixQuaternion x)
{
Quaternion mean = Quaternion.Zero;
for (int i = 0; i < x.RowCount; i++)
{
mean += x[i, 0];
}
mean /= x.RowCount;
for (int i = 0; i < x.RowCount; i++)
{
x[i, 0] -= mean;
}
}
DenseMatrixQuaternion BuildOmega(DenseMatrixQuaternion lamda)
{
DenseMatrixQuaternion Omega = new DenseMatrixQuaternion(mesh.Vertices.Count, 1);
TriMesh.Vertex[] index = new TriMesh.Vertex[3];
foreach (TriMesh.Face face in mesh.Faces)
{
index[0] = face.GetVertex(2);
index[1] = face.GetVertex(0);
index[2] = face.GetVertex(1);
for (int i = 0; i < 3; i++)
{
Quaternion f0 = new Quaternion(index[(i + 0) % 3].Traits.Position, 0);
Quaternion f1 = new Quaternion(index[(i + 1) % 3].Traits.Position, 0);
Quaternion f2 = new Quaternion(index[(i + 2) % 3].Traits.Position, 0);
//Setting Orientation Swap it
TriMesh.Vertex aI = index[(i + 1) % 3];
TriMesh.Vertex bI = index[(i + 2) % 3];
if (aI.Index > bI.Index)
{
aI = index[(i + 2) % 3];
bI = index[(i + 1) % 3];
}
Quaternion aLamda = lamda[aI.Index, 0];
Quaternion bLamda = lamda[bI.Index, 0];
Quaternion e = new Quaternion(bI.Traits.Position, 0) - new Quaternion(aI.Traits.Position, 0);
Quaternion conj = aLamda.Conjugate();
Quaternion bconj = bLamda.Conjugate();
Quaternion partA = conj * e * aLamda;
Quaternion eTilde = (1f / 3f) * partA +
(1f / 6f) * aLamda.Conjugate() * e * bLamda +
(1f / 6f) * bLamda.Conjugate() * e * aLamda +
(1f / 3f) * bLamda.Conjugate() * e * bLamda;
Vector3D u1 = index[(i + 1) % 3].Traits.Position - index[(i + 0) % 3].Traits.Position;
Vector3D u2 = index[(i + 2) % 3].Traits.Position - index[(i + 0) % 3].Traits.Position;
double cotAlpha = u1.Dot(u2) / u1.Cross(u2).Length();
Quaternion q = cotAlpha * eTilde / 2;
Omega[aI.Index, 0] -= q;
Omega[bI.Index, 0] += q;
}
}
RemoveMean(ref Omega);
return Omega;
}
public static DenseMatrixQuaternion operator *(Quaternion left, DenseMatrixQuaternion right)
{
DenseMatrixQuaternion result = new DenseMatrixQuaternion(right.rowCount, right.columnCount);
for (int i = 0; i < right.rowCount; i++)
{
for (int j = 0; j < right.columnCount; j++)
{
result.datas[i, j] = right.datas[i, j] * left;
}
}
return result;
}
public DenseMatrixQuaternion SolveByFactorizedLU(ref DenseMatrixQuaternion b)
{
if (b.ColumnCount > 1)
{
return null;
}
double[][] rhs = new double[4][]; //Arranage By Column
for (int i = 0; i < 4; i++)
{
rhs[i] = new double[4 * b.RowCount];
}
double[][] x = new double[4][];
for (int i = 0; i < 4; i++)
{
x[i] = new double[4 * this.n];
}
for (int i = 0; i < b.RowCount; i++)
{
Quaternion item = b[i, 0];
Matrix4D mat = item.ToMatrix();
rhs[0][4 * i] = mat[0, 0]; rhs[1][4 * i] = mat[0, 1]; rhs[2][4 * i] = mat[0, 2]; rhs[3][4 * i] = mat[0, 3];
rhs[0][4 * i + 1] = mat[1, 0]; rhs[1][4 * i + 1] = mat[1, 1]; rhs[2][4 * i + 1] = mat[1, 2]; rhs[3][4 * i + 1] = mat[1, 3];
rhs[0][4 * i + 2] = mat[2, 0]; rhs[1][4 * i + 2] = mat[2, 1]; rhs[2][4 * i + 2] = mat[2, 2]; rhs[3][4 * i + 2] = mat[2, 3];
rhs[0][4 * i + 3] = mat[3, 0]; rhs[1][4 * i + 3] = mat[3, 1]; rhs[2][4 * i + 3] = mat[3, 2]; rhs[3][4 * i + 3] = mat[3, 3];
}
DenseMatrixQuaternion result = new DenseMatrixQuaternion(n, 1);
//Solve
for (int i = 0; i < 4; i++)
{
SolveLU(ref rhs[i], ref x[i]);
}
for (int i = 0; i < this.n; i++)
{
result[i, 0] = new Quaternion(x[0][4 * i + 1], x[0][4 * i + 2], x[0][4 * i + 3], x[0][4 * i]);
}
x = null;
GC.Collect();
return result;
}
public static DenseMatrixQuaternion Copy(ref DenseMatrixQuaternion B)
{
DenseMatrixQuaternion newMatrix = new DenseMatrixQuaternion();
newMatrix.rowCount = B.rowCount;
newMatrix.columnCount = B.columnCount;
Array.Copy(B.datas, newMatrix.datas, B.datas.Length);
return newMatrix;
}
public Quaternion Dot(DenseMatrixQuaternion right)
{
DenseMatrixQuaternion x = this.Transpose() * right;
Quaternion result = x[0, 0];
return result;
}
