/Scripts/SplineInterpolator.cs

  1using UnityEngine;
  2using System.Collections;
  3using System.Collections.Generic;
  4
  5public enum eEndPointsMode { AUTO, AUTOCLOSED, EXPLICIT }
  6public enum eWrapMode { ONCE, LOOP }
  7public delegate void OnEndCallback();
  8
  9public class SplineInterpolator : MonoBehaviour
 10{
 11	eEndPointsMode mEndPointsMode = eEndPointsMode.AUTO;
 12
 13	internal class SplineNode
 14	{
 15		internal Vector3 Point;
 16		internal Quaternion Rot;
 17		internal float Time;
 18		internal Vector2 EaseIO;
 19
 20		internal SplineNode(Vector3 p, Quaternion q, float t, Vector2 io) { Point = p; Rot = q; Time = t; EaseIO = io; }
 21		internal SplineNode(SplineNode o) { Point = o.Point; Rot = o.Rot; Time = o.Time; EaseIO = o.EaseIO; }
 22	}
 23
 24	List<SplineNode> mNodes = new List<SplineNode>();
 25	string mState = "";
 26	bool mRotations;
 27
 28	OnEndCallback mOnEndCallback;
 29
 30
 31
 32	void Awake()
 33	{
 34		Reset();
 35	}
 36
 37	public void StartInterpolation(OnEndCallback endCallback, bool bRotations, eWrapMode mode)
 38	{
 39		if (mState != "Reset")
 40			throw new System.Exception("First reset, add points and then call here");
 41
 42		mState = mode == eWrapMode.ONCE ? "Once" : "Loop";
 43		mRotations = bRotations;
 44		mOnEndCallback = endCallback;
 45
 46		SetInput();
 47	}
 48
 49	public void Reset()
 50	{
 51		mNodes.Clear();
 52		mState = "Reset";
 53		mCurrentIdx = 1;
 54		mCurrentTime = 0;
 55		mRotations = false;
 56		mEndPointsMode = eEndPointsMode.AUTO;
 57	}
 58
 59	public void AddPoint(Vector3 pos, Quaternion quat, float timeInSeconds, Vector2 easeInOut)
 60	{
 61		if (mState != "Reset")
 62			throw new System.Exception("Cannot add points after start");
 63
 64		mNodes.Add(new SplineNode(pos, quat, timeInSeconds, easeInOut));
 65	}
 66
 67
 68	void SetInput()
 69	{
 70		if (mNodes.Count < 2)
 71			throw new System.Exception("Invalid number of points");
 72
 73		if (mRotations)
 74		{
 75			for (int c = 1; c < mNodes.Count; c++)
 76			{
 77				SplineNode node = mNodes[c];
 78				SplineNode prevNode = mNodes[c - 1];
 79
 80				// Always interpolate using the shortest path -> Selective negation
 81				if (Quaternion.Dot(node.Rot, prevNode.Rot) < 0)
 82				{
 83					node.Rot.x = -node.Rot.x;
 84					node.Rot.y = -node.Rot.y;
 85					node.Rot.z = -node.Rot.z;
 86					node.Rot.w = -node.Rot.w;
 87				}
 88			}
 89		}
 90
 91		if (mEndPointsMode == eEndPointsMode.AUTO)
 92		{
 93			mNodes.Insert(0, mNodes[0]);
 94			mNodes.Add(mNodes[mNodes.Count - 1]);
 95		}
 96		else if (mEndPointsMode == eEndPointsMode.EXPLICIT && (mNodes.Count < 4))
 97			throw new System.Exception("Invalid number of points");
 98	}
 99
100	void SetExplicitMode()
101	{
102		if (mState != "Reset")
103			throw new System.Exception("Cannot change mode after start");
104
105		mEndPointsMode = eEndPointsMode.EXPLICIT;
106	}
107
108	public void SetAutoCloseMode(float joiningPointTime)
109	{
110		if (mState != "Reset")
111			throw new System.Exception("Cannot change mode after start");
112
113		mEndPointsMode = eEndPointsMode.AUTOCLOSED;
114
115		mNodes.Add(new SplineNode(mNodes[0] as SplineNode));
116		mNodes[mNodes.Count - 1].Time = joiningPointTime;
117
118		Vector3 vInitDir = (mNodes[1].Point - mNodes[0].Point).normalized;
119		Vector3 vEndDir = (mNodes[mNodes.Count - 2].Point - mNodes[mNodes.Count - 1].Point).normalized;
120		float firstLength = (mNodes[1].Point - mNodes[0].Point).magnitude;
121		float lastLength = (mNodes[mNodes.Count - 2].Point - mNodes[mNodes.Count - 1].Point).magnitude;
122
123		SplineNode firstNode = new SplineNode(mNodes[0] as SplineNode);
124		firstNode.Point = mNodes[0].Point + vEndDir * firstLength;
125
126		SplineNode lastNode = new SplineNode(mNodes[mNodes.Count - 1] as SplineNode);
127		lastNode.Point = mNodes[0].Point + vInitDir * lastLength;
128
129		mNodes.Insert(0, firstNode);
130		mNodes.Add(lastNode);
131	}
132
133	float mCurrentTime;
134	int mCurrentIdx = 1;
135
136	void Update()
137	{
138		if (mState == "Reset" || mState == "Stopped" || mNodes.Count < 4)
139			return;
140
141		mCurrentTime += Time.deltaTime;
142
143		// We advance to next point in the path
144		if (mCurrentTime >= mNodes[mCurrentIdx + 1].Time)
145		{
146			if (mCurrentIdx < mNodes.Count - 3)
147			{
148				mCurrentIdx++;
149			}
150			else
151			{
152				if (mState != "Loop")
153				{
154					mState = "Stopped";
155
156					// We stop right in the end point
157					transform.position = mNodes[mNodes.Count - 2].Point;
158
159					if (mRotations)
160						transform.rotation = mNodes[mNodes.Count - 2].Rot;
161
162					// We call back to inform that we are ended
163					if (mOnEndCallback != null)
164						mOnEndCallback();
165				}
166				else
167				{
168					mCurrentIdx = 1;
169					mCurrentTime = 0;
170				}
171			}
172		}
173
174		if (mState != "Stopped")
175		{
176			// Calculates the t param between 0 and 1
177			float param = (mCurrentTime - mNodes[mCurrentIdx].Time) / (mNodes[mCurrentIdx + 1].Time - mNodes[mCurrentIdx].Time);
178
179			// Smooth the param
180			param = MathUtils.Ease(param, mNodes[mCurrentIdx].EaseIO.x, mNodes[mCurrentIdx].EaseIO.y);
181
182			transform.position = GetHermiteInternal(mCurrentIdx, param);
183
184			if (mRotations)
185			{
186				transform.rotation = GetSquad(mCurrentIdx, param);
187			}
188		}
189	}
190
191	Quaternion GetSquad(int idxFirstPoint, float t)
192	{
193		Quaternion Q0 = mNodes[idxFirstPoint - 1].Rot;
194		Quaternion Q1 = mNodes[idxFirstPoint].Rot;
195		Quaternion Q2 = mNodes[idxFirstPoint + 1].Rot;
196		Quaternion Q3 = mNodes[idxFirstPoint + 2].Rot;
197
198		Quaternion T1 = MathUtils.GetSquadIntermediate(Q0, Q1, Q2);
199		Quaternion T2 = MathUtils.GetSquadIntermediate(Q1, Q2, Q3);
200
201		return MathUtils.GetQuatSquad(t, Q1, Q2, T1, T2);
202	}
203
204
205
206	public Vector3 GetHermiteInternal(int idxFirstPoint, float t)
207	{
208		float t2 = t * t;
209		float t3 = t2 * t;
210
211		Vector3 P0 = mNodes[idxFirstPoint - 1].Point;
212		Vector3 P1 = mNodes[idxFirstPoint].Point;
213		Vector3 P2 = mNodes[idxFirstPoint + 1].Point;
214		Vector3 P3 = mNodes[idxFirstPoint + 2].Point;
215
216		float tension = 0.5f;	// 0.5 equivale a catmull-rom
217
218		Vector3 T1 = tension * (P2 - P0);
219		Vector3 T2 = tension * (P3 - P1);
220
221		float Blend1 = 2 * t3 - 3 * t2 + 1;
222		float Blend2 = -2 * t3 + 3 * t2;
223		float Blend3 = t3 - 2 * t2 + t;
224		float Blend4 = t3 - t2;
225
226		return Blend1 * P1 + Blend2 * P2 + Blend3 * T1 + Blend4 * T2;
227	}
228
229
230	public Vector3 GetHermiteAtTime(float timeParam)
231	{
232		if (timeParam >= mNodes[mNodes.Count - 2].Time)
233			return mNodes[mNodes.Count - 2].Point;
234
235		int c;
236		for (c = 1; c < mNodes.Count - 2; c++)
237		{
238			if (mNodes[c].Time > timeParam)
239				break;
240		}
241
242		int idx = c - 1;
243		float param = (timeParam - mNodes[idx].Time) / (mNodes[idx + 1].Time - mNodes[idx].Time);
244		param = MathUtils.Ease(param, mNodes[idx].EaseIO.x, mNodes[idx].EaseIO.y);
245
246		return GetHermiteInternal(idx, param);
247	}
248}