コード例 #1
0
        public Vector3d Project(Vector3d vPoint, int identifier = -1)
        {
            int tNearestID        = Spatial.FindNearestTriangle(vPoint);
            DistPoint3Triangle3 q = MeshQueries.TriangleDistance(Mesh, tNearestID, vPoint);

            return(q.TriangleClosest);
        }
コード例 #2
0
        public bool RayIntersect(Ray3d ray, out Vector3d vHit, out Vector3d vHitNormal)
        {
            vHit       = Vector3d.Zero;
            vHitNormal = Vector3d.AxisX;
            int tHitID = Spatial.FindNearestHitTriangle(ray);

            if (tHitID == DMesh3.InvalidID)
            {
                return(false);
            }

            IntrRay3Triangle3 t = MeshQueries.TriangleIntersection(Mesh, tHitID, ray);

            vHit = ray.PointAt(t.RayParameter);
            if (UseFaceNormal == false && Mesh.HasVertexNormals)
            {
                vHitNormal = Mesh.GetTriBaryNormal(tHitID, t.TriangleBaryCoords.x, t.TriangleBaryCoords.y, t.TriangleBaryCoords.z);
            }
            else
            {
                vHitNormal = Mesh.GetTriNormal(tHitID);
            }

            return(true);
        }
コード例 #3
0
            public Vector3d FindNearestAndOffset(Vector3d pos)
            {
                int tNearestID                 = Spatial.FindNearestTriangle(pos);
                DistPoint3Triangle3 q          = MeshQueries.TriangleDistance(Mesh, tNearestID, pos);
                Vector3d            vHitNormal =
                    (UseFaceNormal == false && Mesh.HasVertexNormals) ?
                    Mesh.GetTriBaryNormal(tNearestID, q.TriangleBaryCoords.x, q.TriangleBaryCoords.y, q.TriangleBaryCoords.z)
                        : Mesh.GetTriNormal(tNearestID);

                return(q.TriangleClosest + Distance * vHitNormal);
            }
コード例 #4
0
ファイル: MeshICP.cs プロジェクト: larsbrubaker/agg-sharp
        // for each From[i], find closest point on TargetSurface
        void update_to()
        {
            double max_dist = double.MaxValue;

            bool bNormals = (UseNormals && Source.HasVertexNormals);

            var range = Interval1i.Range(From.Length);

            gParallel.ForEach(range, (vi) =>
            {
                int tid = TargetSurface.FindNearestTriangle(From[vi], max_dist);
                if (tid == DMesh3.InvalidID)
                {
                    Weights[vi] = 0;
                    return;
                }

                DistPoint3Triangle3 d = MeshQueries.TriangleDistance(TargetSurface.Mesh, tid, From[vi]);
                if (d.DistanceSquared > MaxAllowableDistance * MaxAllowableDistance)
                {
                    Weights[vi] = 0;
                    return;
                }

                To[vi]      = d.TriangleClosest;
                Weights[vi] = 1.0f;

                if (bNormals)
                {
                    Vector3d fromN = Rotation * Source.GetVertexNormal(vi);
                    Vector3d toN   = TargetSurface.Mesh.GetTriNormal(tid);
                    double fDot    = fromN.Dot(toN);
                    Debug.Assert(MathUtil.IsFinite(fDot));
                    if (fDot < 0)
                    {
                        Weights[vi] = 0;
                    }
                    else
                    {
                        Weights[vi] += Math.Sqrt(fDot);
                    }
                }
            });
        }
コード例 #5
0
        // do full tree traversal below iBox and make sure that all triangles are further
        // than box-distance-sqr
        void debug_check_child_tri_distances(int iBox, Vector3d p)
        {
            double fBoxDistSqr = box_distance_sqr(iBox, p);

            TreeTraversal t = new TreeTraversal()
            {
                NextTriangleF = (tID) => {
                    double fTriDistSqr = MeshQueries.TriDistanceSqr(mesh, tID, p);
                    if (fTriDistSqr < fBoxDistSqr)
                    {
                        if (Math.Abs(fTriDistSqr - fBoxDistSqr) > MathUtil.ZeroTolerance * 100)
                        {
                            Util.gBreakToDebugger();
                        }
                    }
                }
            };

            tree_traversal(iBox, 0, t);
        }
コード例 #6
0
        void find_nearest_tri(int iBox, Vector3d p, ref double fNearestSqr, ref int tID)
        {
            int idx = box_to_index[iBox];

            if (idx < triangles_end)                // triange-list case, array is [N t1 t2 ... tN]
            {
                int num_tris = index_list[idx];
                for (int i = 1; i <= num_tris; ++i)
                {
                    int    ti          = index_list[idx + i];
                    double fTriDistSqr = MeshQueries.TriDistanceSqr(mesh, ti, p);
                    if (fTriDistSqr < fNearestSqr)
                    {
                        fNearestSqr = fTriDistSqr;
                        tID         = ti;
                    }
                }
            }
            else                                    // internal node, either 1 or 2 child boxes
            {
                int iChild1 = index_list[idx];
                if (iChild1 < 0)                     // 1 child, descend if nearer than cur min-dist
                {
                    iChild1 = (-iChild1) - 1;
                    double fChild1DistSqr = box_distance_sqr(iChild1, p);
                    if (fChild1DistSqr <= fNearestSqr)
                    {
                        find_nearest_tri(iChild1, p, ref fNearestSqr, ref tID);
                    }
                }
                else                                // 2 children, descend closest first
                {
                    iChild1 = iChild1 - 1;
                    int iChild2 = index_list[idx + 1] - 1;

                    double fChild1DistSqr = box_distance_sqr(iChild1, p);
                    double fChild2DistSqr = box_distance_sqr(iChild2, p);
                    if (fChild1DistSqr < fChild2DistSqr)
                    {
                        if (fChild1DistSqr < fNearestSqr)
                        {
                            find_nearest_tri(iChild1, p, ref fNearestSqr, ref tID);
                            if (fChild2DistSqr < fNearestSqr)
                            {
                                find_nearest_tri(iChild2, p, ref fNearestSqr, ref tID);
                            }
                        }
                    }
                    else
                    {
                        if (fChild2DistSqr < fNearestSqr)
                        {
                            find_nearest_tri(iChild2, p, ref fNearestSqr, ref tID);
                            if (fChild1DistSqr < fNearestSqr)
                            {
                                find_nearest_tri(iChild1, p, ref fNearestSqr, ref tID);
                            }
                        }
                    }
                }
            }
        }
コード例 #7
0
        public virtual bool Trim()
        {
            if (Spatial == null)
            {
                Spatial = new DMeshAABBTree3(new DMesh3(Mesh, false, MeshComponents.None));
                Spatial.Build();
            }

            if (seed_tri == -1)
            {
                seed_tri = Spatial.FindNearestTriangle(seed_pt);
            }

            var loop = new MeshFacesFromLoop(Mesh, TrimLine, Spatial, seed_tri);

            MeshFaceSelection selection = loop.ToSelection();

            selection.LocalOptimize(true, true);
            var editor = new MeshEditor(Mesh);

            editor.RemoveTriangles(selection, true);

            var components = new MeshConnectedComponents(Mesh);

            components.FindConnectedT();
            if (components.Count > 1)
            {
                int keep = components.LargestByCount;
                for (int i = 0; i < components.Count; ++i)
                {
                    if (i != keep)
                    {
                        editor.RemoveTriangles(components[i].Indices, true);
                    }
                }
            }
            editor.RemoveAllBowtieVertices(true);

            var  loops   = new MeshBoundaryLoops(Mesh);
            bool loopsOK = false;

            try
            {
                loopsOK = loops.Compute();
            }
            catch (Exception)
            {
                return(false);
            }
            if (!loopsOK)
            {
                return(false);
            }


            // [TODO] to support trimming mesh w/ existing holes, we need to figure out which
            // loop we created in RemoveTriangles above!
            if (loops.Count > 1)
            {
                return(false);
            }

            int[] loopVerts = loops[0].Vertices;

            var borderTris = new MeshFaceSelection(Mesh);

            borderTris.SelectVertexOneRings(loopVerts);
            borderTris.ExpandToOneRingNeighbours(RemeshBorderRings);

            var remesh = new RegionRemesher(Mesh, borderTris.ToArray());

            remesh.Region.MapVerticesToSubmesh(loopVerts);

            double target_len = TargetEdgeLength;

            if (target_len <= 0)
            {
                double mine, maxe, avge;
                MeshQueries.EdgeLengthStatsFromEdges(Mesh, loops[0].Edges, out mine, out maxe, out avge);
                target_len = avge;
            }

            var meshTarget = new MeshProjectionTarget(Spatial.Mesh, Spatial);

            remesh.SetProjectionTarget(meshTarget);
            remesh.SetTargetEdgeLength(target_len);
            remesh.SmoothSpeedT = SmoothingAlpha;

            var curveTarget = new DCurveProjectionTarget(TrimLine);
            var multiTarget = new SequentialProjectionTarget(curveTarget, meshTarget);

            int set_id = 3;

            MeshConstraintUtil.ConstrainVtxLoopTo(remesh, loopVerts, multiTarget, set_id);

            for (int i = 0; i < RemeshRounds; ++i)
            {
                remesh.BasicRemeshPass();
            }

            remesh.BackPropropagate();

            // [TODO] output loop somehow...use MeshConstraints.FindConstrainedEdgesBySetID(set_id)...

            return(true);
        }         // Trim()
コード例 #8
0
        public void Close_Flat()
        {
            double minlen, maxlen, avglen;

            MeshQueries.EdgeLengthStats(Mesh, out minlen, out maxlen, out avglen, 1000);
            double target_edge_len = (TargetEdgeLen <= 0) ? avglen : TargetEdgeLen;

            // massage around boundary loop
            List <int> refinedBorderEdges;

            cleanup_boundary(Mesh, InitialBorderLoop, avglen, out refinedBorderEdges, 3);

            // find new border loop. try to find new loop containing edges from loop we refined in cleanup_boundary,
            // if that fails just use largest loop.
            MeshBoundaryLoops loops = new MeshBoundaryLoops(Mesh);
            int iloop = loops.FindLoopContainingEdge(refinedBorderEdges[0]);

            if (iloop == -1)
            {
                iloop = loops.MaxVerticesLoopIndex;
            }
            EdgeLoop fill_loop = loops.Loops[iloop];

            int extrude_group = (ExtrudeGroup == -1) ? Mesh.AllocateTriangleGroup() : ExtrudeGroup;
            int fill_group    = (FillGroup == -1) ? Mesh.AllocateTriangleGroup() : FillGroup;

            // decide on projection plane
            //AxisAlignedBox3d loopbox = fill_loop.GetBounds();
            //Vector3d topPt = loopbox.Center;
            //if ( bIsUpper ) {
            //    topPt.y = loopbox.Max.y + 0.25 * dims.y;
            //} else {
            //    topPt.y = loopbox.Min.y - 0.25 * dims.y;
            //}
            //Frame3f plane = new Frame3f((Vector3f)topPt);

            // extrude loop to this plane
            MeshExtrudeLoop extrude = new MeshExtrudeLoop(Mesh, fill_loop);

            extrude.PositionF = (v, n, i) => {
                return(FlatClosePlane.ProjectToPlane((Vector3f)v, 1));
            };
            extrude.Extrude(extrude_group);
            MeshValidation.IsBoundaryLoop(Mesh, extrude.NewLoop);

            Debug.Assert(Mesh.CheckValidity());

            // smooth the extrude loop
            MeshLoopSmooth loop_smooth = new MeshLoopSmooth(Mesh, extrude.NewLoop);

            loop_smooth.ProjectF = (v, i) => {
                return(FlatClosePlane.ProjectToPlane((Vector3f)v, 1));
            };
            loop_smooth.Alpha  = 0.5f;
            loop_smooth.Rounds = 100;
            loop_smooth.Smooth();

            Debug.Assert(Mesh.CheckValidity());

            // fill result
            SimpleHoleFiller filler = new SimpleHoleFiller(Mesh, extrude.NewLoop);

            filler.Fill(fill_group);

            Debug.Assert(Mesh.CheckValidity());

            // make selection for remesh region
            MeshFaceSelection remesh_roi = new MeshFaceSelection(Mesh);

            remesh_roi.Select(extrude.NewTriangles);
            remesh_roi.Select(filler.NewTriangles);
            remesh_roi.ExpandToOneRingNeighbours();
            remesh_roi.ExpandToOneRingNeighbours();
            remesh_roi.LocalOptimize(true, true);
            int[] new_roi = remesh_roi.ToArray();

            // get rid of extrude group
            FaceGroupUtil.SetGroupToGroup(Mesh, extrude_group, 0);

            /*  clean up via remesh
             *     - constrain loop we filled to itself
             */

            RegionRemesher r = new RegionRemesher(Mesh, new_roi);

            DCurve3 top_curve = MeshUtil.ExtractLoopV(Mesh, extrude.NewLoop.Vertices);
            DCurveProjectionTarget curve_target = new DCurveProjectionTarget(top_curve);

            int[] top_loop = (int[])extrude.NewLoop.Vertices.Clone();
            r.Region.MapVerticesToSubmesh(top_loop);
            MeshConstraintUtil.ConstrainVtxLoopTo(r.Constraints, r.Mesh, top_loop, curve_target);

            DMeshAABBTree3 spatial = new DMeshAABBTree3(Mesh);

            spatial.Build();
            MeshProjectionTarget target = new MeshProjectionTarget(Mesh, spatial);

            r.SetProjectionTarget(target);

            bool bRemesh = true;

            if (bRemesh)
            {
                r.Precompute();
                r.EnableFlips     = r.EnableSplits = r.EnableCollapses = true;
                r.MinEdgeLength   = target_edge_len;
                r.MaxEdgeLength   = 2 * target_edge_len;
                r.EnableSmoothing = true;
                r.SmoothSpeedT    = 1.0f;
                for (int k = 0; k < 40; ++k)
                {
                    r.BasicRemeshPass();
                }
                r.SetProjectionTarget(null);
                r.SmoothSpeedT = 0.25f;
                for (int k = 0; k < 10; ++k)
                {
                    r.BasicRemeshPass();
                }
                Debug.Assert(Mesh.CheckValidity());

                r.BackPropropagate();
            }

            // smooth around the join region to clean up ugliness
            smooth_region(Mesh, r.Region.BaseBorderV, 3);
        }
コード例 #9
0
        void generate_graph(DenseGrid3f supportGrid, DenseGridTrilinearImplicit distanceField)
        {
            int ni = supportGrid.ni, nj = supportGrid.nj, nk = supportGrid.nk;
            float dx = (float)CellSize;
            Vector3f origin = this.GridOrigin;

            // parameters for initializing cost grid
            float MODEL_SPACE = 0.01f;      // needs small positive so that points on triangles count as inside (eg on ground plane)
            //float MODEL_SPACE = 2.0f*(float)CellSize;
            float CRAZY_DISTANCE = 99999.0f;
            bool UNIFORM_DISTANCE = true;
            float MAX_DIST = 10 * (float)CellSize;

            // parameters for sorting seeds
            Vector3i center_idx = new Vector3i(ni / 2, 0, nk / 2);      // middle
            //Vector3i center_idx = new Vector3i(0, 0, 0);              // corner
            bool reverse_per_layer = true;

            DenseGrid3f costGrid = new DenseGrid3f(supportGrid);
            foreach ( Vector3i ijk in costGrid.Indices() ) {
                Vector3d cell_center = new Vector3f(ijk.x * dx, ijk.y * dx, ijk.z * dx) + origin;
                float f = (float)distanceField.Value(ref cell_center);
                if (f <= MODEL_SPACE)
                    f = CRAZY_DISTANCE;
                else if (UNIFORM_DISTANCE)
                    f = 1.0f;
                else if (f > MAX_DIST)
                    f = MAX_DIST;
                costGrid[ijk] = f;
            }

            // Find seeds on each layer, sort, and add to accumulated bottom-up seeds list.
            // This sorting has an *enormous* effect on the support generation.

            List<Vector3i> seeds = new List<Vector3i>();
            List<Vector3i> layer_seeds = new List<Vector3i>();
            for (int j = 0; j < nj; ++j) {
                layer_seeds.Clear();
                for (int k = 0; k < nk; ++k) {
                    for (int i = 0; i < ni; ++i) {
                        if (supportGrid[i, j, k] == SUPPORT_TIP_BASE)
                            layer_seeds.Add(new Vector3i(i, j, k));
                    }
                }

                layer_seeds.Sort((a, b) => {
                    Vector3i pa = a; pa.y = 0;
                    Vector3i pb = b; pb.y = 0;
                    int sa = (pa-center_idx).LengthSquared, sb = (pb-center_idx).LengthSquared;
                    return sa.CompareTo(sb);
                });

                // reversing sort order is intresting?
                if(reverse_per_layer)
                    layer_seeds.Reverse();

                seeds.AddRange(layer_seeds);
            }
            HashSet<Vector3i> seed_indices = new HashSet<Vector3i>(seeds);

            // gives very different results...
            if (ProcessBottomUp == false)
                seeds.Reverse();

            // for linear index a, is this a node we allow in graph? (ie graph bounds)
            Func<int, bool> node_filter_f = (a) => {
                Vector3i ai = costGrid.to_index(a);
                // why not y check??
                return ai.x > 0 &&  ai.z > 0 && ai.x != ni - 1 && ai.y != nj - 1 && ai.z != nk - 1;
            };

            // distance from linear index a to linear index b
            // this defines the cost field we want to find shortest path through
            Func<int, int, float> node_dist_f = (a, b) => {
                Vector3i ai = costGrid.to_index(a), bi = costGrid.to_index(b);
                if (bi.y >= ai.y)               // b.y should always be a.y-1
                    return float.MaxValue;
                float sg = supportGrid[bi];

                // don't connect to tips
                //if (sg == SUPPORT_TIP_BASE || sg == SUPPORT_TIP_TOP)
                //    return float.MaxValue;
                if (sg == SUPPORT_TIP_TOP)
                    return float.MaxValue;

                if (sg < 0)
                    return -999999;    // if b is already used, we will terminate there, so this is a good choice

                // otherwise cost is sqr-grid-distance + costGrid value  (which is basically distance to surface)
                float c = costGrid[b];
                float f = (float)(Math.Sqrt((bi - ai).LengthSquared) * CellSize);
                //float f = 0;
                return c + f;
            };

            // which linear-index nbrs to consider for linear index a
            Func<int, IEnumerable<int>> neighbour_f = (a) => {
                Vector3i ai = costGrid.to_index(a);
                return down_neighbours(ai, costGrid);
            };

            // when do we terminate
            Func<int, bool> terminate_f = (a) => {
                Vector3i ai = costGrid.to_index(a);
                // terminate if we hit existing support path
                if (seed_indices.Contains(ai) == false && supportGrid[ai] < 0)
                    return true;
                // terminate if we hit ground plane
                if (ai.y == 0)
                    return true;
                return false;
            };

            DijkstraGraphDistance dijkstra = new DijkstraGraphDistance(ni * nj * nk, false,
                node_filter_f, node_dist_f, neighbour_f);
            dijkstra.TrackOrder = true;

            List<int> path = new List<int>();

            Graph = new DGraph3();
            Dictionary<Vector3i, int> CellToGraph = new Dictionary<Vector3i, int>();
            TipVertices = new HashSet<int>();
            TipBaseVertices = new HashSet<int>();
            GroundVertices = new HashSet<int>();

            // seeds are tip-base points
            for (int k = 0; k < seeds.Count; ++k) {
                // add seed point (which is a tip-base vertex) as seed for dijkstra prop
                int seed = costGrid.to_linear(seeds[k]);
                dijkstra.Reset();
                dijkstra.AddSeed(seed, 0);

                // compute to termination (ground, existing node, etc)
                int base_node = dijkstra.ComputeToNode(terminate_f);
                if (base_node < 0)
                    base_node = dijkstra.GetOrder().Last();

                // extract the path
                path.Clear();
                dijkstra.GetPathToSeed(base_node, path);
                int N = path.Count;

                // first point on path is termination point.
                // create vertex for it if we have not yet
                Vector3i basept_idx = supportGrid.to_index(path[0]);
                int basept_vid;
                if ( CellToGraph.TryGetValue(basept_idx, out basept_vid) == false ) {
                    Vector3d curv = get_cell_center(basept_idx);
                    if (basept_idx.y == 0) {
                        curv.y = 0;
                    }
                    basept_vid = Graph.AppendVertex(curv);
                    if (basept_idx.y == 0) {
                        GroundVertices.Add(basept_vid);
                    }
                    CellToGraph[basept_idx] = basept_vid;
                }

                int cur_vid = basept_vid;

                // now walk up path and create vertices as necessary
                for (int i = 0; i < N; ++i) {
                    int idx = path[i];
                    if ( supportGrid[idx] >= 0 )
                        supportGrid[idx] = SUPPORT_GRID_USED;
                    if ( i > 0 ) {
                        Vector3i next_idx = supportGrid.to_index(path[i]);
                        int next_vid;
                        if (CellToGraph.TryGetValue(next_idx, out next_vid) == false) {
                            Vector3d nextv = get_cell_center(next_idx);
                            next_vid = Graph.AppendVertex(nextv);
                            CellToGraph[next_idx] = next_vid;
                        }
                        Graph.AppendEdge(cur_vid, next_vid);
                        cur_vid = next_vid;
                    }
                }

                // seed was tip-base so we should always get back there. Then we
                // explicitly add tip-top and edge to it.
                if ( supportGrid[path[N-1]] == SUPPORT_TIP_BASE ) {
                    Vector3i vec_idx = supportGrid.to_index(path[N-1]);
                    TipBaseVertices.Add(CellToGraph[vec_idx]);

                    Vector3i tip_idx = vec_idx + Vector3i.AxisY;
                    int tip_vid;
                    if (CellToGraph.TryGetValue(tip_idx, out tip_vid) == false) {
                        Vector3d tipv = get_cell_center(tip_idx);
                        tip_vid = Graph.AppendVertex(tipv);
                        CellToGraph[tip_idx] = tip_vid;
                        Graph.AppendEdge(cur_vid, tip_vid);
                        TipVertices.Add(tip_vid);
                    }
                }

            }

            /*
             * Snap tips to surface
             */

            gParallel.ForEach(TipVertices, (tip_vid) => {
                bool snapped = false;
                Vector3d v = Graph.GetVertex(tip_vid);
                Frame3f hitF;
                // try shooting ray straight up. if that hits, and point is close, we use it
                if (MeshQueries.RayHitPointFrame(Mesh, MeshSpatial, new Ray3d(v, Vector3d.AxisY), out hitF)) {
                    if (v.Distance(hitF.Origin) < 2 * CellSize) {
                        v = hitF.Origin;
                        snapped = true;
                    }
                }

                // if that failed, try straight down
                if (!snapped) {
                    if (MeshQueries.RayHitPointFrame(Mesh, MeshSpatial, new Ray3d(v, -Vector3d.AxisY), out hitF)) {
                        if (v.Distance(hitF.Origin) < CellSize) {
                            v = hitF.Origin;
                            snapped = true;
                        }
                    }
                }

                // if it missed, or hit pt was too far, find nearest point and try that
                if (!snapped) {
                    hitF = MeshQueries.NearestPointFrame(Mesh, MeshSpatial, v);
                    if (v.Distance(hitF.Origin) < 2 * CellSize) {
                        v = hitF.Origin;
                        snapped = true;
                    }
                    // can this ever fail? tips should always be within 2 cells...
                }
                if (snapped)
                    Graph.SetVertex(tip_vid, v);
            });
        }
コード例 #10
0
        void constrained_smooth(DGraph3 graph, double surfDist, double dotThresh, double alpha, int rounds)
        {
            int NV = graph.MaxVertexID;
            Vector3d[] pos = new Vector3d[NV];

            for (int ri = 0; ri < rounds; ++ri) {

                gParallel.ForEach(graph.VertexIndices(), (vid) => {
                    Vector3d v = graph.GetVertex(vid);

                    if ( GroundVertices.Contains(vid) || TipVertices.Contains(vid) ) {
                        pos[vid] = v;
                        return;
                    }

                    // for tip base vertices, we could allow them to move down and away within angle cone...
                    if (TipBaseVertices.Contains(vid)) {
                        pos[vid] = v;
                        return;
                    }

                    // compute smoothed position of vtx
                    Vector3d centroid = Vector3d.Zero; int nbr_count = 0;
                    foreach (int nbr_vid in graph.VtxVerticesItr(vid)) {
                        centroid += graph.GetVertex(nbr_vid);
                        nbr_count++;
                    }
                    if (nbr_count == 1) {
                        pos[vid] = v;
                        return;
                    }
                    centroid /= nbr_count;
                    Vector3d vnew = (1 - alpha) * v + (alpha) * centroid;

                    // make sure we don't violate angle constraint to any nbrs
                    int attempt = 0;
                    try_again:
                    foreach ( int nbr_vid in graph.VtxVerticesItr(vid)) {
                        Vector3d dv = graph.GetVertex(nbr_vid) - vnew;
                        dv.Normalize();
                        double dot = dv.Dot(Vector3d.AxisY);
                        if ( Math.Abs(dot) < dotThresh ) {
                            if (attempt++ < 3) {
                                vnew = Vector3d.Lerp(v, vnew, 0.66);
                                goto try_again;
                            } else {
                                pos[vid] = v;
                                return;
                            }
                        }
                    }

                    // offset from nearest point on surface
                    Frame3f fNearest = MeshQueries.NearestPointFrame(Mesh, MeshSpatial, vnew, true);
                    Vector3d vNearest = fNearest.Origin;
                    double dist = vnew.Distance(vNearest);
                    bool inside = MeshSpatial.IsInside(vnew);

                    if (inside || dist < surfDist) {
                        Vector3d normal = fNearest.Z;
                        // don't push down?
                        if (normal.Dot(Vector3d.AxisY) < 0) {
                            normal.y = 0; normal.Normalize();
                        }
                        vnew = fNearest.Origin + surfDist * normal;
                    }

                    pos[vid] = vnew;
                });

                foreach (int vid in graph.VertexIndices())
                    graph.SetVertex(vid, pos[vid]);
            }
        }