Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
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
You can’t perform that action at this time.