/// <summary>
/// Calculates the best fit sphere for a series of points. Providing a larger list of points increases accuracy.
/// </summary>
/// <param name="localVertices">Local space vertices</param>
/// <returns>The best fit sphere</returns>
private BestFitSphere CalculateBestFitSphere(List <Vector3> localVertices)
{
// # of points.
int n = localVertices.Count;
// Calculate average x, y, and z value of vertices.
float xAvg, yAvg, zAvg = xAvg = yAvg = 0.0f;
foreach (Vector3 vertex in localVertices)
{
xAvg += vertex.x;
yAvg += vertex.y;
zAvg += vertex.z;
}
xAvg = xAvg * (1.0f / n);
yAvg = yAvg * (1.0f / n);
zAvg = zAvg * (1.0f / n);
// Do some fun math with matrices
// B Vector.
Vector3 B = Vector3.zero;
// Can use a 4x4 as a 3x3 with the 4x4 as 0,0,0,1 in the last row/column.
Matrix4x4 AM = new Matrix4x4(Vector4.zero, Vector4.zero, Vector4.zero, new Vector4(0, 0, 0, 1));
float x2, y2, z2 = x2 = y2 = 0.0f;
foreach (Vector3 vertex in localVertices)
{
AM[0, 0] += 2 * (vertex.x * (vertex.x - xAvg)) / n;
AM[0, 1] += 2 * (vertex.x * (vertex.y - yAvg)) / n;
AM[0, 2] += 2 * (vertex.x * (vertex.z - zAvg)) / n;
AM[1, 0] += 2 * (vertex.y * (vertex.x - xAvg)) / n;
AM[1, 1] += 2 * (vertex.y * (vertex.y - yAvg)) / n;
AM[1, 2] += 2 * (vertex.y * (vertex.z - zAvg)) / n;
AM[2, 0] += 2 * (vertex.z * (vertex.x - xAvg)) / n;
AM[2, 1] += 2 * (vertex.z * (vertex.y - yAvg)) / n;
AM[2, 2] += 2 * (vertex.z * (vertex.z - zAvg)) / n;
x2 = vertex.x * vertex.x;
y2 = vertex.y * vertex.y;
z2 = vertex.z * vertex.z;
B.x += ((x2 + y2 + z2) * (vertex.x - xAvg)) / n;
B.y += ((x2 + y2 + z2) * (vertex.y - yAvg)) / n;
B.z += ((x2 + y2 + z2) * (vertex.z - zAvg)) / n;
}
// Calculate the center of the best-fit sphere.
Vector3 center = (AM.transpose * AM).inverse * AM.transpose * B;
// Calculate radius.
float radius = 0.0f;
foreach (Vector3 vertex in localVertices)
{
radius += Mathf.Pow((vertex.x - center.x), 2) + Mathf.Pow(vertex.y - center.y, 2) + Mathf.Pow(vertex.z - center.z, 2);
}
radius = Mathf.Sqrt(radius / localVertices.Count);
BestFitSphere bfs = new BestFitSphere(center, radius);
return(bfs);
}
/// <summary>
/// Creates a capsule collider using the height from first 2 vertices, and then getting radius from the best fit sphere algorithm.
/// </summary>
/// <param name="worldVertices">List of world vertices</param>
/// <param name="properties">Properties of collider</param>
/// <returns></returns>
public Collider CreateCapsuleCollider_BestFit(List <Vector3> worldVertices, EasyColliderProperties properties)
{
if (worldVertices.Count >= 3)
{
if (properties.Orientation == COLLIDER_ORIENTATION.ROTATED)
{
GameObject obj = CreateGameObjectOrientation(worldVertices, properties.AttachTo, "Rotated Capsule Collider");
if (obj != null)
{
properties.AttachTo = obj;
}
}
// use local verts.
List <Vector3> localVertices = ToLocalVerts(properties.AttachTo.transform, worldVertices);
// height from first 2 verts selected.
Vector3 v0 = localVertices[0];
Vector3 v1 = localVertices[1];
float height = Vector3.Distance(v0, v1);
float dX = Mathf.Abs(v1.x - v0.x);
float dY = Mathf.Abs(v1.y - v0.y);
float dZ = Mathf.Abs(v1.z - v0.z);
localVertices.RemoveAt(1);
localVertices.RemoveAt(0);
// radius from best fit sphere of the rest of the vertices.
BestFitSphere bfs = CalculateBestFitSphere(localVertices);
CapsuleCollider capsuleCollider = Undo.AddComponent <CapsuleCollider>(properties.AttachTo);
Vector3 center = bfs.Center;
if (dX > dY && dX > dZ)
{
capsuleCollider.direction = 0;
center.x = (v1.x + v0.x) / 2;
}
else if (dY > dX && dY > dZ)
{
capsuleCollider.direction = 1;
center.y = (v1.y + v0.y) / 2;
}
else
{
capsuleCollider.direction = 2;
center.z = (v1.z + v0.z) / 2;
}
capsuleCollider.center = center;
capsuleCollider.height = height;
capsuleCollider.radius = bfs.Radius;
SetPropertiesOnCollider(capsuleCollider, properties);
return(capsuleCollider);
}
return(null);
}
/// <summary>
/// Creates a sphere collider using the best fit sphere algorithm.
/// </summary>
/// <param name="worldVertices">List of world space vertices</param>
/// <param name="properties">Properties of collider</param>
/// <returns></returns>
public Collider CreateSphereCollider_BestFit(List <Vector3> worldVertices, EasyColliderProperties properties)
{
if (worldVertices.Count >= 2)
{
// Convert to local space.
List <Vector3> localVertices = ToLocalVerts(properties.AttachTo.transform, worldVertices);
BestFitSphere bfs = CalculateBestFitSphere(localVertices);
SphereCollider sphereCollider = Undo.AddComponent <SphereCollider>(properties.AttachTo);
sphereCollider.radius = bfs.Radius;
sphereCollider.center = bfs.Center;
SetPropertiesOnCollider(sphereCollider, properties);
return(sphereCollider);
}
return(null);
}
