// find distance x0 is from triangle x1-x2-x3
static float point_triangle_distance(ref Vector3f x0, ref Vector3f x1, ref Vector3f x2, ref Vector3f x3)
{
// first find barycentric coordinates of closest point on infinite plane
Vector3f x13 = (x1 - x3);
Vector3f x23 = (x2 - x3);
Vector3f x03 = (x0 - x3);
float m13 = x13.LengthSquared, m23 = x23.LengthSquared, d = x13.Dot(x23);
float invdet = 1.0f / Math.Max(m13 * m23 - d * d, 1e-30f);
float a = x13.Dot(x03), b = x23.Dot(x03);
// the barycentric coordinates themselves
float w23 = invdet * (m23 * a - d * b);
float w31 = invdet * (m13 * b - d * a);
float w12 = 1 - w23 - w31;
if (w23 >= 0 && w31 >= 0 && w12 >= 0) // if we're inside the triangle
{
return(x0.Distance(w23 * x1 + w31 * x2 + w12 * x3));
}
else // we have to clamp to one of the edges
{
if (w23 > 0) // this rules out edge 2-3 for us
{
return(Math.Min(point_segment_distance(ref x0, ref x1, ref x2), point_segment_distance(ref x0, ref x1, ref x3)));
}
else if (w31 > 0) // this rules out edge 1-3
{
return(Math.Min(point_segment_distance(ref x0, ref x1, ref x2), point_segment_distance(ref x0, ref x2, ref x3)));
}
else // w12 must be >0, ruling out edge 1-2
{
return(Math.Min(point_segment_distance(ref x0, ref x1, ref x3), point_segment_distance(ref x0, ref x2, ref x3)));
}
}
}
void update_neighbours_sparse(GraphNode parent)
{
Vector3f parentPos = PositionF(parent.id);
float parentDist = parent.graph_distance;
foreach (int nbr_id in NeighboursF(parent.id))
{
GraphNode nbr = get_node(nbr_id);
if (nbr.frozen)
{
continue;
}
float nbr_dist = parentDist + parentPos.Distance(PositionF(nbr_id));
if (SparseQueue.Contains(nbr))
{
if (nbr_dist < nbr.priority)
{
nbr.parent = parent;
nbr.graph_distance = nbr_dist;
SparseQueue.Update(nbr, nbr.graph_distance);
}
}
else
{
nbr.parent = parent;
nbr.graph_distance = nbr_dist;
SparseQueue.Enqueue(nbr, nbr.graph_distance);
}
}
}
// find distance x0 is from segment x1-x2
static float point_segment_distance(ref Vector3f x0, ref Vector3f x1, ref Vector3f x2)
{
Vector3f dx = x2 - x1;
float m2 = dx.LengthSquared;
// find parameter value of closest point on segment
float s12 = (dx.Dot(x2 - x0) / m2);
if (s12 < 0)
{
s12 = 0;
}
else if (s12 > 1)
{
s12 = 1;
}
// and find the distance
return(x0.Distance(s12 * x1 + (1 - s12) * x2));
}
protected virtual DMesh3 BuildMesh_TolerantWeld(STLSolid solid, double weld_tolerance)
{
var builder = new DMesh3Builder();
builder.AppendNewMesh(false, false, false, false);
DVectorArray3f vertices = solid.Vertices;
int N = vertices.Count;
int[] mapV = new int[N];
AxisAlignedBox3d bounds = AxisAlignedBox3d.Empty;
for (int i = 0; i < N; ++i)
{
bounds.Contain(vertices[i]);
}
// [RMS] because we are only searching within tiny radius, there is really no downside to
// using lots of bins here, except memory usage. If we don't, and the mesh has a ton of triangles
// very close together (happens all the time on big meshes!), then this step can start
// to take an *extremely* long time!
int num_bins = 256;
if (N > 100000)
{
num_bins = 512;
}
if (N > 1000000)
{
num_bins = 1024;
}
if (N > 2000000)
{
num_bins = 2048;
}
if (N > 5000000)
{
num_bins = 4096;
}
var uniqueV = new PointHashGrid3d <int>(bounds.MaxDim / (float)num_bins, -1);
var pos = new Vector3f[N];
for (int vi = 0; vi < N; ++vi)
{
Vector3f v = vertices[vi];
var pair = uniqueV.FindNearestInRadius(v, weld_tolerance, (vid) =>
{
return(v.Distance(pos[vid]));
});
if (pair.Key == -1)
{
int vid = builder.AppendVertex(v.x, v.y, v.z);
uniqueV.InsertPoint(vid, v);
mapV[vi] = vid;
pos[vid] = v;
}
else
{
mapV[vi] = pair.Key;
}
}
append_mapped_triangles(solid, builder, mapV);
return(builder.Meshes[0]);
}
