123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333 |
- using UnityEngine;
- using System.Collections;
- 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 {
-
- [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")]
- void SupportGroup() {
- Application.OpenURL("https://groups.google.com/forum/#!forum/final-ik");
- }
-
-
- [ContextMenu("Asset Store Thread")]
- void ASThread() {
- Application.OpenURL("http://forum.unity3d.com/threads/final-ik-full-body-ik-aim-look-at-fabrik-ccd-ik-1-0-released.222685/");
- }
- #region Main Interface
-
-
-
-
- [Range(0f, 180f)] public float twistLimit = 180;
-
-
-
- [Range(0, 3)] public int smoothIterations = 0;
-
-
-
-
-
-
-
- public void SetLimitPoints(LimitPoint[] points) {
- if (points.Length < 3) {
- LogWarning("The polygon must have at least 3 Limit Points.");
- return;
- }
- this.points = points;
- BuildReachCones();
- }
-
- #endregion Main Interface
-
-
- protected override Quaternion LimitRotation(Quaternion rotation) {
- if (reachCones.Length == 0) Start();
-
- Quaternion swing = LimitSwing(rotation);
-
- return LimitTwist(swing, axis, secondaryAxis, twistLimit);
- }
-
-
- [System.Serializable]
- public class ReachCone {
- public Vector3[] tetrahedron;
- public float volume;
- public Vector3 S, B;
-
- public Vector3 o { get { return tetrahedron[0]; }}
- public Vector3 a { get { return tetrahedron[1]; }}
- public Vector3 b { get { return tetrahedron[2]; }}
- public Vector3 c { get { return tetrahedron[3]; }}
-
- public ReachCone(Vector3 _o, Vector3 _a, Vector3 _b, Vector3 _c) {
- this.tetrahedron = new Vector3[4];
- this.tetrahedron[0] = _o;
- this.tetrahedron[1] = _a;
- this.tetrahedron[2] = _b;
- this.tetrahedron[3] = _c;
-
- this.volume = 0;
- this.S = Vector3.zero;
- this.B = Vector3.zero;
- }
-
- public bool isValid { get { return volume > 0; }}
-
- public void Calculate() {
- Vector3 crossAB = Vector3.Cross(a, b);
- volume = Vector3.Dot(crossAB, c) / 6.0f;
-
- S = Vector3.Cross(a, b).normalized;
- B = Vector3.Cross(b, c).normalized;
- }
- }
-
-
- [System.Serializable]
- public class LimitPoint {
- public Vector3 point;
- public float tangentWeight;
-
- public LimitPoint() {
- this.point = Vector3.forward;
- this.tangentWeight = 1;
- }
- }
-
- [HideInInspector] public LimitPoint[] points;
- [HideInInspector] public Vector3[] P;
- [HideInInspector] public ReachCone[] reachCones = new ReachCone[0];
-
- void Start() {
- if (points.Length < 3) ResetToDefault();
-
-
- for (int i = 0; i < reachCones.Length; i++) {
- if (!reachCones[i].isValid) {
- if (smoothIterations <= 0) {
- int nextPoint = 0;
- if (i < reachCones.Length - 1) nextPoint = i + 1;
- else nextPoint = 0;
- LogWarning("Reach Cone {point " + i + ", point " + nextPoint + ", 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;
- }
-
- #region Precalculations
-
-
- public void ResetToDefault() {
- points = new LimitPoint[4];
- for (int i = 0; i < points.Length; i++) points[i] = new LimitPoint();
-
- Quaternion swing1Rotation = Quaternion.AngleAxis(45, Vector3.right);
- Quaternion swing2Rotation = Quaternion.AngleAxis(45, Vector3.up);
-
- points[0].point = (swing1Rotation * swing2Rotation) * axis;
- points[1].point = (Quaternion.Inverse(swing1Rotation) * swing2Rotation) * axis;
- points[2].point = (Quaternion.Inverse(swing1Rotation) * Quaternion.Inverse(swing2Rotation)) * axis;
- points[3].point = (swing1Rotation * Quaternion.Inverse(swing2Rotation)) * 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 i = 0; i < smoothIterations; i++) P = SmoothPoints();
-
-
- reachCones = new ReachCone[P.Length];
- for (int i = 0; i < reachCones.Length - 1; i++) {
- reachCones[i] = new ReachCone(Vector3.zero, axis.normalized, P[i], P[i + 1]);
- }
-
- reachCones[P.Length - 1] = new ReachCone(Vector3.zero, axis.normalized, P[P.Length - 1], P[0]);
-
- for (int i = 0; i < reachCones.Length; i++) reachCones[i].Calculate();
- }
-
-
- private Vector3[] SmoothPoints() {
-
- Vector3[] Q = new Vector3[P.Length * 2];
-
- float scalar = GetScalar(P.Length);
-
-
- for (int i = 0; i < Q.Length; i+= 2) Q[i] = PointToTangentPlane(P[i / 2], 1);
-
-
- for (int i = 1; i < Q.Length; i+= 2) {
- Vector3 minus2 = Vector3.zero;
- Vector3 plus1 = Vector3.zero;
- Vector3 plus2 = Vector3.zero;
-
- if (i > 1 && i < Q.Length - 2) {
- minus2 = Q[i - 2];
- plus2 = Q[i + 1];
- } else if (i == 1) {
- minus2 = Q[Q.Length - 2];
- plus2 = Q[i + 1];
- } else if (i == Q.Length - 1) {
- minus2 = Q[i - 2];
- plus2 = Q[0];
- }
-
- if (i < Q.Length - 1) plus1 = Q[i + 1];
- else plus1 = Q[0];
-
- int t = Q.Length / points.Length;
-
-
- Q[i] = (0.5f * (Q[i - 1] + plus1)) + (scalar * points[i / t].tangentWeight * (plus1 - minus2)) + (scalar * points[i / t].tangentWeight * (Q[i - 1] - plus2));
- }
-
- for (int i = 0; i < Q.Length; i++) Q[i] = TangentPointToSphere(Q[i], 1);
-
- return Q;
- }
-
-
- private float GetScalar(int k) {
-
- if (k <= 3) return .1667f;
- if (k == 4) return .1036f;
- if (k == 5) return .0850f;
- if (k == 6) return .0773f;
- if (k == 7) return .0700f;
- return .0625f;
- }
-
-
- private Vector3 PointToTangentPlane(Vector3 p, float r) {
- float d = Vector3.Dot(axis, p);
- float u = (2 * r * r) / ((r * r) + d);
- return (u * p) + ((1 - u) * -axis);
- }
-
-
- private Vector3 TangentPointToSphere(Vector3 q, float r) {
- float d = Vector3.Dot(q - axis, q - axis);
- float u = (4 * r * r) / ((4 * r * r) + d);
- return (u * q) + ((1 - u) * -axis);
- }
-
- #endregion Precalculations
-
- #region Runtime calculations
-
-
- private Quaternion LimitSwing(Quaternion rotation) {
- if (rotation == Quaternion.identity) return rotation;
-
- Vector3 L = rotation * axis;
-
- int r = GetReachCone(L);
-
-
- if (r == -1) {
- if (!Warning.logged) LogWarning("RotationLimitPolygonal reach cones are invalid.");
- return rotation;
- }
-
-
- float v = Vector3.Dot(reachCones[r].B, L);
- if (v > 0) return rotation;
-
-
- Vector3 rotationNormal = Vector3.Cross(axis, L);
-
-
- L = Vector3.Cross(-reachCones[r].B, rotationNormal);
-
-
- Quaternion toLimits = Quaternion.FromToRotation(rotation * axis, L);
-
-
- return toLimits * rotation;
- }
-
-
- private int GetReachCone(Vector3 L) {
- float p = 0;
- float p1 = Vector3.Dot(reachCones[0].S, L);
-
- for (int i = 0; i < reachCones.Length; i++) {
- p = p1;
-
- if (i < reachCones.Length - 1) p1 = Vector3.Dot(reachCones[i + 1].S, L);
- else p1 = Vector3.Dot(reachCones[0].S, L);
-
- if (p >= 0 && p1 < 0) return i;
- }
-
- return -1;
- }
-
- #endregion Runtime calculations
- }
- }
|