4点と面する球の中心と半径を求める　Unity/C#

CircumcentreSolver.cs

/// <summary>
/// Given four points in 3D space, solves for a sphere such that all four points
/// lie on the sphere's surface.
/// </summary>
/// <remarks>
/// Translated from Javascript on http://www.convertalot.com/sphere_solver.html, originally
/// linked to by http://stackoverflow.com/questions/13600739/calculate-centre-of-sphere-whose-surface-contains-4-points-c.
/// </remarks>
public class CircumcentreSolver
{
    private const float ZERO = 0;
    private double m_X0, m_Y0, m_Z0;
    private double m_Radius;
    private double[,] P =
            {
                { ZERO, ZERO, ZERO },
                { ZERO, ZERO, ZERO },
                { ZERO, ZERO, ZERO },
                { ZERO, ZERO, ZERO }
            };

    /// <summary>
    /// The centre of the resulting sphere.
    /// </summary>
    public double[] Centre
    {
        get { return new double[] { this.m_X0, this.m_Y0, this.m_Z0 }; }
    }

    public Vector3 Center
    {
        get
        {
            return new Vector3((float) this.m_X0, (float)this.m_Y0, (float)this.m_Z0);
        }
    }
    public float RadiusF
    {
        get
        {
            return (float)this.m_Radius;
        }
    }
    /// <summary>
    /// The radius of the resulting sphere.
    /// </summary>
    public double RadiusD
    {
        get { return this.m_Radius; }
    }

    /// <summary>
    /// Whether the result was a valid sphere.
    /// </summary>
    public bool Valid
    {
        get { return this.m_Radius != 0; }
    }

    /// <summary>
    /// Computes the centre of a sphere such that all four specified points in
    /// 3D space lie on the sphere's surface.
    /// </summary>
    /// <param name="a">The first point (array of 3 doubles for X, Y, Z).</param>
    /// <param name="b">The second point (array of 3 doubles for X, Y, Z).</param>
    /// <param name="c">The third point (array of 3 doubles for X, Y, Z).</param>
    /// <param name="d">The fourth point (array of 3 doubles for X, Y, Z).</param>
    public CircumcentreSolver(Vector3[] pos)
    {
        this.Compute(pos);
    }

    /// <summary>
    /// Evaluate the determinant.
    /// </summary>
    private void Compute(Vector3[] pos)
    {
        P[0, 0] = pos[0].x;
        P[0, 1] = pos[0].y;
        P[0, 2] = pos[0].z;
        P[1, 0] = pos[1].x;
        P[1, 1] = pos[1].y;
        P[1, 2] = pos[1].z;
        P[2, 0] = pos[2].x;
        P[2, 1] = pos[2].y;
        P[2, 2] = pos[2].z;
        P[3, 0] = pos[3].x;
        P[3, 1] = pos[3].y;
        P[3, 2] = pos[3].z;

        // Compute result sphere.
        this.Sphere();
    }

    private void Sphere()
    {
        double r, m11, m12, m13, m14, m15;
        double[,] a =
                {
                    { ZERO, ZERO, ZERO, ZERO },
                    { ZERO, ZERO, ZERO, ZERO },
                    { ZERO, ZERO, ZERO, ZERO },
                    { ZERO, ZERO, ZERO, ZERO }
                };

        // Find minor 1, 1.
        for (int i = 0; i < 4; i++)
        {
            a[i, 0] = P[i, 0];
            a[i, 1] = P[i, 1];
            a[i, 2] = P[i, 2];
            a[i, 3] = 1;
        }
        m11 = this.Determinant(a, 4);

        // Find minor 1, 2.
        for (int i = 0; i < 4; i++)
        {
            a[i, 0] = P[i, 0] * P[i, 0] + P[i, 1] * P[i, 1] + P[i, 2] * P[i, 2];
            a[i, 1] = P[i, 1];
            a[i, 2] = P[i, 2];
            a[i, 3] = 1;
        }
        m12 = this.Determinant(a, 4);

        // Find minor 1, 3.
        for (int i = 0; i < 4; i++)
        {
            a[i, 0] = P[i, 0] * P[i, 0] + P[i, 1] * P[i, 1] + P[i, 2] * P[i, 2];
            a[i, 1] = P[i, 0];
            a[i, 2] = P[i, 2];
            a[i, 3] = 1;
        }
        m13 = this.Determinant(a, 4);

        // Find minor 1, 4.
        for (int i = 0; i < 4; i++)
        {
            a[i, 0] = P[i, 0] * P[i, 0] + P[i, 1] * P[i, 1] + P[i, 2] * P[i, 2];
            a[i, 1] = P[i, 0];
            a[i, 2] = P[i, 1];
            a[i, 3] = 1;
        }
        m14 = this.Determinant(a, 4);

        // Find minor 1, 5.
        for (int i = 0; i < 4; i++)
        {
            a[i, 0] = P[i, 0] * P[i, 0] + P[i, 1] * P[i, 1] + P[i, 2] * P[i, 2];
            a[i, 1] = P[i, 0];
            a[i, 2] = P[i, 1];
            a[i, 3] = P[i, 2];
        }
        m15 = this.Determinant(a, 4);

        // Calculate result.
        if (m11 == 0)
        {
            this.m_X0 = 0;
            this.m_Y0 = 0;
            this.m_Z0 = 0;
            this.m_Radius = 0;
        }
        else
        {
            this.m_X0 = 0.5 * m12 / m11;
            this.m_Y0 = -0.5 * m13 / m11;
            this.m_Z0 = 0.5 * m14 / m11;
            this.m_Radius = System.Math.Sqrt(this.m_X0 * this.m_X0 + this.m_Y0 * this.m_Y0 + this.m_Z0 * this.m_Z0 - m15 / m11);
        }
    }

    /// <summary>
    /// Recursive definition of determinate using expansion by minors.
    /// </summary>
    private double Determinant(double[,] a, double n)
    {
        int i, j, j1, j2;
        double d = 0;
        double[,] m =
                {
                    { ZERO, ZERO, ZERO, ZERO },
                    { ZERO, ZERO, ZERO, ZERO },
                    { ZERO, ZERO, ZERO, ZERO },
                    { ZERO, ZERO, ZERO, ZERO }
                };

        if (n == 2)
        {
            // Terminate recursion.
            d = a[0, 0] * a[1, 1] - a[1, 0] * a[0, 1];
        }
        else
        {
            d = 0;
            for (j1 = 0; j1 < n; j1++) // Do each column.
            {
                for (i = 1; i < n; i++) // Create minor.
                {
                    j2 = 0;
                    for (j = 0; j < n; j++)
                    {
                        if (j == j1) continue;
                        m[i - 1, j2] = a[i, j];
                        j2++;
                    }
                }

                // Sum (+/-)cofactor * minor.
                d = d + System.Math.Pow(-1.0, j1) * a[0, j1] * this.Determinant(m, n - 1);
            }
        }

        return d;
    }
}