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 = (V3d.Distance(planePoint, position) - majorRadius).Abs();
var distanceToCircle = (V3d.DistanceSquared(planePoint, p) + distanceOnPlane.Square()).Sqrt();
return((distanceToCircle - minorRadius).Abs());
}
/// <summary>
/// Creates clusters for all points which are within an epsilon
/// distance from each other. This algorithm uses a hash-grid and
/// only works fast if the supplied epsilon is small enough that
/// not too many points fall within each cluster. Thus it is ideal
/// for merging vertices in meshes and point sets, that are different
/// due to numerical inaccuracies.
/// </summary>
public PointEpsilonClustering(V3d[] pa, double epsilon, IRandomUniform rnd = null)
{
rnd = rnd ?? new RandomSystem();
var count = pa.Length;
Alloc(count);
var ca = m_indexArray;
var dict = new IntDict <int>(count, stackDuplicateKeys: true);
int rndBits = 0; int bit = 0;
var ha = new int[8];
var eps2 = epsilon * epsilon;
for (int i = 0; i < count; i++)
{
var p = pa[i]; p.HashCodes8(epsilon, ha);
int ci = ca[i]; if (ca[ci] != ci)
{
do
{
ci = ca[ci];
} while (ca[ci] != ci); ca[i] = ci;
}
for (int hi = 0; hi < 8; hi++)
{
foreach (var j in dict.ValuesWithKey(ha[hi]))
{
int cj = ca[j]; if (ca[cj] != cj)
{
do
{
cj = ca[cj];
} while (ca[cj] != cj); ca[j] = cj;
}
if (ci == cj || V3d.DistanceSquared(p, pa[j]) >= eps2)
{
continue;
}
bit >>= 1; if (bit == 0)
{
rndBits = rnd.UniformInt(); bit = 1 << 30;
}
if ((rndBits & bit) != 0)
{
ca[ci] = cj; ca[i] = cj; ci = cj;
}
else
{
ca[cj] = ci; ca[j] = ci;
}
}
}
dict[ha[0]] = i;
}
Init();
}
public bool ClosestPoint(int[] objectIndexArray, int firstIndex, int indexCount, V3d queryPoint, Func <IIntersectableObjectSet, int, bool> ios_index_objectFilter, Func <IIntersectableObjectSet, int, int, ObjectClosestPoint, bool> ios_index_part_ocp_pointFilter, ref ObjectClosestPoint closestPoint)
{
var minDist2 = closestPoint.DistanceSquared;
var minIndex = -1;
var minPos = V3d.NaN;
for (int i = 0; i < indexCount; i++)
{
var id = objectIndexArray[firstIndex + i];
if (ios_index_objectFilter(this, id))
{
GetTriangle(id, out V3d p0, out V3d p1, out V3d p2);
var p = queryPoint.GetClosestPointOnTriangle(p0, p1, p2);
var d = V3d.DistanceSquared(p, queryPoint);
if (d < minDist2)
{
d = minDist2;
minIndex = id;
minPos = p;
}
}
}
if (minIndex >= 0)
{
closestPoint = new ObjectClosestPoint()
{
Distance = Fun.Sqrt(minDist2),
DistanceSquared = minDist2,
Point = minPos,
SetObject = new SetObject(this, minIndex),
ObjectStack = new List <SetObject>(), // TODO
Coord = V2d.Zero // TODO
};
return(true);
}
else
{
return(false);
}
}
/// <summary>
///
/// </summary>
/// <param name="objectIndexArray"></param>
/// <param name="firstIndex"></param>
/// <param name="indexCount"></param>
/// <param name="query"></param>
/// <param name="ios_index_objectFilter">not implemented</param>
/// <param name="ios_index_part_ocp_pointFilter">not implemented</param>
/// <param name="closest"></param>
/// <returns></returns>
public bool ClosestPoint(
int[] objectIndexArray, int firstIndex, int indexCount,
V3d query,
Func <IIntersectableObjectSet, int, bool> ios_index_objectFilter,
Func <IIntersectableObjectSet, int, int, ObjectClosestPoint, bool> ios_index_part_ocp_pointFilter,
ref ObjectClosestPoint closest)
{
bool result = false;
for (int i = firstIndex, e = firstIndex + indexCount; i < e; i++)
{
int index = objectIndexArray[i];
V3d p = query.GetClosestPointOn(Sphere3ds[index]);
double d2 = V3d.DistanceSquared(query, p);
if (d2 < closest.DistanceSquared)
{
result = closest.Set(d2, p, this, index);
}
}
return(result);
}
public bool ClosestPoint(
int[] objectIndexArray, int firstIndex, int indexCount, V3d queryPoint,
Func <IIntersectableObjectSet, int, bool> ios_index_objectFilter,
Func <IIntersectableObjectSet, int, int, ObjectClosestPoint, bool> ios_index_part_ocp_pointFilter,
ref ObjectClosestPoint closestPoint)
{
bool result = false;
var pa = m_positionArray;
for (int i = firstIndex, e = firstIndex + indexCount; i < e; i++)
{
int li = objectIndexArray[i];
V3d p = queryPoint.GetClosestPointOnLine(pa[li], pa[li + 1]);
double d2 = V3d.DistanceSquared(queryPoint, p);
if (d2 < closestPoint.DistanceSquared)
{
result = closestPoint.Set(d2, p, this, li);
}
}
return(result);
}
public bool ClosestPoint(
int[] objectIndexArray, int firstIndex, int indexCount,
V3d query,
Func <IIntersectableObjectSet, int, bool> ios_index_objectFilter,
Func <IIntersectableObjectSet, int, int, ObjectClosestPoint, bool> ios_index_part_ocp_pointFilter,
ref ObjectClosestPoint closest)
{
var plist = Position3dList;
bool result = false;
for (int i = firstIndex, e = firstIndex + indexCount; i < e; i++)
{
int index = objectIndexArray[i];
int pi = index * 3;
V3d p = query.GetClosestPointOnTriangle(
plist[pi], plist[pi + 1], plist[pi + 2]);
double d2 = V3d.DistanceSquared(query, p);
if (d2 < closest.DistanceSquared)
{
result = closest.Set(d2, p, this, index);
}
}
return(result);
}
public bool ClosestPoint(
int[] objectIndexArray, int firstIndex, int indexCount,
V3d queryPoint,
Func <IIntersectableObjectSet, int, bool> ios_index_objectFilter,
Func <IIntersectableObjectSet, int, int, ObjectClosestPoint, bool> ios_index_part_ocp_pointFilter,
ref ObjectClosestPoint closestPoint)
{
bool result = false;
int[] fia = FirstIndexArray, via = VertexIndexArray;
var pa = PositionArray;
for (int i = firstIndex, e = firstIndex + indexCount; i < e; i++)
{
int oi = objectIndexArray[i];
if (ios_index_objectFilter != null &&
!ios_index_objectFilter(this, oi))
{
continue;
}
int fvi = fia[oi], fve = fia[oi + 1];
V3d p0 = pa[via[fvi++]], p1 = pa[via[fvi++]];
while (fvi < fve)
{
var p2 = pa[via[fvi++]];
V3d p = queryPoint.GetClosestPointOnTriangle(p0, p1, p2);
double d2 = V3d.DistanceSquared(queryPoint, p);
if (d2 < closestPoint.DistanceSquared)
{
result = closestPoint.Set(d2, p, this, oi);
}
p1 = p2;
}
}
return(result);
}
public bool Contains(V3d pt)
{
return(V3d.DistanceSquared(m_sphere.Center, pt) <= m_radiusSquared);
}
/// <summary>
/// Creates clusters of planes within a certain epsilon from each other
/// (euclidean distance between normal vectors and between offsets).
/// This algorithm uses a 4d hash-grid and only works fast if the supplied
/// epsilons are small enough that not too many planes fall within each cluster.
/// Thus it is ideal for merging planes with small variations in orientation and
/// offset due to numerical inaccuracies.
/// </summary>
public PlaneEpsilonClustering(
int count, TArray pa,
Func <TArray, int, V3d> getNormal,
Func <TArray, int, double> getDist,
double epsNormal, double epsDist,
double deltaEpsFactor = 0.25, IRandomUniform rnd = null)
{
rnd = rnd ?? new RandomSystem();
Alloc(count);
var ca = m_indexArray;
var dict = new IntDict <int>(count, stackDuplicateKeys: true);
int rndBits = 0; int bit = 0;
var ha = new int[16];
var ne2 = epsNormal * epsNormal;
var de2 = epsDist * epsDist;
var deps = (ne2 + de2) * deltaEpsFactor * deltaEpsFactor;
for (int i = 0; i < count; i++)
{
var ni = getNormal(pa, i); var di = getDist(pa, i);
ni.HashCodes16(di, epsNormal, epsDist, ha);
int ci = ca[i]; if (ca[ci] != ci)
{
do
{
ci = ca[ci];
} while (ca[ci] != ci); ca[i] = ci;
}
double dmin = double.MaxValue;
for (int hi = 0; hi < 16; hi++)
{
foreach (var j in dict.ValuesWithKey(ha[hi]))
{
int cj = ca[j]; if (ca[cj] != cj)
{
do
{
cj = ca[cj];
} while (ca[cj] != cj); ca[j] = cj;
}
if (ci == cj)
{
continue;
}
var dd = Fun.Square(di - getDist(pa, j)); if (dd >= de2)
{
continue;
}
var dn = V3d.DistanceSquared(ni, getNormal(pa, j)); if (dn >= ne2)
{
continue;
}
var d = dn + dd; if (d < dmin)
{
dmin = d;
}
bit >>= 1; if (bit == 0)
{
rnd.UniformInt(); bit = 1 << 30;
}
if ((rndBits & bit) != 0)
{
ca[ci] = cj; ca[i] = cj; ci = cj;
}
else
{
ca[cj] = ci; ca[j] = ci;
}
}
}
if (dmin > deps)
{
dict[ha[0]] = i; // only sparsely populate hashtable for performance reasons
}
}
Init();
}
