/Scripts/MathUtils.cs

  1using UnityEngine;
  2using System.Collections;
  3using System.Collections.Generic;
  4
  5public class MathUtils
  6{
  7	public static float GetQuatLength(Quaternion q)
  8	{
  9		return Mathf.Sqrt(q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w);
 10	}
 11
 12	public static Quaternion GetQuatConjugate(Quaternion q)
 13	{
 14		return new Quaternion(-q.x, -q.y, -q.z, q.w);
 15	}
 16
 17	/// <summary>
 18	/// Logarithm of a unit quaternion. The result is not necessary a unit quaternion.
 19	/// </summary>
 20	public static Quaternion GetQuatLog(Quaternion q)
 21	{
 22		Quaternion res = q;
 23		res.w = 0;
 24
 25		if (Mathf.Abs(q.w) < 1.0f)
 26		{
 27			float theta = Mathf.Acos(q.w);
 28			float sin_theta = Mathf.Sin(theta);
 29
 30			if (Mathf.Abs(sin_theta) > 0.0001)
 31			{
 32				float coef = theta / sin_theta;
 33				res.x = q.x * coef;
 34				res.y = q.y * coef;
 35				res.z = q.z * coef;
 36			}
 37		}
 38
 39		return res;
 40	}
 41
 42	public static Quaternion GetQuatExp(Quaternion q)
 43	{
 44		Quaternion res = q;
 45
 46		float fAngle = Mathf.Sqrt(q.x * q.x + q.y * q.y + q.z * q.z);
 47		float fSin = Mathf.Sin(fAngle);
 48
 49		res.w = Mathf.Cos(fAngle);
 50
 51		if (Mathf.Abs(fSin) > 0.0001)
 52		{
 53			float coef = fSin / fAngle;
 54			res.x = coef * q.x;
 55			res.y = coef * q.y;
 56			res.z = coef * q.z;
 57		}
 58
 59		return res;
 60	}
 61
 62	/// <summary>
 63	/// SQUAD Spherical Quadrangle interpolation [Shoe87]
 64	/// </summary>
 65	public static Quaternion GetQuatSquad(float t, Quaternion q0, Quaternion q1, Quaternion a0, Quaternion a1)
 66	{
 67		float slerpT = 2.0f * t * (1.0f - t);
 68
 69		Quaternion slerpP = Slerp(q0, q1, t);
 70		Quaternion slerpQ = Slerp(a0, a1, t);
 71
 72		return Slerp(slerpP, slerpQ, slerpT);
 73	}
 74
 75	public static Quaternion GetSquadIntermediate(Quaternion q0, Quaternion q1, Quaternion q2)
 76	{
 77		Quaternion q1Inv = GetQuatConjugate(q1);
 78		Quaternion p0 = GetQuatLog(q1Inv * q0);
 79		Quaternion p2 = GetQuatLog(q1Inv * q2);
 80		Quaternion sum = new Quaternion(-0.25f * (p0.x + p2.x), -0.25f * (p0.y + p2.y), -0.25f * (p0.z + p2.z), -0.25f * (p0.w + p2.w));
 81
 82		return q1 * GetQuatExp(sum);
 83	}
 84
 85	/// <summary>
 86	/// Smooths the input parameter t.
 87	/// If less than k1 ir greater than k2, it uses a sin.
 88	/// Between k1 and k2 it uses linear interp.
 89	/// </summary>
 90	public static float Ease(float t, float k1, float k2)
 91	{
 92		float f; float s;
 93
 94		f = k1 * 2 / Mathf.PI + k2 - k1 + (1.0f - k2) * 2 / Mathf.PI;
 95
 96		if (t < k1)
 97		{
 98			s = k1 * (2 / Mathf.PI) * (Mathf.Sin((t / k1) * Mathf.PI / 2 - Mathf.PI / 2) + 1);
 99		}
100		else
101			if (t < k2)
102			{
103				s = (2 * k1 / Mathf.PI + t - k1);
104			}
105			else
106			{
107				s = 2 * k1 / Mathf.PI + k2 - k1 + ((1 - k2) * (2 / Mathf.PI)) * Mathf.Sin(((t - k2) / (1.0f - k2)) * Mathf.PI / 2);
108			}
109
110		return (s / f);
111	}
112
113	/// <summary>
114	/// We need this because Quaternion.Slerp always uses the shortest arc.
115	/// </summary>
116	public static Quaternion Slerp(Quaternion p, Quaternion q, float t)
117	{
118		Quaternion ret;
119
120		float fCos = Quaternion.Dot(p, q);
121
122		if ((1.0f + fCos) > 0.00001)
123		{
124			float fCoeff0, fCoeff1;
125
126			if ((1.0f - fCos) > 0.00001)
127			{
128				float omega = Mathf.Acos(fCos);
129				float invSin = 1.0f / Mathf.Sin(omega);
130				fCoeff0 = Mathf.Sin((1.0f - t) * omega) * invSin;
131				fCoeff1 = Mathf.Sin(t * omega) * invSin;
132			}
133			else
134			{
135				fCoeff0 = 1.0f - t;
136				fCoeff1 = t;
137			}
138
139			ret.x = fCoeff0 * p.x + fCoeff1 * q.x;
140			ret.y = fCoeff0 * p.y + fCoeff1 * q.y;
141			ret.z = fCoeff0 * p.z + fCoeff1 * q.z;
142			ret.w = fCoeff0 * p.w + fCoeff1 * q.w;
143		}
144		else
145		{
146			float fCoeff0 = Mathf.Sin((1.0f - t) * Mathf.PI * 0.5f);
147			float fCoeff1 = Mathf.Sin(t * Mathf.PI * 0.5f);
148
149			ret.x = fCoeff0 * p.x - fCoeff1 * p.y;
150			ret.y = fCoeff0 * p.y + fCoeff1 * p.x;
151			ret.z = fCoeff0 * p.z - fCoeff1 * p.w;
152			ret.w = p.z;
153		}
154
155		return ret;
156	}
157}