4点と面する球の中心と半径を求める Unity/C#
/// <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; | |
} | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment