/// <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); }