public Matrix3D Inverse()
{
double a = e[0];
double b = e[1];
double c = e[2];
double d = e[3];
double E = e[4];
double f = e[5];
double g = e[6];
double h = e[7];
double i = e[8];
double det = a * (E * i - f * h) - b * (d * i - f * g) + c * (d * h - E * g);
if (det == 0) throw new ArithmeticException();
Matrix3D inv = new Matrix3D();
inv[0] = (E * i - f * h) / det;
inv[1] = (c * h - b * i) / det;
inv[2] = (b * f - c * E) / det;
inv[3] = (f * g - d * i) / det;
inv[4] = (a * i - c * g) / det;
inv[5] = (c * d - a * f) / det;
inv[6] = (d * h - E * g) / det;
inv[7] = (b * g - a * h) / det;
inv[8] = (a * E - b * d) / det;
return inv;
}
public Matrix3D Transpose()
{
Matrix3D m = new Matrix3D();
for (int i = 0; i < row_size; i++)
for (int j = 0; j < row_size; j++)
m[j, i] = this[i, j];
return m;
}
public SparseMatrix BuildLaplaceMatrixDual()
{
// build dual Laplacian weight matrix L
int vn = this.Vertices.Count;
int fn = this.Faces.Count;
SparseMatrix L = new SparseMatrix(fn, vn, 6);
for (int i = 0; i < fn; i++)
{
int f1 = this.Faces[i].GetFace(0).Index;
int f2 = this.Faces[i].GetFace(1).Index;
int f3 = this.Faces[i].GetFace(2).Index;
Vector3D dv = this.DualGetVertexPosition(i);
Vector3D dv1 = this.DualGetVertexPosition(f1);
Vector3D dv2 = this.DualGetVertexPosition(f2);
Vector3D dv3 = this.DualGetVertexPosition(f3);
Vector3D u = dv - dv3;
Vector3D v1 = dv1 - dv3;
Vector3D v2 = dv2 - dv3;
Vector3D normal = (v1.Cross(v2)).Normalize();
Matrix3D M = new Matrix3D(v1, v2, normal);
Vector3D coord = M.Inverse() * u;
double alpha;
alpha = 1.0 / 3.0;
L.AddValueTo(i, this.Faces[i].GetVertex(0).Index, alpha);
L.AddValueTo(i, this.Faces[i].GetVertex(1).Index, alpha);
L.AddValueTo(i, this.Faces[i].GetVertex(2).Index, alpha);
alpha = coord[0] / 3.0;
L.AddValueTo(i, this.Faces[f1].GetVertex(0).Index, -alpha);
L.AddValueTo(i, this.Faces[f1].GetVertex(1).Index, -alpha);
L.AddValueTo(i, this.Faces[f1].GetVertex(2).Index, -alpha);
alpha = coord[1] / 3.0;
L.AddValueTo(i, this.Faces[f2].GetVertex(0).Index, -alpha);
L.AddValueTo(i, this.Faces[f2].GetVertex(1).Index, -alpha);
L.AddValueTo(i, this.Faces[f2].GetVertex(2).Index, -alpha);
alpha = (1.0 - coord[0] - coord[1]) / 3.0;
L.AddValueTo(i, this.Faces[f3].GetVertex(0).Index, -alpha);
L.AddValueTo(i, this.Faces[f3].GetVertex(1).Index, -alpha);
L.AddValueTo(i, this.Faces[f3].GetVertex(2).Index, -alpha);
}
L.SortElement();
return L;
}
public static Matrix3D Orthogonalize(Vector3D u, Vector3D v)
{
Vector3D a = u.Normalize();
Vector3D temp = v - (v.Dot(a)) * a;
Vector3D b = temp.Normalize();
Vector3D c = a.Cross(b).Normalize();
Matrix3D e = new Matrix3D(a, b, c);
return e;
}
//public static void PreserveVolume(TriMesh mesh)
//{
// double oldVolume = TriMeshUtil.ComputeVolume(Mesh);
// double newVolume = TriMeshUtil.ComputeVolume(Mesh);
// for (int i = 0; i < n; i++)
// {
// Mesh.Vertices[i].Traits.Position.x *= Math.Pow(oldVolume / newVolume, 1 / 3);
// Mesh.Vertices[i].Traits.Position.y *= Math.Pow(oldVolume / newVolume, 1 / 3);
// Mesh.Vertices[i].Traits.Position.z *= Math.Pow(oldVolume / newVolume, 1 / 3);
// }
//}
public static void PreModifiyGeometry(TriMesh mesh)
{
double ModifyVertexScale = 100;
int n = mesh.Vertices.Count;
for (int j = 0; j < 10; j++)
{
bool updated = false;
TriMeshUtil.SetUpNormalVertex(mesh);
for (int i = 0; i < n; i++)
{
// build matrix U
Matrix3D U = new Matrix3D();
Vector3D u1 = mesh.Vertices[i].Traits.Position;
foreach (TriMesh.Vertex neighbor in mesh.Vertices[i].Vertices)
{
Vector3D u21 = neighbor.Traits.Position - u1;
U += u21.OuterCross(u21);
}
// Test for degenerated
Matrix3D invU = U.InverseSVD();
if (Matrix3D.lastSVDIsFullRank == false)
{
Vector3D normal = mesh.Vertices[i].Traits.Normal;
double area = 0;
foreach (TriMesh.Face face in mesh.Vertices[i].Faces)
{
area += TriMeshUtil.ComputeAreaFace(face);
}
Vector3D newU1 = u1 + normal.Normalize() * Math.Sqrt(area / 3.0) / ModifyVertexScale;
mesh.Vertices[i].Traits.Position = newU1;
updated = true;
}
}
if (!updated)
break;
}
}
public SVD2(Matrix3D m)
{
double[,] uArray = new double[3, 3];
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
uArray[i, j] = m[i, j];
}
}
double[,] vArray;
double[] eArray;
ASVD.svdcmp(uArray, out eArray, out vArray);
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
U[i, j] = uArray[i, j];
V[i, j] = vArray[i, j];
}
E[i] = eArray[i];
}
}
public static Matrix3D Rotate(double angle)
{
Matrix3D rotate = new Matrix3D();
double cosX = Math.Cos(angle);
double sinX = Math.Sin(angle);
rotate[0, 0] = cosX; rotate[0, 1] = -sinX;
rotate[1, 0] = sinX; rotate[1, 1] = cosX;
rotate[2, 2] = 1;
return rotate;
}
public static Matrix3D operator /(Matrix3D m, double d)
{
Matrix3D ret = new Matrix3D();
for (int i = 0; i < len; i++) ret[i] = m[i] / d;
return ret;
}
public static Matrix3D operator -(Matrix3D m1, Matrix3D m2)
{
Matrix3D ret = new Matrix3D();
for (int i = 0; i < len; i++) ret[i] = m1[i] - m2[i];
return ret;
}
public static Matrix3D operator *(Matrix3D m1, Matrix3D m2)
{
Matrix3D ret = new Matrix3D();
for (int i = 0; i < row_size; i++)
for (int j = 0; j < row_size; j++)
{
ret[i, j] = 0.0;
for (int k = 0; k < row_size; k++)
ret[i, j] += m1[i, k] * m2[k, j];
}
return ret;
}
public Matrix3D SVDRotation(ref Matrix3D matrix)
{
double[,] X = new double[3, 3];
for (int i = 0; i < 3;i++ )
{
for(int j=0;j<3;j++)
{
X[i, j] = matrix[i, j];
}
}
double[,] R = SVDRotation(ref X);
Matrix3D result = new Matrix3D();
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
result[i, j] = R[i, j];
}
}
return result;
}
private Matrix3D QuatToMatrix3d(Vector4D q)
{
double n = q.Dot(q);
double s = (n > 0.0) ? (2.0 / n) : 0.0f;
double xs, ys, zs;
double wx, wy, wz;
double xx, xy, xz;
double yy, yz, zz;
xs = q.x * s; ys = q.y * s; zs = q.z * s;
wx = q.w * xs; wy = q.w * ys; wz = q.w * zs;
xx = q.x * xs; xy = q.x * ys; xz = q.x * zs;
yy = q.y * ys; yz = q.y * zs; zz = q.z * zs;
Matrix3D m = new Matrix3D();
m[0,0] = 1.0 - (yy + zz); m[1,0] = xy - wz; m[2,0] = xz + wy;
m[0,1] = xy + wz; m[1,1] = 1.0 - (xx + zz); m[2,1] = yz - wx;
m[0,2] = xz - wy; m[1,2] = yz + wx; m[2,2] = 1.0 - (xx + yy);
return m;
}
public static double[] ComputeDualLap(ref NonManifoldMesh mesh)
{
int vn = mesh.VertexCount;
int fn = mesh.FaceCount;
double[] dLap = new double[fn * 3];
mesh.ComputeDualPosition();
for (int i = 0, j = 0; i < fn; i++, j += 3)
{
Vector3D u = new Vector3D(mesh.DualVertexPos, j);
Vector3D v1 = new Vector3D(mesh.DualVertexPos, mesh.AdjFF[i][0] * 3);
Vector3D v2 = new Vector3D(mesh.DualVertexPos, mesh.AdjFF[i][1] * 3);
Vector3D v3 = new Vector3D(mesh.DualVertexPos, mesh.AdjFF[i][2] * 3);
Vector3D normal = ((v1 - v3).Cross(v2 - v3)).Normalize();
Matrix3D m = new Matrix3D(v1 - v3, v2 - v3, normal);
Vector3D coord = m.Inverse() * (u - v3);
//dLap[j] = coord.x;
//dLap[j + 1] = coord.y;
//dLap[j + 2] = coord.z;
dLap[j] = normal.x * coord[2];
dLap[j + 1] = normal.x * coord[2];
dLap[j + 2] = normal.x * coord[2];
}
return dLap;
}
public static SparseMatrix BuildMatrixDualL(ref NonManifoldMesh mesh)
{
// build dual Laplacian weight matrix L
int vn = mesh.VertexCount;
int fn = mesh.FaceCount;
SparseMatrix L = new SparseMatrix(fn, vn, 6);
for (int i = 0; i < fn; i++)
{
int f1 = mesh.AdjFF[i][0];
int f2 = mesh.AdjFF[i][1];
int f3 = mesh.AdjFF[i][2];
Vector3D dv = mesh.GetDualPosition(i);
Vector3D dv1 = mesh.GetDualPosition(f1);
Vector3D dv2 = mesh.GetDualPosition(f2);
Vector3D dv3 = mesh.GetDualPosition(f3);
Vector3D u = dv - dv3;
Vector3D v1 = dv1 - dv3;
Vector3D v2 = dv2 - dv3;
Vector3D normal = (v1.Cross(v2)).Normalize();
Matrix3D M = new Matrix3D(v1, v2, normal);
Vector3D coord = M.Inverse() * u;
double alpha;
alpha = 1.0 / 3.0;
for (int j = 0, k = i * 3; j < 3; j++)
L.AddValueTo(i, mesh.FaceIndex[k++], alpha);
alpha = coord[0] / 3.0;
for (int j = 0, k = f1 * 3; j < 3; j++)
L.AddValueTo(i, mesh.FaceIndex[k++], -alpha);
alpha = coord[1] / 3.0;
for (int j = 0, k = f2 * 3; j < 3; j++)
L.AddValueTo(i, mesh.FaceIndex[k++], -alpha);
alpha = (1.0 - coord[0] - coord[1]) / 3.0;
for (int j = 0, k = f3 * 3; j < 3; j++)
L.AddValueTo(i, mesh.FaceIndex[k++], -alpha);
}
L.SortElement();
return L;
}
public double[][] ComputeLaplacianDual(TriMesh mesh)
{
int fn = mesh.Faces.Count;
double[][] laplacian = new double[3][];
laplacian[0] = new double[fn];
laplacian[1] = new double[fn];
laplacian[2] = new double[fn];
for (int i = 0; i < fn; i++)
{
int f1 = mesh.Faces[i].GetFace(0).Index;
int f2 = mesh.Faces[i].GetFace(1).Index;
int f3 = mesh.Faces[i].GetFace(2).Index;
Vector3D u = mesh.DualGetVertexPosition(i);
Vector3D v1 = mesh.DualGetVertexPosition(f1);
Vector3D v2 = mesh.DualGetVertexPosition(f2);
Vector3D v3 = mesh.DualGetVertexPosition(f3);
Vector3D normal = ((v1 - v3).Cross(v2 - v3)).Normalize();
Matrix3D m = new Matrix3D(v1 - v3, v2 - v3, normal);
Vector3D coord = m.Inverse() * (u - v3);
laplacian[0][i] = normal.x * coord[2];
laplacian[1][i] = normal.y * coord[2];
laplacian[2][i] = normal.z * coord[2];
}
return laplacian;
}
public Matrix3D(Matrix3D m) : this(m.e) { }
public Matrix3D SVDRotationMinus()
{
SVD svd = new SVD(e, 3, 3);
Matrix3D rot = new Matrix3D(svd.RotationMinus);
lastSVDIsFullRank = svd.FullRank;
return rot;
}
public Matrix3D OrthogonalFactor(double eps)
{
Matrix3D Q = new Matrix3D(this);
Matrix3D Q2 = new Matrix3D();
double err = 0;
do
{
Q2 = (Q + Q.InverseTranspose()) / 2.0;
err = (Q2 - Q).SqNorm();
Q = Q2;
} while (err > eps);
return Q2;
}
public Matrix3D InverseSVD()
{
SVD svd = new SVD(e, 3, 3);
Matrix3D inv = new Matrix3D(svd.Inverse);
lastSVDIsFullRank = svd.FullRank;
return inv;
}
public static Matrix3D IdentityMatrix()
{
Matrix3D m = new Matrix3D();
m[0] = m[4] = m[8] = 1.0;
return m;
}
public SVDInfo SVDInfo(ref Matrix3D matrix)
{
double[,] X = new double[3, 3];
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
X[i, j] = matrix[i, j];
}
}
OutPut(X, "A");
matlab.Execute(@"[U,S,V] = svd(A)");
matlab.Execute(@"R = V*U'; if( det(R) < 0 ) U(:,end) = -U(:,end); R = V*U'; end");
// matlab.Execute(@"R = V*U'");
matlab.Execute(@"DetU=det(U)");
matlab.Execute(@"DetS=det(S)");
matlab.Execute(@"DetV=det(V)");
matlab.Execute(@"DetR=det(R)");
matlab.Execute(@"DetA=det(A)");
double[,] A= GetMatrix("A");
double[,] R = GetMatrix("R");
double[,] U = GetMatrix("U");
double[,] S= GetMatrix("S");
double[,] V = GetMatrix("V");
SVDInfo svdInfo = new SVDInfo();
svdInfo.DetU = matlab.GetVariable("DetU", "base");
svdInfo.DetS = matlab.GetVariable("DetS", "base");
svdInfo.DetV = matlab.GetVariable("DetV", "base");
svdInfo.DetR = matlab.GetVariable("DetR", "base");
svdInfo.DetA = matlab.GetVariable("DetA", "base");
svdInfo.A = new Matrix3D();
svdInfo.R = new Matrix3D();
svdInfo.U = new Matrix3D();
svdInfo.S = new Matrix3D();
svdInfo.V = new Matrix3D();
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
svdInfo.A[i, j] = A[i, j];
svdInfo.R[i, j] = R[i, j];
svdInfo.U[i, j] = U[i, j];
svdInfo.S[i, j] = S[i, j];
svdInfo.V[i, j] = V[i, j];
}
}
return svdInfo;
}
public static Matrix3D ComputeRotationRodrigues(Vector3D first, Vector3D second, int step)
{
Vector3D cross = first.Cross(second);
double sin = cross.Length();
double cos = first.Dot(second);
//if (cos > 0.99)
//{
// return Matrix3D.IdentityMatrix();
//}
double angle = Math.Acos(cos);
cos = Math.Cos(angle / step);
sin = Math.Sin(angle / step);
cross = cross.Normalize();
double x = cross.x;
double y = cross.y;
double z = cross.z;
Matrix3D result = Matrix3D.IdentityMatrix();
//result[0, 0] = cos+(1-cos)*x*x;
//result[0, 1] = x * y*(1 - cos) - z * sin;
//result[0, 2] = y* sin+x*z*(1-cos);
//result[1, 0] = z * sin + x * y * (1 - cos);
//result[1, 1] = cos + y * y * (1 - cos);
//result[1, 2] = -x * sin + y * z * (1 - cos);
//result[2, 0] = -y * sin + x * z * (1 - cos);
//result[2, 1] = x * sin + y * z * (1 - cos);
//result[2, 2] =cos + z* z * (1 - cos);
//result[0, 0] = cos*(y*y+z*z) + x * x;
//result[0, 1] = x * y * (1 - cos) - z * sin;
//result[0, 2] = y * sin + x * z * (1 - cos);
//result[1, 0] = z * sin + x * y * (1 - cos);
//result[1, 1] = cos*(x*x+z*z) + y * y ;
//result[1, 2] = -x * sin + y * z * (1 - cos);
//result[2, 0] = -y * sin + x * z * (1 - cos);
//result[2, 1] = x * sin + y * z * (1 - cos);
//result[2, 2] = cos*(x*x+y*y) + z * z ;
result[0, 0] = cos * (1 - x * x) + x * x;
result[0, 1] = x * y * (1 - cos) - z * sin;
result[0, 2] = y * sin + x * z * (1 - cos);
result[1, 0] = z * sin + x * y * (1 - cos);
result[1, 1] = cos * (1 - y * y) + y * y;
result[1, 2] = -x * sin + y * z * (1 - cos);
result[2, 0] = -y * sin + x * z * (1 - cos);
result[2, 1] = x * sin + y * z * (1 - cos);
result[2, 2] = cos * (1 - z * z) + z * z;
// result = result.Transpose();
return(result);
}
