123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265 |
- using UnityEngine;
- using System.Collections;
- namespace RootMotion {
- /// <summary>
- /// Helper methods for dealing with Quaternions.
- /// </summary>
- public static class QuaTools {
- /// <summary>
- /// Returns yaw angle (-180 - 180) of 'forward' vector relative to rotation space defined by spaceForward and spaceUp axes.
- /// </summary>
- public static float GetYaw(Quaternion space, Vector3 forward)
- {
- Vector3 dirLocal = Quaternion.Inverse(space) * forward;
- return Mathf.Atan2(dirLocal.x, dirLocal.z) * Mathf.Rad2Deg;
- }
- /// <summary>
- /// Returns pitch angle (-90 - 90) of 'forward' vector relative to rotation space defined by spaceForward and spaceUp axes.
- /// </summary>
- public static float GetPitch(Quaternion space, Vector3 forward)
- {
- forward = forward.normalized;
- Vector3 dirLocal = Quaternion.Inverse(space) * forward;
- return -Mathf.Asin(dirLocal.y) * Mathf.Rad2Deg;
- }
- /// <summary>
- /// Returns bank angle (-180 - 180) of 'forward' and 'up' vectors relative to rotation space defined by spaceForward and spaceUp axes.
- /// </summary>
- public static float GetBank(Quaternion space, Vector3 forward, Vector3 up)
- {
- Vector3 spaceUp = space * Vector3.up;
- Quaternion invSpace = Quaternion.Inverse(space);
- forward = invSpace * forward;
- up = invSpace * up;
- Quaternion q = Quaternion.Inverse(Quaternion.LookRotation(spaceUp, forward));
- up = q * up;
- return Mathf.Atan2(up.x, up.z) * Mathf.Rad2Deg;
- }
- /// <summary>
- /// Returns yaw angle (-180 - 180) of 'forward' vector relative to rotation space defined by spaceForward and spaceUp axes.
- /// </summary>
- public static float GetYaw(Quaternion space, Quaternion rotation)
- {
- Vector3 dirLocal = Quaternion.Inverse(space) * (rotation * Vector3.forward);
- return Mathf.Atan2(dirLocal.x, dirLocal.z) * Mathf.Rad2Deg;
- }
- /// <summary>
- /// Returns pitch angle (-90 - 90) of 'forward' vector relative to rotation space defined by spaceForward and spaceUp axes.
- /// </summary>
- public static float GetPitch(Quaternion space, Quaternion rotation)
- {
- Vector3 dirLocal = Quaternion.Inverse(space) * (rotation * Vector3.forward);
- return -Mathf.Asin(dirLocal.y) * Mathf.Rad2Deg;
- }
- /// <summary>
- /// Returns bank angle (-180 - 180) of 'forward' and 'up' vectors relative to rotation space defined by spaceForward and spaceUp axes.
- /// </summary>
- public static float GetBank(Quaternion space, Quaternion rotation)
- {
- Vector3 spaceUp = space * Vector3.up;
-
- Quaternion invSpace = Quaternion.Inverse(space);
- Vector3 forward = invSpace * (rotation * Vector3.forward);
- Vector3 up = invSpace * (rotation * Vector3.up);
- Quaternion q = Quaternion.Inverse(Quaternion.LookRotation(spaceUp, forward));
- up = q * up;
- return Mathf.Atan2(up.x, up.z) * Mathf.Rad2Deg;
- }
- /// <summary>
- /// Optimized Quaternion.Lerp
- /// </summary>
- public static Quaternion Lerp(Quaternion fromRotation, Quaternion toRotation, float weight) {
- if (weight <= 0f) return fromRotation;
- if (weight >= 1f) return toRotation;
- return Quaternion.Lerp(fromRotation, toRotation, weight);
- }
- /// <summary>
- /// Optimized Quaternion.Slerp
- /// </summary>
- public static Quaternion Slerp(Quaternion fromRotation, Quaternion toRotation, float weight) {
- if (weight <= 0f) return fromRotation;
- if (weight >= 1f) return toRotation;
- return Quaternion.Slerp(fromRotation, toRotation, weight);
- }
- /// <summary>
- /// Returns the rotation from identity Quaternion to "q", interpolated linearily by "weight".
- /// </summary>
- public static Quaternion LinearBlend(Quaternion q, float weight) {
- if (weight <= 0f) return Quaternion.identity;
- if (weight >= 1f) return q;
- return Quaternion.Lerp(Quaternion.identity, q, weight);
- }
- /// <summary>
- /// Returns the rotation from identity Quaternion to "q", interpolated spherically by "weight".
- /// </summary>
- public static Quaternion SphericalBlend(Quaternion q, float weight) {
- if (weight <= 0f) return Quaternion.identity;
- if (weight >= 1f) return q;
- return Quaternion.Slerp(Quaternion.identity, q, weight);
- }
- /// <summary>
- /// Creates a FromToRotation, but makes sure it's axis remains fixed near to the Quaternion singularity point.
- /// </summary>
- /// <returns>
- /// The from to rotation around an axis.
- /// </returns>
- /// <param name='fromDirection'>
- /// From direction.
- /// </param>
- /// <param name='toDirection'>
- /// To direction.
- /// </param>
- /// <param name='axis'>
- /// Axis. Should be normalized before passing into this method.
- /// </param>
- public static Quaternion FromToAroundAxis(Vector3 fromDirection, Vector3 toDirection, Vector3 axis) {
- Quaternion fromTo = Quaternion.FromToRotation(fromDirection, toDirection);
-
- float angle = 0;
- Vector3 freeAxis = Vector3.zero;
-
- fromTo.ToAngleAxis(out angle, out freeAxis);
-
- float dot = Vector3.Dot(freeAxis, axis);
- if (dot < 0) angle = -angle;
-
- return Quaternion.AngleAxis(angle, axis);
- }
-
- /// <summary>
- /// Gets the rotation that can be used to convert a rotation from one axis space to another.
- /// </summary>
- public static Quaternion RotationToLocalSpace(Quaternion space, Quaternion rotation) {
- return Quaternion.Inverse(Quaternion.Inverse(space) * rotation);
- }
- /// <summary>
- /// Gets the Quaternion from rotation "from" to rotation "to".
- /// </summary>
- public static Quaternion FromToRotation(Quaternion from, Quaternion to) {
- if (to == from) return Quaternion.identity;
- return to * Quaternion.Inverse(from);
- }
- /// <summary>
- /// Gets the closest direction axis to a vector. Input vector must be normalized!
- /// </summary>
- public static Vector3 GetAxis(Vector3 v) {
- Vector3 closest = Vector3.right;
- bool neg = false;
- float x = Vector3.Dot(v, Vector3.right);
- float maxAbsDot = Mathf.Abs(x);
- if (x < 0f) neg = true;
- float y = Vector3.Dot(v, Vector3.up);
- float absDot = Mathf.Abs(y);
- if (absDot > maxAbsDot) {
- maxAbsDot = absDot;
- closest = Vector3.up;
- neg = y < 0f;
- }
- float z = Vector3.Dot(v, Vector3.forward);
- absDot = Mathf.Abs(z);
- if (absDot > maxAbsDot) {
- closest = Vector3.forward;
- neg = z < 0f;
- }
- if (neg) closest = -closest;
- return closest;
- }
- /// <summary>
- /// Clamps the rotation similar to V3Tools.ClampDirection.
- /// </summary>
- public static Quaternion ClampRotation(Quaternion rotation, float clampWeight, int clampSmoothing) {
- if (clampWeight >= 1f) return Quaternion.identity;
- if (clampWeight <= 0f) return rotation;
- float angle = Quaternion.Angle(Quaternion.identity, rotation);
- float dot = 1f - (angle / 180f);
- float targetClampMlp = Mathf.Clamp(1f - ((clampWeight - dot) / (1f - dot)), 0f, 1f);
- float clampMlp = Mathf.Clamp(dot / clampWeight, 0f, 1f);
-
- // Sine smoothing iterations
- for (int i = 0; i < clampSmoothing; i++) {
- float sinF = clampMlp * Mathf.PI * 0.5f;
- clampMlp = Mathf.Sin(sinF);
- }
-
- return Quaternion.Slerp(Quaternion.identity, rotation, clampMlp * targetClampMlp);
- }
- /// <summary>
- /// Clamps an angular value.
- /// </summary>
- public static float ClampAngle(float angle, float clampWeight, int clampSmoothing) {
- if (clampWeight >= 1f) return 0f;
- if (clampWeight <= 0f) return angle;
-
- float dot = 1f - (Mathf.Abs(angle) / 180f);
- float targetClampMlp = Mathf.Clamp(1f - ((clampWeight - dot) / (1f - dot)), 0f, 1f);
- float clampMlp = Mathf.Clamp(dot / clampWeight, 0f, 1f);
-
- // Sine smoothing iterations
- for (int i = 0; i < clampSmoothing; i++) {
- float sinF = clampMlp * Mathf.PI * 0.5f;
- clampMlp = Mathf.Sin(sinF);
- }
-
- return Mathf.Lerp(0f, angle, clampMlp * targetClampMlp);
- }
- /// <summary>
- /// Used for matching the rotations of objects that have different orientations.
- /// </summary>
- public static Quaternion MatchRotation(Quaternion targetRotation, Vector3 targetforwardAxis, Vector3 targetUpAxis, Vector3 forwardAxis, Vector3 upAxis) {
- Quaternion f = Quaternion.LookRotation(forwardAxis, upAxis);
- Quaternion fTarget = Quaternion.LookRotation(targetforwardAxis, targetUpAxis);
- Quaternion d = targetRotation * fTarget;
- return d * Quaternion.Inverse(f);
- }
- /// <summary>
- /// Converts an Euler rotation from 0 to 360 representation to -180 to 180.
- /// </summary>
- public static Vector3 ToBiPolar(Vector3 euler)
- {
- return new Vector3(ToBiPolar(euler.x), ToBiPolar(euler.y), ToBiPolar(euler.z));
- }
- /// <summary>
- /// Converts an angular value from 0 to 360 representation to -180 to 180.
- /// </summary>
- public static float ToBiPolar(float angle)
- {
- angle = angle % 360f;
- if (angle >= 180f) return angle - 360f;
- if (angle <= -180f) return angle + 360f;
- return angle;
- }
- }
- }
|