using System; using UnityEngine; namespace RootMotion.FinalIK { [HelpURL("http://www.root-motion.com/finalikdox/html/page14.html")] [AddComponentMenu("Scripts/RootMotion.FinalIK/Rotation Limits/Rotation Limit Polygonal")] public class RotationLimitPolygonal : RotationLimit { [Serializable] public class ReachCone { public Vector3[] tetrahedron; public float volume; public Vector3 S; public Vector3 B; public Vector3 o => tetrahedron[0]; public Vector3 a => tetrahedron[1]; public Vector3 b => tetrahedron[2]; public Vector3 c => tetrahedron[3]; public bool isValid => volume > 0f; public ReachCone(Vector3 _o, Vector3 _a, Vector3 _b, Vector3 _c) { tetrahedron = new Vector3[4]; tetrahedron[0] = _o; tetrahedron[1] = _a; tetrahedron[2] = _b; tetrahedron[3] = _c; volume = 0f; S = Vector3.zero; B = Vector3.zero; } public void Calculate() { Vector3 lhs = Vector3.Cross(a, b); volume = Vector3.Dot(lhs, c) / 6f; S = Vector3.Cross(a, b).normalized; B = Vector3.Cross(b, c).normalized; } } [Serializable] public class LimitPoint { public Vector3 point; public float tangentWeight; public LimitPoint() { point = Vector3.forward; tangentWeight = 1f; } } [Range(0f, 180f)] public float twistLimit = 180f; [Range(0f, 3f)] public int smoothIterations; [HideInInspector] public LimitPoint[] points; [HideInInspector] public Vector3[] P; [HideInInspector] public ReachCone[] reachCones = new ReachCone[0]; [ContextMenu("User Manual")] private void OpenUserManual() { Application.OpenURL("http://www.root-motion.com/finalikdox/html/page14.html"); } [ContextMenu("Scrpt Reference")] private void OpenScriptReference() { Application.OpenURL("http://www.root-motion.com/finalikdox/html/class_root_motion_1_1_final_i_k_1_1_rotation_limit_polygonal.html"); } [ContextMenu("Support Group")] private void SupportGroup() { Application.OpenURL("https://groups.google.com/forum/#!forum/final-ik"); } [ContextMenu("Asset Store Thread")] private void ASThread() { Application.OpenURL("http://forum.unity3d.com/threads/final-ik-full-body-ik-aim-look-at-fabrik-ccd-ik-1-0-released.222685/"); } public void SetLimitPoints(LimitPoint[] points) { if (points.Length < 3) { LogWarning("The polygon must have at least 3 Limit Points."); return; } this.points = points; BuildReachCones(); } protected override Quaternion LimitRotation(Quaternion rotation) { if (reachCones.Length == 0) { Start(); } return RotationLimit.LimitTwist(LimitSwing(rotation), axis, base.secondaryAxis, twistLimit); } private void Start() { if (points.Length < 3) { ResetToDefault(); } for (int i = 0; i < reachCones.Length; i++) { if (!reachCones[i].isValid) { if (smoothIterations <= 0) { int num = 0; num = ((i < reachCones.Length - 1) ? (i + 1) : 0); LogWarning("Reach Cone {point " + i + ", point " + num + ", Origin} has negative volume. Make sure Axis vector is in the reachable area and the polygon is convex."); } else { LogWarning("One of the Reach Cones in the polygon has negative volume. Make sure Axis vector is in the reachable area and the polygon is convex."); } } } axis = axis.normalized; } public void ResetToDefault() { points = new LimitPoint[4]; for (int i = 0; i < points.Length; i++) { points[i] = new LimitPoint(); } Quaternion quaternion = Quaternion.AngleAxis(45f, Vector3.right); Quaternion quaternion2 = Quaternion.AngleAxis(45f, Vector3.up); points[0].point = quaternion * quaternion2 * axis; points[1].point = Quaternion.Inverse(quaternion) * quaternion2 * axis; points[2].point = Quaternion.Inverse(quaternion) * Quaternion.Inverse(quaternion2) * axis; points[3].point = quaternion * Quaternion.Inverse(quaternion2) * axis; BuildReachCones(); } public void BuildReachCones() { smoothIterations = Mathf.Clamp(smoothIterations, 0, 3); P = new Vector3[points.Length]; for (int i = 0; i < points.Length; i++) { P[i] = points[i].point.normalized; } for (int j = 0; j < smoothIterations; j++) { P = SmoothPoints(); } reachCones = new ReachCone[P.Length]; for (int k = 0; k < reachCones.Length - 1; k++) { reachCones[k] = new ReachCone(Vector3.zero, axis.normalized, P[k], P[k + 1]); } reachCones[P.Length - 1] = new ReachCone(Vector3.zero, axis.normalized, P[P.Length - 1], P[0]); for (int l = 0; l < reachCones.Length; l++) { reachCones[l].Calculate(); } } private Vector3[] SmoothPoints() { Vector3[] array = new Vector3[P.Length * 2]; float scalar = GetScalar(P.Length); for (int i = 0; i < array.Length; i += 2) { array[i] = PointToTangentPlane(P[i / 2], 1f); } for (int j = 1; j < array.Length; j += 2) { Vector3 vector = Vector3.zero; Vector3 zero = Vector3.zero; Vector3 vector2 = Vector3.zero; if (j > 1 && j < array.Length - 2) { vector = array[j - 2]; vector2 = array[j + 1]; } else if (j == 1) { vector = array[^2]; vector2 = array[j + 1]; } else if (j == array.Length - 1) { vector = array[j - 2]; vector2 = array[0]; } zero = ((j >= array.Length - 1) ? array[0] : array[j + 1]); int num = array.Length / points.Length; array[j] = 0.5f * (array[j - 1] + zero) + scalar * points[j / num].tangentWeight * (zero - vector) + scalar * points[j / num].tangentWeight * (array[j - 1] - vector2); } for (int k = 0; k < array.Length; k++) { array[k] = TangentPointToSphere(array[k], 1f); } return array; } private float GetScalar(int k) { if (k <= 3) { return 0.1667f; } return k switch { 4 => 0.1036f, 5 => 0.085f, 6 => 0.0773f, 7 => 0.07f, _ => 0.0625f, }; } private Vector3 PointToTangentPlane(Vector3 p, float r) { float num = Vector3.Dot(axis, p); float num2 = 2f * r * r / (r * r + num); return num2 * p + (1f - num2) * -axis; } private Vector3 TangentPointToSphere(Vector3 q, float r) { float num = Vector3.Dot(q - axis, q - axis); float num2 = 4f * r * r / (4f * r * r + num); return num2 * q + (1f - num2) * -axis; } private Quaternion LimitSwing(Quaternion rotation) { if (rotation == Quaternion.identity) { return rotation; } Vector3 vector = rotation * axis; int reachCone = GetReachCone(vector); if (reachCone == -1) { if (!Warning.logged) { LogWarning("RotationLimitPolygonal reach cones are invalid."); } return rotation; } if (Vector3.Dot(reachCones[reachCone].B, vector) > 0f) { return rotation; } Vector3 rhs = Vector3.Cross(axis, vector); vector = Vector3.Cross(-reachCones[reachCone].B, rhs); return Quaternion.FromToRotation(rotation * axis, vector) * rotation; } private int GetReachCone(Vector3 L) { float num = Vector3.Dot(reachCones[0].S, L); for (int i = 0; i < reachCones.Length; i++) { float num2 = num; num = ((i >= reachCones.Length - 1) ? Vector3.Dot(reachCones[0].S, L) : Vector3.Dot(reachCones[i + 1].S, L)); if (num2 >= 0f && num < 0f) { return i; } } return -1; } } }