private static Sym3x3 ComputeWeightedCovariance(int n, Vector3F[] points, float[] weights)
{
// compute the centroid
float total = 0.0f;
Vector3F centroid = new Vector3F(0.0f);
for (int i = 0; i < n; ++i)
{
total += weights[i];
centroid += weights[i] * points[i];
}
if (total > float.Epsilon)
centroid /= total;
// accumulate the covariance matrix
Sym3x3 covariance = new Sym3x3();
for (int i = 0; i < n; ++i)
{
Vector3F a = points[i] - centroid;
Vector3F b = weights[i] * a;
covariance.M0 += a.X * b.X;
covariance.M1 += a.X * b.Y;
covariance.M2 += a.X * b.Z;
covariance.M3 += a.Y * b.Y;
covariance.M4 += a.Y * b.Z;
covariance.M5 += a.Z * b.Z;
}
// return it
return covariance;
}
private static Vector3F ComputePrincipleComponent(Sym3x3 matrix)
{
Vector4F row0 = new Vector4F(matrix.M0, matrix.M1, matrix.M2, 0.0f);
Vector4F row1 = new Vector4F(matrix.M1, matrix.M3, matrix.M4, 0.0f);
Vector4F row2 = new Vector4F(matrix.M2, matrix.M4, matrix.M5, 0.0f);
Vector4F v = Vector4F.One;
const int POWER_ITERATION_COUNT = 8;
for (int i = 0; i < POWER_ITERATION_COUNT; ++i)
{
// matrix multiply
Vector4F w = row0 * v.X;
w = MultiplyAdd(row1, v.Y, w);
w = MultiplyAdd(row2, v.Z, w);
// get max component from xyz in all channels
Vector4F a = new Vector4F(Math.Max(w.X, Math.Max(w.Y, w.Z)));
// divide through and advance
v = w / a;
}
return v.XYZ;
}
