protected DMesh3 MakeDebugGraphMesh()
{
DMesh3 graphMesh = new DMesh3();
graphMesh.EnableVertexColors(Vector3f.One);
foreach (int vid in Graph.VertexIndices())
{
if (TipVertices.Contains(vid))
{
MeshEditor.AppendBox(graphMesh, Graph.GetVertex(vid), 0.3f, Colorf.Green);
}
else if (TipBaseVertices.Contains(vid))
{
MeshEditor.AppendBox(graphMesh, Graph.GetVertex(vid), 0.225f, Colorf.Magenta);
}
else if (GroundVertices.Contains(vid))
{
MeshEditor.AppendBox(graphMesh, Graph.GetVertex(vid), 0.35f, Colorf.Blue);
}
else
{
MeshEditor.AppendBox(graphMesh, Graph.GetVertex(vid), 0.15f, Colorf.White);
}
}
foreach (int eid in Graph.EdgeIndices())
{
Segment3d seg = Graph.GetEdgeSegment(eid);
MeshEditor.AppendLine(graphMesh, seg, 0.1f);
}
return(graphMesh);
}
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]);
}
}
}
