public static double GetMinimalDistance(V3d p, V3d position, V3d direction, double majorRadius, double minorRadius)
{
var plane = new Plane3d(direction, position);
var planePoint = p.GetClosestPointOn(plane);
var distanceOnPlane = (Vec.Distance(planePoint, position) - majorRadius).Abs();
var distanceToCircle = (Vec.DistanceSquared(planePoint, p) + distanceOnPlane.Square()).Sqrt();
return((distanceToCircle - minorRadius).Abs());
}
public static void ComputeCircumCircleSquared(
V2d p0, V2d p1, V2d p2,
out V2d center, out double radiusSquared)
{
double y01abs = System.Math.Abs(p0.Y - p1.Y);
double y12abs = System.Math.Abs(p1.Y - p2.Y);
if (y01abs < Constant <double> .PositiveTinyValue &&
y12abs < Constant <double> .PositiveTinyValue)
{
center = V2d.NaN; radiusSquared = -1.0; return;
}
double xc, yc;
if (y01abs < Constant <double> .PositiveTinyValue)
{
double m2 = (p1.X - p2.X) / (p2.Y - p1.Y);
double m12x = 0.5 * (p1.X + p2.X);
double m12y = 0.5 * (p1.Y + p2.Y);
xc = 0.5 * (p1.X + p0.X);
yc = m2 * (xc - m12x) + m12y;
}
else if (y12abs < Constant <double> .PositiveTinyValue)
{
double m1 = (p0.X - p1.X) / (p1.Y - p0.Y);
double m01x = 0.5 * (p0.X + p1.X);
double m01y = 0.5 * (p0.Y + p1.Y);
xc = 0.5 * (p2.X + p1.X);
yc = m1 * (xc - m01x) + m01y;
}
else
{
double m1 = (p0.X - p1.X) / (p1.Y - p0.Y);
double m2 = (p1.X - p2.X) / (p2.Y - p1.Y);
double m01x = 0.5 * (p0.X + p1.X);
double m01y = 0.5 * (p0.Y + p1.Y);
double m12x = 0.5 * (p1.X + p2.X);
double m12y = 0.5 * (p1.Y + p2.Y);
double m12 = m1 - m2;
if (System.Math.Abs(m12) < Constant <double> .PositiveTinyValue)
{
center = V2d.NaN; radiusSquared = -1.0; return;
}
xc = (m1 * m01x - m2 * m12x + m12y - m01y) / m12;
if (y01abs > y12abs)
{
yc = m1 * (xc - m01x) + m01y;
}
else
{
yc = m2 * (xc - m12x) + m12y;
}
}
center = new V2d(xc, yc);
radiusSquared = Vec.DistanceSquared(p0, center);
}
public static __tpolygon__ WithoutMultiplePoints(
this __tpolygon__ polygon, double eps = 1e-8)
{
eps *= eps;
var opc = polygon.PointCount;
var pa = new __tvec__[opc];
var pc = 0;
pa[0] = polygon[0];
for (int pi = 1; pi < opc; pi++)
{
if (Vec.DistanceSquared(pa[pc], polygon[pi]) > eps)
{
pa[++pc] = polygon[pi];
}
}
if (Vec.DistanceSquared(pa[pc], polygon[0]) > eps)
{
++pc;
}
return(new __tpolygon__(pa, pc));
}
/// <summary>
/// Compute the Delaunay triangluation of the supplied points. Note that
/// the supplied point array must be by 3 larger than the actual number of
/// points.
/// </summary>
private static void Triangulate2d(V2d[] pa,
out Triangle1i[] ta, out int triangleCount)
{
int vc = pa.Length - 4;
int tcMax = 2 * vc + 2; // sharp upper bound with no degenerates
int ecMax = 6 * vc - 3; // sharp bound: last step replaces all tris
ta = new Triangle1i[tcMax];
var ra = new double[tcMax];
var ca = new V2d[tcMax];
var ea = new Line1i[ecMax];
// -------------------------------------- set up the supertriangle
ta[0] = new Triangle1i(vc, vc + 2, vc + 1);
ra[0] = -1.0;
ta[1] = new Triangle1i(vc, vc + 3, vc + 2);
ra[1] = -1.0;
int tc = 2, cc = 0; // triangle count, complete count
// ------------- superquad vertices at the end of vertex array
var box = new Box2d(pa.Take(vc)).EnlargedBy(0.1);
V2d center = box.Center, size = box.Size;
pa[vc + 0] = box.Min;
pa[vc + 1] = new V2d(box.Min.X, box.Max.Y);
pa[vc + 2] = box.Max;
pa[vc + 3] = new V2d(box.Max.X, box.Min.Y);
// ------------------------------ include the points one at a time
for (int i = 0; i < vc; i++)
{
V2d p = pa[i];
int ec = 0;
/*
* if the point lies inside the circumcircle then the triangle
* is removed and its edges are added to the edge array
*/
for (int ti = cc; ti < tc; ti++)
{
var tr = ta[ti];
double r2 = ra[ti];
if (r2 < 0.0)
{
Triangle2d.ComputeCircumCircleSquared(
pa[tr.I0], pa[tr.I1], pa[tr.I2],
out center, out r2);
ra[ti] = r2; ca[ti] = center;
}
else
{
center = ca[ti];
}
// ---------------- include this if points are sorted by X
if (center.X < p.X && (p.X - center.X).Square() > r2)
{
Fun.Swap(ref ta[ti], ref ta[cc]);
ra[ti] = ra[cc]; ca[ti] = ca[cc];
++cc; continue;
}
if (Vec.DistanceSquared(p, center) <= r2)
{
int nec = ec + 3;
if (nec >= ecMax)
{
ecMax = Fun.Max(nec, (int)(1.1 * (double)ecMax));
Array.Resize(ref ea, ecMax);
}
ea[ec] = tr.Line01; ea[ec + 1] = tr.Line12; ea[ec + 2] = tr.Line20;
--tc; ec = nec;
ta[ti] = ta[tc]; ra[ti] = ra[tc]; ca[ti] = ca[tc];
--ti;
}
}
// ---------------------------------------- tag multiple edges
for (int ei = 0; ei < ec - 1; ei++)
{
for (int ej = ei + 1; ej < ec; ej++)
{
if (ea[ei].I0 == ea[ej].I1 && ea[ei].I1 == ea[ej].I0)
{
ea[ei] = Line1i.Invalid; ea[ej] = Line1i.Invalid;
}
}
}
// ------------------ form new triangles for the current point
for (int ei = 0; ei < ec; ei++)
{
var e = ea[ei];
if (e.I0 < 0 || e.I1 < 0)
{
continue; // skip tagged edges
}
if (tc >= tcMax) // necessary for degenerate cases
{
tcMax = Fun.Max(tcMax + 1, (int)(1.1 * (double)tcMax));
Array.Resize(ref ta, tcMax);
Array.Resize(ref ra, tcMax);
Array.Resize(ref ca, tcMax);
}
ta[tc] = new Triangle1i(e.I0, e.I1, i);
ra[tc++] = -1.0;
}
}
// ------------------ remove triangles with supertriangle vertices
for (int ti = 0; ti < tc; ti++)
{
if (ta[ti].I0 >= vc || ta[ti].I1 >= vc || ta[ti].I2 >= vc)
{
ta[ti--] = ta[--tc];
}
}
triangleCount = tc;
}
