public static Vec3 ComputePrincipleComponent(Sym3x3 matrix) { // compute the cubic coefficients float c0 = matrix[0] * matrix[3] * matrix[5] + 2.0f * matrix[1] * matrix[2] * matrix[4] - matrix[0] * matrix[4] * matrix[4] - matrix[3] * matrix[2] * matrix[2] - matrix[5] * matrix[1] * matrix[1]; float c1 = matrix[0] * matrix[3] + matrix[0] * matrix[5] + matrix[3] * matrix[5] - matrix[1] * matrix[1] - matrix[2] * matrix[2] - matrix[4] * matrix[4]; float c2 = matrix[0] + matrix[3] + matrix[5]; // compute the quadratic coefficients float a = c1 - (1.0f / 3.0f) * c2 * c2; float b = (-2.0f / 27.0f) * c2 * c2 * c2 + (1.0f / 3.0f) * c1 * c2 - c0; // compute the root count check float Q = 0.25f * b * b + (1.0f / 27.0f) * a * a * a; // test the multiplicity if (float.Epsilon < Q) { // only one root, which implies we have a multiple of the identity return new Vec3(1.0f); } else if (Q < -float.Epsilon) { // three distinct roots float theta = CMath.Atan2(CMath.Sqrt(-Q), -0.5f * b); float rho = CMath.Sqrt(0.25f * b * b - Q); float rt = CMath.Pow(rho, 1.0f / 3.0f); float ct = CMath.Cos(theta / 3.0f); float st = CMath.Sin(theta / 3.0f); float l1 = (1.0f / 3.0f) * c2 + 2.0f * rt * ct; float l2 = (1.0f / 3.0f) * c2 - rt * (ct + (float)CMath.Sqrt(3.0f) * st); float l3 = (1.0f / 3.0f) * c2 - rt * (ct - (float)CMath.Sqrt(3.0f) * st); // pick the larger if (Math.Abs(l2) > Math.Abs(l1)) l1 = l2; if (Math.Abs(l3) > Math.Abs(l1)) l1 = l3; // get the eigenvector return GetMultiplicity1Evector(matrix, l1); } else { // two roots float rt; if (b < 0.0f) rt = -CMath.Pow(-0.5f * b, 1.0f / 3.0f); else rt = CMath.Pow(0.5f * b, 1.0f / 3.0f); float l1 = (1.0f / 3.0f) * c2 + rt; // repeated float l2 = (1.0f / 3.0f) * c2 - 2.0f * rt; // get the eigenvector if (Math.Abs(l1) > Math.Abs(l2)) return GetMultiplicity2Evector(matrix, l1); else return GetMultiplicity1Evector(matrix, l2); } }
private static Vec3 GetMultiplicity1Evector(Sym3x3 matrix, float evalue) { // compute M Sym3x3 m = new Sym3x3(0f); m[0] = matrix[0] - evalue; m[1] = matrix[1]; m[2] = matrix[2]; m[3] = matrix[3] - evalue; m[4] = matrix[4]; m[5] = matrix[5] - evalue; // compute U Sym3x3 u = new Sym3x3(0f); u[0] = m[3] * m[5] - m[4] * m[4]; u[1] = m[2] * m[4] - m[1] * m[5]; u[2] = m[1] * m[4] - m[2] * m[3]; u[3] = m[0] * m[5] - m[2] * m[2]; u[4] = m[1] * m[2] - m[4] * m[0]; u[5] = m[0] * m[3] - m[1] * m[1]; // find the largest component float mc = Math.Abs(u[0]); int mi = 0; for (int i = 1; i < 6; ++i) { float c = Math.Abs(u[i]); if (c > mc) { mc = c; mi = i; } } // pick the column with this component switch (mi) { case 0: return new Vec3(u[0], u[1], u[2]); case 1: case 3: return new Vec3(u[1], u[3], u[4]); default: return new Vec3(u[2], u[4], u[5]); } }
private static Vec3 GetMultiplicity2Evector(Sym3x3 matrix, float evalue) { // compute M Sym3x3 m = new Sym3x3(0f); m[0] = matrix[0] - evalue; m[1] = matrix[1]; m[2] = matrix[2]; m[3] = matrix[3] - evalue; m[4] = matrix[4]; m[5] = matrix[5] - evalue; // find the largest component float mc = Math.Abs(m[0]); int mi = 0; for (int i = 1; i < 6; ++i) { float c = Math.Abs(m[i]); if (c > mc) { mc = c; mi = i; } } // pick the first eigenvector based on this index switch (mi) { case 0: case 1: return new Vec3(-m[1], m[0], 0.0f); case 2: return new Vec3(m[2], 0.0f, -m[0]); case 3: case 4: return new Vec3(0.0f, -m[4], m[3]); default: return new Vec3(0.0f, -m[5], m[4]); } }
public static Sym3x3 ComputeWeightedCovariance(int n, Vec3[] points, float[] weights) { // compute the centroid float total = 0.0f; Vec3 centroid = new Vec3(0.0f); for (int i = 0; i < n; ++i) { total += weights[i]; centroid += weights[i] * points[i]; } centroid /= total; // accumulate the covariance matrix Sym3x3 covariance = new Sym3x3(0.0f); for (int i = 0; i < n; ++i) { Vec3 a = points[i] - centroid; Vec3 b = weights[i] * a; covariance[0] += a.X() * b.X(); covariance[1] += a.X() * b.Y(); covariance[2] += a.X() * b.Z(); covariance[3] += a.Y() * b.Y(); covariance[4] += a.Y() * b.Z(); covariance[5] += a.Z() * b.Z(); } // return it return covariance; }