RotationLimitPolygonal.cs 11 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333
  1. using UnityEngine;
  2. using System.Collections;
  3. namespace RootMotion.FinalIK {
  4. /// <summary>
  5. /// Using a spherical polygon to limit the range of rotation on universal and ball-and-socket joints. A reach cone is specified as a spherical polygon
  6. /// on the surface of a a reach sphere that defines all positions the longitudinal segment axis beyond the joint can take.
  7. ///
  8. /// This class is based on the "Fast and Easy Reach-Cone Joint Limits" paper by Jane Wilhelms and Allen Van Gelder.
  9. /// Computer Science Dept., University of California, Santa Cruz, CA 95064. August 2, 2001
  10. /// http://users.soe.ucsc.edu/~avg/Papers/jtl.pdf
  11. ///
  12. /// </summary>
  13. [HelpURL("http://www.root-motion.com/finalikdox/html/page14.html")]
  14. [AddComponentMenu("Scripts/RootMotion.FinalIK/Rotation Limits/Rotation Limit Polygonal")]
  15. public class RotationLimitPolygonal : RotationLimit {
  16. // Open the User Manual URL
  17. [ContextMenu("User Manual")]
  18. private void OpenUserManual() {
  19. Application.OpenURL("http://www.root-motion.com/finalikdox/html/page14.html");
  20. }
  21. // Open the Script Reference URL
  22. [ContextMenu("Scrpt Reference")]
  23. private void OpenScriptReference() {
  24. Application.OpenURL("http://www.root-motion.com/finalikdox/html/class_root_motion_1_1_final_i_k_1_1_rotation_limit_polygonal.html");
  25. }
  26. // Link to the Final IK Google Group
  27. [ContextMenu("Support Group")]
  28. void SupportGroup() {
  29. Application.OpenURL("https://groups.google.com/forum/#!forum/final-ik");
  30. }
  31. // Link to the Final IK Asset Store thread in the Unity Community
  32. [ContextMenu("Asset Store Thread")]
  33. void ASThread() {
  34. Application.OpenURL("http://forum.unity3d.com/threads/final-ik-full-body-ik-aim-look-at-fabrik-ccd-ik-1-0-released.222685/");
  35. }
  36. #region Main Interface
  37. /// <summary>
  38. /// Limit of twist rotation around the main axis.
  39. /// </summary>
  40. [Range(0f, 180f)] public float twistLimit = 180;
  41. /// <summary>
  42. /// The number of smoothing iterations applied to the polygon.
  43. /// </summary>
  44. [Range(0, 3)] public int smoothIterations = 0;
  45. /// <summary>
  46. /// Sets the limit points and recalculates the reach cones.
  47. /// </summary>
  48. /// <param name='_points'>
  49. /// _points.
  50. /// </param>
  51. public void SetLimitPoints(LimitPoint[] points) {
  52. if (points.Length < 3) {
  53. LogWarning("The polygon must have at least 3 Limit Points.");
  54. return;
  55. }
  56. this.points = points;
  57. BuildReachCones();
  58. }
  59. #endregion Main Interface
  60. /*
  61. * Limits the rotation in the local space of this instance's Transform.
  62. * */
  63. protected override Quaternion LimitRotation(Quaternion rotation) {
  64. if (reachCones.Length == 0) Start();
  65. // Subtracting off-limits swing
  66. Quaternion swing = LimitSwing(rotation);
  67. // Apply twist limits
  68. return LimitTwist(swing, axis, secondaryAxis, twistLimit);
  69. }
  70. /*
  71. * Tetrahedron composed of 2 Limit points, the origin and an axis point.
  72. * */
  73. [System.Serializable]
  74. public class ReachCone {
  75. public Vector3[] tetrahedron;
  76. public float volume;
  77. public Vector3 S, B;
  78. public Vector3 o { get { return tetrahedron[0]; }}
  79. public Vector3 a { get { return tetrahedron[1]; }}
  80. public Vector3 b { get { return tetrahedron[2]; }}
  81. public Vector3 c { get { return tetrahedron[3]; }}
  82. public ReachCone(Vector3 _o, Vector3 _a, Vector3 _b, Vector3 _c) {
  83. this.tetrahedron = new Vector3[4];
  84. this.tetrahedron[0] = _o; // Origin
  85. this.tetrahedron[1] = _a; // Axis
  86. this.tetrahedron[2] = _b; // Limit Point 1
  87. this.tetrahedron[3] = _c; // Limit Point 2
  88. this.volume = 0;
  89. this.S = Vector3.zero;
  90. this.B = Vector3.zero;
  91. }
  92. public bool isValid { get { return volume > 0; }}
  93. public void Calculate() {
  94. Vector3 crossAB = Vector3.Cross(a, b);
  95. volume = Vector3.Dot(crossAB, c) / 6.0f;
  96. S = Vector3.Cross(a, b).normalized;
  97. B = Vector3.Cross(b, c).normalized;
  98. }
  99. }
  100. /*
  101. * The points defining the polygon
  102. * */
  103. [System.Serializable]
  104. public class LimitPoint {
  105. public Vector3 point;
  106. public float tangentWeight;
  107. public LimitPoint() {
  108. this.point = Vector3.forward;
  109. this.tangentWeight = 1;
  110. }
  111. }
  112. [HideInInspector] public LimitPoint[] points;
  113. [HideInInspector] public Vector3[] P;
  114. [HideInInspector] public ReachCone[] reachCones = new ReachCone[0];
  115. void Start() {
  116. if (points.Length < 3) ResetToDefault();
  117. // Check if Limit Points are valid
  118. for (int i = 0; i < reachCones.Length; i++) {
  119. if (!reachCones[i].isValid) {
  120. if (smoothIterations <= 0) {
  121. int nextPoint = 0;
  122. if (i < reachCones.Length - 1) nextPoint = i + 1;
  123. else nextPoint = 0;
  124. 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.");
  125. } 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.");
  126. }
  127. }
  128. axis = axis.normalized;
  129. }
  130. #region Precalculations
  131. /*
  132. * Apply the default initial setup of 4 Limit Points
  133. * */
  134. public void ResetToDefault() {
  135. points = new LimitPoint[4];
  136. for (int i = 0; i < points.Length; i++) points[i] = new LimitPoint();
  137. Quaternion swing1Rotation = Quaternion.AngleAxis(45, Vector3.right);
  138. Quaternion swing2Rotation = Quaternion.AngleAxis(45, Vector3.up);
  139. points[0].point = (swing1Rotation * swing2Rotation) * axis;
  140. points[1].point = (Quaternion.Inverse(swing1Rotation) * swing2Rotation) * axis;
  141. points[2].point = (Quaternion.Inverse(swing1Rotation) * Quaternion.Inverse(swing2Rotation)) * axis;
  142. points[3].point = (swing1Rotation * Quaternion.Inverse(swing2Rotation)) * axis;
  143. BuildReachCones();
  144. }
  145. /*
  146. * Recalculate reach cones if the Limit Points have changed
  147. * */
  148. public void BuildReachCones() {
  149. smoothIterations = Mathf.Clamp(smoothIterations, 0, 3);
  150. // Make another array for the points so that they could be smoothed without changing the initial points
  151. P = new Vector3[points.Length];
  152. for (int i = 0; i < points.Length; i++) P[i] = points[i].point.normalized;
  153. for (int i = 0; i < smoothIterations; i++) P = SmoothPoints();
  154. // Calculating the reach cones
  155. reachCones = new ReachCone[P.Length];
  156. for (int i = 0; i < reachCones.Length - 1; i++) {
  157. reachCones[i] = new ReachCone(Vector3.zero, axis.normalized, P[i], P[i + 1]);
  158. }
  159. reachCones[P.Length - 1] = new ReachCone(Vector3.zero, axis.normalized, P[P.Length - 1], P[0]);
  160. for (int i = 0; i < reachCones.Length; i++) reachCones[i].Calculate();
  161. }
  162. /*
  163. * Automatically adds virtual limit points to smooth the polygon
  164. * */
  165. private Vector3[] SmoothPoints() {
  166. // Create the new point array with double length
  167. Vector3[] Q = new Vector3[P.Length * 2];
  168. float scalar = GetScalar(P.Length); // Get the constant used for interpolation
  169. // Project all the existing points on a plane that is tangent to the unit sphere at the Axis point
  170. for (int i = 0; i < Q.Length; i+= 2) Q[i] = PointToTangentPlane(P[i / 2], 1);
  171. // Interpolate the new points
  172. for (int i = 1; i < Q.Length; i+= 2) {
  173. Vector3 minus2 = Vector3.zero;
  174. Vector3 plus1 = Vector3.zero;
  175. Vector3 plus2 = Vector3.zero;
  176. if (i > 1 && i < Q.Length - 2) {
  177. minus2 = Q[i - 2];
  178. plus2 = Q[i + 1];
  179. } else if (i == 1) {
  180. minus2 = Q[Q.Length - 2];
  181. plus2 = Q[i + 1];
  182. } else if (i == Q.Length - 1) {
  183. minus2 = Q[i - 2];
  184. plus2 = Q[0];
  185. }
  186. if (i < Q.Length - 1) plus1 = Q[i + 1];
  187. else plus1 = Q[0];
  188. int t = Q.Length / points.Length;
  189. // Interpolation
  190. Q[i] = (0.5f * (Q[i - 1] + plus1)) + (scalar * points[i / t].tangentWeight * (plus1 - minus2)) + (scalar * points[i / t].tangentWeight * (Q[i - 1] - plus2));
  191. }
  192. // Project the points from tangent plane to the sphere
  193. for (int i = 0; i < Q.Length; i++) Q[i] = TangentPointToSphere(Q[i], 1);
  194. return Q;
  195. }
  196. /*
  197. * Returns scalar values used for interpolating smooth positions between limit points
  198. * */
  199. private float GetScalar(int k) {
  200. // Values k (number of points) == 3, 4 and 6 are calculated by analytical geometry, values 5 and 7 were estimated by interpolation
  201. if (k <= 3) return .1667f;
  202. if (k == 4) return .1036f;
  203. if (k == 5) return .0850f;
  204. if (k == 6) return .0773f;
  205. if (k == 7) return .0700f;
  206. return .0625f; // Cubic spline fit
  207. }
  208. /*
  209. * Project a point on the sphere to a plane that is tangent to the unit sphere at the Axis point
  210. * */
  211. private Vector3 PointToTangentPlane(Vector3 p, float r) {
  212. float d = Vector3.Dot(axis, p);
  213. float u = (2 * r * r) / ((r * r) + d);
  214. return (u * p) + ((1 - u) * -axis);
  215. }
  216. /*
  217. * Project a point on the tangent plane to the sphere
  218. * */
  219. private Vector3 TangentPointToSphere(Vector3 q, float r) {
  220. float d = Vector3.Dot(q - axis, q - axis);
  221. float u = (4 * r * r) / ((4 * r * r) + d);
  222. return (u * q) + ((1 - u) * -axis);
  223. }
  224. #endregion Precalculations
  225. #region Runtime calculations
  226. /*
  227. * Applies Swing limit to the rotation
  228. * */
  229. private Quaternion LimitSwing(Quaternion rotation) {
  230. if (rotation == Quaternion.identity) return rotation; // Assuming initial rotation is in the reachable area
  231. Vector3 L = rotation * axis; // Test this vector against the reach cones
  232. int r = GetReachCone(L); // Get the reach cone to test against (can be only 1)
  233. // Just in case we are running our application with invalid reach cones
  234. if (r == -1) {
  235. if (!Warning.logged) LogWarning("RotationLimitPolygonal reach cones are invalid.");
  236. return rotation;
  237. }
  238. // Dot product of cone normal and rotated axis
  239. float v = Vector3.Dot(reachCones[r].B, L);
  240. if (v > 0) return rotation; // Rotation is reachable
  241. // Find normal for a plane defined by origin, axis, and rotated axis
  242. Vector3 rotationNormal = Vector3.Cross(axis, L);
  243. // Find the line where this plane intersects with the reach cone plane
  244. L = Vector3.Cross(-reachCones[r].B, rotationNormal);
  245. // Rotation from current(illegal) swing rotation to the limited(legal) swing rotation
  246. Quaternion toLimits = Quaternion.FromToRotation(rotation * axis, L);
  247. // Subtract the illegal rotation
  248. return toLimits * rotation;
  249. }
  250. /*
  251. * Finding the reach cone to test against
  252. * */
  253. private int GetReachCone(Vector3 L) {
  254. float p = 0;
  255. float p1 = Vector3.Dot(reachCones[0].S, L);
  256. for (int i = 0; i < reachCones.Length; i++) {
  257. p = p1;
  258. if (i < reachCones.Length - 1) p1 = Vector3.Dot(reachCones[i + 1].S, L);
  259. else p1 = Vector3.Dot(reachCones[0].S, L);
  260. if (p >= 0 && p1 < 0) return i;
  261. }
  262. return -1;
  263. }
  264. #endregion Runtime calculations
  265. }
  266. }