/// <summary>
/// Attempts to encapsulate a cloud of points within a sphere. This may not produce an optimal or exact fit. The results
/// are approximate.
/// </summary>
/// <param name="points">The list of points with which to build an enclosing sphere.</param>
/// <param name="capsule">Returns the sphere containing all points.</param>
public static void Fit(IList<Vector3> points, out Sphere sphere)
{
if (points.Count < 2)
{
sphere = new Sphere(points.Count == 0 ? Vector3.Zero : points[0], 0f);
return;
}
var weights = new float[points.Count];
var f = 1f / points.Count;
for (int i = 0; i < weights.Length; i++)
weights[i] = f;
while (true)
{
sphere.Center = Vector3.Zero;
for(int i = 0; i < points.Count; i++)
{
var p = points[i];
Vector3.Multiply(ref p, weights[i], out p);
Vector3.Add(ref sphere.Center, ref p, out sphere.Center);
}
float sumWeightedRadiusSq = 0f, sumWeightedRadius = 0f;
sphere.Radius = 0f;
for (int i = 0; i < points.Count; i++)
{
var p = points[i];
float radiusSq;
Vector3.DistanceSquared(ref sphere.Center, ref p, out radiusSq);
sumWeightedRadiusSq += radiusSq * weights[i];
float radius = (float)Math.Sqrt(radiusSq);
if (sphere.Radius < radius)
sphere.Radius = radius;
weights[i] *= radius;
sumWeightedRadius += weights[i];
}
if (sphere.Radius - (float)Math.Sqrt(sumWeightedRadiusSq) < sphere.Radius * Constants.Epsilon)
break;
for (int i = 0; i < points.Count; i++)
{
weights[i] /= sumWeightedRadius;
}
}
}
/// <summary>
/// Intersects a line segment with the shape and returns the intersection point closest to the beginning of the segment.
/// </summary>
/// <param name="segment">The line segment to intersect.</param>
/// <param name="scalar">A value between 0 and 1 indicating how far along the segment the intersection occurs.</param>
/// <param name="p">The point of the intersection closest to the beginning of the line segment.</param>
/// <returns>Returns a value indicating whether there is an intersection.</returns>
public bool Intersect(ref Segment segment, out float scalar, out Vector3 p)
{
Vector3 d, m, n;
float md, nd, dd;
Vector3.Subtract(ref P2, ref P1, out d);
Vector3.Subtract(ref segment.P1, ref P1, out m);
Vector3.Subtract(ref segment.P2, ref segment.P1, out n);
Vector3.Dot(ref d, ref m, out md);
Vector3.Dot(ref d, ref n, out nd);
Vector3.Dot(ref d, ref d, out dd);
if (!(md < 0f && md + nd < 0f) &&
!(md > dd && md + nd > dd))
{
float nn, mn, k;
Vector3.Dot(ref n, ref n, out nn);
Vector3.Dot(ref m, ref n, out mn);
float a = dd * nn - nd * nd;
Vector3.Dot(ref m, ref m, out k);
k -= Radius * Radius;
float c = dd * k - md * md;
if (Math.Abs(a) >= Constants.Epsilon)
{
float b = dd * mn - nd * md;
float discr = b * b - a * c;
if (discr >= 0f)
{
float t = (-b - (float)Math.Sqrt(discr)) / a;
if (t >= 0f && t <= 1f &&
md + t * nd >= 0f &&
md + t * nd <= dd)
{
scalar = t;
Vector3.Multiply(ref n, t, out p);
Vector3.Add(ref segment.P1, ref p, out p);
return true;
}
}
}
}
var cap = new Sphere(P1, Radius);
float s;
scalar = float.MaxValue;
p = Vector3.Zero;
Vector3 v;
if (cap.Intersect(ref segment, out s, out v))
{
scalar = s;
p = v;
}
cap.Center = P2;
if (cap.Intersect(ref segment, out s, out v) && s < scalar)
{
scalar = s;
p = v;
}
return scalar >= 0f && scalar <= 1f;
}
/// <summary>
/// Construct the collision part from a sphere.
/// </summary>
/// <param name="sphere">The sphere defining the center and radius of the part.</param>
public SpherePart(Sphere sphere)
{
_body = World = sphere;
}
