public V3d GetClosestPoint(V3d point)
{
var ray = new Ray3d(Origin, Direction);
var cp = point.GetClosestPointOn(ray);
var radius = GetRadius(GetHeight(point));
var dir = (point - cp).Normalized * radius;
var p0 = cp + dir;
var p1 = point.GetClosestPointOn(new Ray3d(Origin, (p0 - Origin).Normalized));
return((V3d.Distance(point, p1) < V3d.Distance(point, p0)) ? p1 : p0);
}
/// <summary>
/// P0 has height 0.0, P1 has height 1.0
/// </summary>
public double GetHeight(V3d p)
{
var dir = (P1 - P0).Normalized;
var pp = p.GetClosestPointOn(new Ray3d(P0, dir));
return((pp - P0).Dot(dir));
}
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());
}
/// <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);
}