/// <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; }