QuaTools.cs 9.4 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265
  1. using UnityEngine;
  2. using System.Collections;
  3. namespace RootMotion {
  4. /// <summary>
  5. /// Helper methods for dealing with Quaternions.
  6. /// </summary>
  7. public static class QuaTools {
  8. /// <summary>
  9. /// Returns yaw angle (-180 - 180) of 'forward' vector relative to rotation space defined by spaceForward and spaceUp axes.
  10. /// </summary>
  11. public static float GetYaw(Quaternion space, Vector3 forward)
  12. {
  13. Vector3 dirLocal = Quaternion.Inverse(space) * forward;
  14. return Mathf.Atan2(dirLocal.x, dirLocal.z) * Mathf.Rad2Deg;
  15. }
  16. /// <summary>
  17. /// Returns pitch angle (-90 - 90) of 'forward' vector relative to rotation space defined by spaceForward and spaceUp axes.
  18. /// </summary>
  19. public static float GetPitch(Quaternion space, Vector3 forward)
  20. {
  21. forward = forward.normalized;
  22. Vector3 dirLocal = Quaternion.Inverse(space) * forward;
  23. return -Mathf.Asin(dirLocal.y) * Mathf.Rad2Deg;
  24. }
  25. /// <summary>
  26. /// Returns bank angle (-180 - 180) of 'forward' and 'up' vectors relative to rotation space defined by spaceForward and spaceUp axes.
  27. /// </summary>
  28. public static float GetBank(Quaternion space, Vector3 forward, Vector3 up)
  29. {
  30. Vector3 spaceUp = space * Vector3.up;
  31. Quaternion invSpace = Quaternion.Inverse(space);
  32. forward = invSpace * forward;
  33. up = invSpace * up;
  34. Quaternion q = Quaternion.Inverse(Quaternion.LookRotation(spaceUp, forward));
  35. up = q * up;
  36. return Mathf.Atan2(up.x, up.z) * Mathf.Rad2Deg;
  37. }
  38. /// <summary>
  39. /// Returns yaw angle (-180 - 180) of 'forward' vector relative to rotation space defined by spaceForward and spaceUp axes.
  40. /// </summary>
  41. public static float GetYaw(Quaternion space, Quaternion rotation)
  42. {
  43. Vector3 dirLocal = Quaternion.Inverse(space) * (rotation * Vector3.forward);
  44. return Mathf.Atan2(dirLocal.x, dirLocal.z) * Mathf.Rad2Deg;
  45. }
  46. /// <summary>
  47. /// Returns pitch angle (-90 - 90) of 'forward' vector relative to rotation space defined by spaceForward and spaceUp axes.
  48. /// </summary>
  49. public static float GetPitch(Quaternion space, Quaternion rotation)
  50. {
  51. Vector3 dirLocal = Quaternion.Inverse(space) * (rotation * Vector3.forward);
  52. return -Mathf.Asin(dirLocal.y) * Mathf.Rad2Deg;
  53. }
  54. /// <summary>
  55. /// Returns bank angle (-180 - 180) of 'forward' and 'up' vectors relative to rotation space defined by spaceForward and spaceUp axes.
  56. /// </summary>
  57. public static float GetBank(Quaternion space, Quaternion rotation)
  58. {
  59. Vector3 spaceUp = space * Vector3.up;
  60. Quaternion invSpace = Quaternion.Inverse(space);
  61. Vector3 forward = invSpace * (rotation * Vector3.forward);
  62. Vector3 up = invSpace * (rotation * Vector3.up);
  63. Quaternion q = Quaternion.Inverse(Quaternion.LookRotation(spaceUp, forward));
  64. up = q * up;
  65. return Mathf.Atan2(up.x, up.z) * Mathf.Rad2Deg;
  66. }
  67. /// <summary>
  68. /// Optimized Quaternion.Lerp
  69. /// </summary>
  70. public static Quaternion Lerp(Quaternion fromRotation, Quaternion toRotation, float weight) {
  71. if (weight <= 0f) return fromRotation;
  72. if (weight >= 1f) return toRotation;
  73. return Quaternion.Lerp(fromRotation, toRotation, weight);
  74. }
  75. /// <summary>
  76. /// Optimized Quaternion.Slerp
  77. /// </summary>
  78. public static Quaternion Slerp(Quaternion fromRotation, Quaternion toRotation, float weight) {
  79. if (weight <= 0f) return fromRotation;
  80. if (weight >= 1f) return toRotation;
  81. return Quaternion.Slerp(fromRotation, toRotation, weight);
  82. }
  83. /// <summary>
  84. /// Returns the rotation from identity Quaternion to "q", interpolated linearily by "weight".
  85. /// </summary>
  86. public static Quaternion LinearBlend(Quaternion q, float weight) {
  87. if (weight <= 0f) return Quaternion.identity;
  88. if (weight >= 1f) return q;
  89. return Quaternion.Lerp(Quaternion.identity, q, weight);
  90. }
  91. /// <summary>
  92. /// Returns the rotation from identity Quaternion to "q", interpolated spherically by "weight".
  93. /// </summary>
  94. public static Quaternion SphericalBlend(Quaternion q, float weight) {
  95. if (weight <= 0f) return Quaternion.identity;
  96. if (weight >= 1f) return q;
  97. return Quaternion.Slerp(Quaternion.identity, q, weight);
  98. }
  99. /// <summary>
  100. /// Creates a FromToRotation, but makes sure it's axis remains fixed near to the Quaternion singularity point.
  101. /// </summary>
  102. /// <returns>
  103. /// The from to rotation around an axis.
  104. /// </returns>
  105. /// <param name='fromDirection'>
  106. /// From direction.
  107. /// </param>
  108. /// <param name='toDirection'>
  109. /// To direction.
  110. /// </param>
  111. /// <param name='axis'>
  112. /// Axis. Should be normalized before passing into this method.
  113. /// </param>
  114. public static Quaternion FromToAroundAxis(Vector3 fromDirection, Vector3 toDirection, Vector3 axis) {
  115. Quaternion fromTo = Quaternion.FromToRotation(fromDirection, toDirection);
  116. float angle = 0;
  117. Vector3 freeAxis = Vector3.zero;
  118. fromTo.ToAngleAxis(out angle, out freeAxis);
  119. float dot = Vector3.Dot(freeAxis, axis);
  120. if (dot < 0) angle = -angle;
  121. return Quaternion.AngleAxis(angle, axis);
  122. }
  123. /// <summary>
  124. /// Gets the rotation that can be used to convert a rotation from one axis space to another.
  125. /// </summary>
  126. public static Quaternion RotationToLocalSpace(Quaternion space, Quaternion rotation) {
  127. return Quaternion.Inverse(Quaternion.Inverse(space) * rotation);
  128. }
  129. /// <summary>
  130. /// Gets the Quaternion from rotation "from" to rotation "to".
  131. /// </summary>
  132. public static Quaternion FromToRotation(Quaternion from, Quaternion to) {
  133. if (to == from) return Quaternion.identity;
  134. return to * Quaternion.Inverse(from);
  135. }
  136. /// <summary>
  137. /// Gets the closest direction axis to a vector. Input vector must be normalized!
  138. /// </summary>
  139. public static Vector3 GetAxis(Vector3 v) {
  140. Vector3 closest = Vector3.right;
  141. bool neg = false;
  142. float x = Vector3.Dot(v, Vector3.right);
  143. float maxAbsDot = Mathf.Abs(x);
  144. if (x < 0f) neg = true;
  145. float y = Vector3.Dot(v, Vector3.up);
  146. float absDot = Mathf.Abs(y);
  147. if (absDot > maxAbsDot) {
  148. maxAbsDot = absDot;
  149. closest = Vector3.up;
  150. neg = y < 0f;
  151. }
  152. float z = Vector3.Dot(v, Vector3.forward);
  153. absDot = Mathf.Abs(z);
  154. if (absDot > maxAbsDot) {
  155. closest = Vector3.forward;
  156. neg = z < 0f;
  157. }
  158. if (neg) closest = -closest;
  159. return closest;
  160. }
  161. /// <summary>
  162. /// Clamps the rotation similar to V3Tools.ClampDirection.
  163. /// </summary>
  164. public static Quaternion ClampRotation(Quaternion rotation, float clampWeight, int clampSmoothing) {
  165. if (clampWeight >= 1f) return Quaternion.identity;
  166. if (clampWeight <= 0f) return rotation;
  167. float angle = Quaternion.Angle(Quaternion.identity, rotation);
  168. float dot = 1f - (angle / 180f);
  169. float targetClampMlp = Mathf.Clamp(1f - ((clampWeight - dot) / (1f - dot)), 0f, 1f);
  170. float clampMlp = Mathf.Clamp(dot / clampWeight, 0f, 1f);
  171. // Sine smoothing iterations
  172. for (int i = 0; i < clampSmoothing; i++) {
  173. float sinF = clampMlp * Mathf.PI * 0.5f;
  174. clampMlp = Mathf.Sin(sinF);
  175. }
  176. return Quaternion.Slerp(Quaternion.identity, rotation, clampMlp * targetClampMlp);
  177. }
  178. /// <summary>
  179. /// Clamps an angular value.
  180. /// </summary>
  181. public static float ClampAngle(float angle, float clampWeight, int clampSmoothing) {
  182. if (clampWeight >= 1f) return 0f;
  183. if (clampWeight <= 0f) return angle;
  184. float dot = 1f - (Mathf.Abs(angle) / 180f);
  185. float targetClampMlp = Mathf.Clamp(1f - ((clampWeight - dot) / (1f - dot)), 0f, 1f);
  186. float clampMlp = Mathf.Clamp(dot / clampWeight, 0f, 1f);
  187. // Sine smoothing iterations
  188. for (int i = 0; i < clampSmoothing; i++) {
  189. float sinF = clampMlp * Mathf.PI * 0.5f;
  190. clampMlp = Mathf.Sin(sinF);
  191. }
  192. return Mathf.Lerp(0f, angle, clampMlp * targetClampMlp);
  193. }
  194. /// <summary>
  195. /// Used for matching the rotations of objects that have different orientations.
  196. /// </summary>
  197. public static Quaternion MatchRotation(Quaternion targetRotation, Vector3 targetforwardAxis, Vector3 targetUpAxis, Vector3 forwardAxis, Vector3 upAxis) {
  198. Quaternion f = Quaternion.LookRotation(forwardAxis, upAxis);
  199. Quaternion fTarget = Quaternion.LookRotation(targetforwardAxis, targetUpAxis);
  200. Quaternion d = targetRotation * fTarget;
  201. return d * Quaternion.Inverse(f);
  202. }
  203. /// <summary>
  204. /// Converts an Euler rotation from 0 to 360 representation to -180 to 180.
  205. /// </summary>
  206. public static Vector3 ToBiPolar(Vector3 euler)
  207. {
  208. return new Vector3(ToBiPolar(euler.x), ToBiPolar(euler.y), ToBiPolar(euler.z));
  209. }
  210. /// <summary>
  211. /// Converts an angular value from 0 to 360 representation to -180 to 180.
  212. /// </summary>
  213. public static float ToBiPolar(float angle)
  214. {
  215. angle = angle % 360f;
  216. if (angle >= 180f) return angle - 360f;
  217. if (angle <= -180f) return angle + 360f;
  218. return angle;
  219. }
  220. }
  221. }