internal void CalculateNormal(C5.IPriorityQueue<CloudPoint> neighbors) { Vector avg = neighbors.Aggregate(new Vector(3), (sum, val) => sum + val.location) / (double)neighbors.Count; Matrix cov = ((double)1 / neighbors.Count) * neighbors.Aggregate(new Matrix(3, 3), (sum, val) => sum + (val.location - avg).ToColumnMatrix() * (val.location - avg).ToRowMatrix()); // that dude says the smallest eigenvalue is the normal // first column of eigenvectors is the smallest eigenvalued one normal = cov.EigenVectors.GetColumnVector(0); // orient normal toward viewpoint // to do this we have to push the origin into the world frame //v = ZigInput.ConvertImageToWorldSpace(v); var viewp = ZigInput.ConvertImageToWorldSpace(Vector3.zero); normal = ((normal * (viewp.ToVector() - this.location)) > 0 ? normal : -normal); normal.Normalize(); }