4点を内包する最小の球の中心と半径を求める C#/Unity

GetBallFrom4Points.cs

using UnityEngine;

/// <summary>
/// 4点を通る最小の球の中心と半径を求める C#
/// @see http://marupeke296.com/COL_3D_No22_BoundingSphere.html
/// </summary>

public static class ToolUtil
{
    public static bool GetBallFrom4Points(Vector3[] pos, out Vector3 outP, out float outR)
    {
        outP = Vector3.zero;
        outR = 0f;
        if (pos.Length != 4)
        {
            Debug.Log("4つの点を与えてください");
            return false;
        }

        calc4PointMinimumBS(pos[0], pos[1], pos[2], pos[3], out outP, out outR);

        if (outR != 0f)
            return true;
        else
            return false;
    }

    static void calc3PointBS(Vector3 p0, Vector3 p1, Vector3 p2, out Vector3 outP, out float outR)
    {
        // 鈍角三角形？
        float dot0 = Vector3.Dot(p1 - p0, p2 - p0);
        if (dot0 <= 0.0f)
        {
            outP = (p1 + p2) / 2.0f;
            outR = Vector3.Distance(p1, p2) / 2.0f;
            return;
        }
        float dot1 = Vector3.Dot(p0 - p1, p2 - p1);
        if (dot1 <= 0.0f)
        {
            outP = (p0 + p2) / 2.0f;
            outR = Vector3.Distance(p0, p2) / 2.0f;
            return;
        }
        float dot2 = Vector3.Dot((p0 - p2), (p1 - p2));
        if (dot2 <= 0.0f)
        {
            outP = (p0 + p1) / 2.0f;
            outR = Vector3.Distance(p0, p1) / 2.0f;
            return;
        }

        // 鋭角三角形
        // 3頂点から等距離にある点が中心。連立方程式を解きます。
        Vector3 N;
        N = Vector3.Cross((p1 - p0), (p2 - p0));
        Vector3 v0, v1, e0, e1;
        v0 = Vector3.Cross((p2 - p1), N);
        v1 = Vector3.Cross((p2 - p0), N);
        e0 = (p2 + p1) * 0.5f;
        e1 = (p2 + p0) * 0.5f;
        float a = Vector3.Dot(v0, v1);
        float b = Vector3.Dot(v0, v0);
        float c = -Vector3.Dot((e1 - e0), v0);
        float d = Vector3.Dot(v1, v1);
        float e = -Vector3.Dot((e1 - e0), v1);

        float div = -a * a + b * d;
        float t = (-a * c + b * e) / div;
        float s = (-c * d + a * e) / div;

        outP = e0 + s * v0;
        outR = Vector3.Distance(outP, -p0);
    }

    static void calc2PointBS(Vector3 p0, Vector3 p1, out Vector3 outP, out float outR)
    {
        outP = (p0 + p1) / 2.0f;
        outR = Vector3.Distance(p0, p1) / 2.0f;
    }

    static bool checkEdgeMinimum(Vector3 p0, Vector3 p1, Vector3 other0, Vector3 other1, out Vector3 outP, out float outR)
    {
        calc2PointBS(p0, p1, out outP, out outR);

        if (Vector3.Distance(outP, other0) > outR || Vector3.Distance(outP, other1) > outR)
            return false;

        return true;
    }

    static bool checkTriangleMinimum(Vector3 p0, Vector3 p1, Vector3 p2, Vector3 other0, out Vector3 outP, out float outR)
    {
        calc3PointBS(p0, p1, p2, out outP, out outR);
        if (Vector3.Distance(outP, other0) > outR)
            return false;

        return true;
    }



    static void calc4PointBS(Vector3 p0, Vector3 p1, Vector3 p2, Vector3 p3, out Vector3 outP, out float outR)
    {
        // 三角形p0p1p2を囲う境界球の中心点を取得
        Vector3 c0;
        float tmpR;
        calc3PointBS(p0, p1, p2, out c0, out tmpR);

        Vector3 v;
        v = Vector3.Cross(p1 - p0, p2 - p0);
        v = v.normalized;

        // len(P-p0) = len(P-p3)となるPを算出
        float D = Vector3.Dot(p0, p0) - Vector3.Dot(p3, p3);
        float a = (-2.0f * Vector3.Dot(c0, (p0 - p3)) + D) / (2.0f * Vector3.Dot(v, (p0 - p3)));

        outP = c0 + a * v;
        outR = Vector3.Distance(outP, p0);
    }



    static void calc4PointMinimumBS(Vector3 p0, Vector3 p1, Vector3 p2, Vector3 p3, out Vector3 outP, out float outR)
    {
        // 6エッジ、4三角面をチェック
        if (checkEdgeMinimum(p0, p1, p2, p3, out outP, out outR) == true) return;
        if (checkEdgeMinimum(p0, p2, p1, p3, out outP, out outR) == true) return;
        if (checkEdgeMinimum(p0, p3, p1, p2, out outP, out outR) == true) return;
        if (checkEdgeMinimum(p1, p2, p0, p3, out outP, out outR) == true) return;
        if (checkEdgeMinimum(p1, p3, p0, p2, out outP, out outR) == true) return;
        if (checkEdgeMinimum(p2, p3, p0, p1, out outP, out outR) == true) return;
        if (checkTriangleMinimum(p0, p1, p2, p3, out outP, out outR) == true) return;
        if (checkTriangleMinimum(p0, p1, p3, p2, out outP, out outR) == true) return;
        if (checkTriangleMinimum(p0, p2, p3, p1, out outP, out outR) == true) return;
        if (checkTriangleMinimum(p1, p2, p3, p0, out outP, out outR) == true) return;

        // 4点を通るのが最小球
        calc4PointBS(p0, p1, p2, p3, out outP, out outR);
    }

}

TestCalcCenterOf4Points.cs

using UnityEngine;

namespace xxx.xxx
{
    public class TestCalcCenterOf4Points : MonoBehaviour
    {
        [SerializeField]
        float PinScale = 0.01f;

        Vector3 outP;
        float outR;
        Vector3[] targetPos;
        // Use this for initialization
        void Start()
        {
            targetPos = new Vector3[4];
            //targetPos[0] = new Vector3(1, 2, 0);
            //targetPos[1] = new Vector3(0, 1, 0);
            //targetPos[2] = new Vector3(3, 1, -1);
            //targetPos[3] = new Vector3(-1, 1, 0);
            targetPos[0] = new Vector3(2, 1, -1);
            targetPos[1] = new Vector3(0, 3, -1);
            targetPos[2] = new Vector3(-2, 1, -1);
            targetPos[3] = new Vector3(0, 1, 1);
            CalcBS(targetPos);
            Visualize();
        }

        void CalcBS(Vector3[] pos)
        {
            ToolUtil.GetBallFrom4Points(pos, out outP, out outR);

            float minR = outR;
            float maxR = outR;
            float aveR = 0f;

            foreach(var p in targetPos)
            {
                float dist = Vector3.Distance(outP, p);
                if (minR > dist) minR = dist;
                if (maxR < dist) maxR = dist;
                aveR += dist;
            }
            aveR = aveR / targetPos.Length;
            Debug.Log("OutP:" + outP);
            Debug.Log("OutR:" + outR);
            Debug.Log("minR:" + minR);
            Debug.Log("maxR:" + maxR);
            Debug.Log("aveR:" + aveR);

        }
        void Visualize()
        {
            foreach(var p in targetPos)
            {
                GenPin(p,Color.white);
            }
            GenPin(outP,Color.red);
            GenSphere();
        }

        void GenPin(Vector3 pos,Color color)
        {
            GameObject g = GameObject.CreatePrimitive(PrimitiveType.Cube);
            g.transform.SetParent(transform);
            g.transform.localPosition = pos;
            g.transform.localScale = Vector3.one * PinScale;
            g.GetComponent<Renderer>().material.color = color;
        }
        void GenSphere()
        {
            GameObject g = GameObject.CreatePrimitive(PrimitiveType.Sphere);
            g.transform.SetParent(transform);
            g.transform.localPosition = outP;
            g.transform.localScale = Vector3.one * outR * 2;
            Material m = g.GetComponent<Renderer>().sharedMaterial;
            m.SetFloat("_Mode", 3f);
            m.color = new Color(0, 0, 1, 0.2f);

        }
    }
}