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