コード例 #1
0
    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();
    }