/// <summary>
/// Returns shortest distance from segment to the point
/// </summary>
public double DistanceTo(Point3d p)
{
return(p.DistanceTo(this));
}
/// <summary>
/// Shortest distance between triangle and point
/// </summary>
public double DistanceTo(Point3d p)
{
Point3d closest_point = this.ClosestPoint(p);
return(p.DistanceTo(closest_point));
}
/// <summary>
/// Calculates the point on the ellipsoid's boundary closest to given point.
/// </summary>
public Point3d ClosestPoint(Point3d p)
{
// Algorithm by Dr. Robert Nurnberg
// http://wwwf.imperial.ac.uk/~rn/distance2ellipse.pdf
Coord3d local_coord = new Coord3d(this.Center, this._v1, this._v2);
p = p.ConvertTo(local_coord);
if (GeometRi3D.AlmostEqual(p.X, 0) && GeometRi3D.AlmostEqual(p.Y, 0))
{
// Center point, choose any minor-axis
return(new Point3d(0, C, 0, local_coord));
}
double theta = Atan2(A * p.Y, B * p.X);
double phi = Atan2(p.Z, C * Sqrt((p.X * p.X) / (A * A) + (p.Y * p.Y) / (B * B)));
int iter = 0;
int max_iter = 100;
Point3d n0 = p.Copy();
while (iter < max_iter)
{
iter += 1;
double ct = Cos(theta);
double st = Sin(theta);
double cp = Cos(phi);
double sp = Sin(phi);
double F1 = (A * A - B * B) * ct * st * cp - p.X * A * st + p.Y * B * ct;
double F2 = (A * A * ct * ct + B * B * st * st - C * C) * sp * cp - p.X * A * sp * ct - p.Y * B * sp * st + p.Z * C * cp;
double a11 = (A * A - B * B) * (ct * ct - st * st) * cp - p.X * A * ct - p.Y * B * st;
double a12 = -(A * A - B * B) * ct * st * sp;
double a21 = -2.0 * (A * A - B * B) * ct * st * cp * sp + p.X * A * sp * st - p.Y * B * sp * ct;
double a22 = (A * A * ct * ct + B * B * st * st - C * C) * (cp * cp - sp * sp) - p.X * A * cp * ct - p.Y * B * cp * st - p.Z * C * sp;
double det = a11 * a22 - a12 * a21;
if (det == 0)
{
throw new Exception("Zero determinant");
}
// Calc reverse matrix B[ij] = A[ij]^-1
double b11 = a22 / det;
double b12 = -a12 / det;
double b21 = -a21 / det;
double b22 = a11 / det;
theta = theta - (b11 * F1 + b12 * F2);
phi = phi - (b21 * F1 + b22 * F2);
Point3d n = new Point3d(A * Cos(phi) * Cos(theta), B * Cos(phi) * Sin(theta), C * Sin(phi), local_coord);
if (n0.DistanceTo(n) < GeometRi3D.Tolerance)
{
return(n);
}
n0 = n.Copy();
}
return(n0);
}
/// <summary>
/// Shortest distance between two circles (including interior points) (approximate solution)
/// <para> The output points may be not unique in case of parallel or intersecting circles.</para>
/// </summary>
/// <param name="c">Target circle</param>
/// <param name="p1">Closest point on source circle</param>
/// <param name="p2">Closest point on target circle</param>
public double DistanceTo(Circle3d c, out Point3d p1, out Point3d p2)
{
if (this.Normal.IsParallelTo(c.Normal))
{
Point3d projection = c.Center.ProjectionTo(this.ToPlane);
double dist = projection.DistanceTo(this.Center);
double vdist = projection.DistanceTo(c.Center);
if (dist < this.R + c.R)
{
if (projection.BelongsTo(this))
{
p1 = projection;
p2 = c.Center;
}
else
{
p1 = this.Center.Translate(this.R * new Vector3d(this.Center, projection).Normalized);
p2 = p1.ProjectionTo(c.ToPlane);
}
return(vdist);
}
else
{
Vector3d v = new Vector3d(this.Center, projection).Normalized;
p1 = this.Center.Translate(this.R * v);
p2 = c.Center.Translate(-c.R * v);
return(Sqrt((dist - this.R - c.R) * (dist - this.R - c.R) + vdist * vdist));
}
}
double tol = GeometRi3D.Tolerance;
bool mode = GeometRi3D.UseAbsoluteTolerance;
GeometRi3D.Tolerance = GeometRi3D.DefaultTolerance;
GeometRi3D.UseAbsoluteTolerance = true;
object obj = this.IntersectionWith(c);
if (obj != null)
{
// Restore initial state
GeometRi3D.UseAbsoluteTolerance = mode;
GeometRi3D.Tolerance = tol;
if (obj.GetType() == typeof(Point3d))
{
p1 = (Point3d)obj;
p2 = (Point3d)obj;
}
else if (obj.GetType() == typeof(Segment3d))
{
p1 = ((Segment3d)obj).P1;
p2 = ((Segment3d)obj).P1;
}
else
{
p1 = ((Circle3d)obj).Center;
p2 = ((Circle3d)obj).Center;
}
return(0);
}
Point3d p1_1, p1_2, p2_1, p2_2;
double dist1 = _distance_cicle_to_circle(this, c, out p1_1, out p1_2);
double dist2 = _distance_cicle_to_circle(c, this, out p2_1, out p2_2);
// Restore initial state
GeometRi3D.UseAbsoluteTolerance = mode;
GeometRi3D.Tolerance = tol;
if (dist1 < dist2)
{
p1 = p1_1;
p2 = p1_2;
}
else
{
p1 = p2_2;
p2 = p2_1;
}
return(Min(dist1, dist2));
}
