public double DistanceToPoint(Vector3 point)
{
return(point.DistanceTo(this.center) - this.radius);
}
public double DistanceSqToSegment(Vector3 v0, Vector3 v1, Vector3 optionalPointOnRay = null, Vector3 optionalPointOnSegment = null)
{
var segCenter = new Vector3();
var segDir = new Vector3();
var diff = new Vector3();
// from http://www.geometrictools.com/GTEngine/Include/Mathematics/GteDistRaySegment.h
// It returns the min distance between the ray and the segment
// defined by v0 and v1
// It can also set two optional targets :
// - The closest point on the ray
// - The closest point on the segment
segCenter.Copy(v0).Add(v1).MultiplyScalar(0.5);
segDir.Copy(v1).Sub(v0).Normalize();
diff.Copy(this.origin).Sub(segCenter);
var segExtent = v0.DistanceTo(v1) * 0.5;
var a01 = -this.direction.Dot(segDir);
var b0 = diff.Dot(this.direction);
var b1 = -diff.Dot(segDir);
var c = diff.LengthSq();
var det = _Math.Abs(1 - a01 * a01);
double s0, s1, sqrDist, extDet;
if (det > 0)
{
// The ray and segment are not parallel.
s0 = a01 * b1 - b0;
s1 = a01 * b0 - b1;
extDet = segExtent * det;
if (s0 >= 0)
{
if (s1 >= -extDet)
{
if (s1 <= extDet)
{
// region 0
// Minimum at interior points of ray and segment.
var invDet = 1 / det;
s0 *= invDet;
s1 *= invDet;
sqrDist = s0 * (s0 + a01 * s1 + 2 * b0) + s1 * (a01 * s0 + s1 + 2 * b1) + c;
}
else
{
// region 1
s1 = segExtent;
s0 = _Math.Max(0, -(a01 * s1 + b0));
sqrDist = -s0 * s0 + s1 * (s1 + 2 * b1) + c;
}
}
else
{
// region 5
s1 = -segExtent;
s0 = _Math.Max(0, -(a01 * s1 + b0));
sqrDist = -s0 * s0 + s1 * (s1 + 2 * b1) + c;
}
}
else
{
if (s1 <= -extDet)
{
// region 4
s0 = _Math.Max(0, -(-a01 * segExtent + b0));
s1 = (s0 > 0) ? -segExtent : _Math.Min(_Math.Max(-segExtent, -b1), segExtent);
sqrDist = -s0 * s0 + s1 * (s1 + 2 * b1) + c;
}
else if (s1 <= extDet)
{
// region 3
s0 = 0;
s1 = _Math.Min(_Math.Max(-segExtent, -b1), segExtent);
sqrDist = s1 * (s1 + 2 * b1) + c;
}
else
{
// region 2
s0 = _Math.Max(0, -(a01 * segExtent + b0));
s1 = (s0 > 0) ? segExtent : _Math.Min(_Math.Max(-segExtent, -b1), segExtent);
sqrDist = -s0 * s0 + s1 * (s1 + 2 * b1) + c;
}
}
}
else
{
// Ray and segment are parallel.
s1 = (a01 > 0) ? -segExtent : segExtent;
s0 = _Math.Max(0, -(a01 * s1 + b0));
sqrDist = -s0 * s0 + s1 * (s1 + 2 * b1) + c;
}
if (optionalPointOnRay != null)
{
optionalPointOnRay.Copy(this.direction).MultiplyScalar(s0).Add(this.origin);
}
if (optionalPointOnSegment != null)
{
optionalPointOnSegment.Copy(segDir).MultiplyScalar(s1).Add(segCenter);
}
return(sqrDist);
}
