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); }