The idea to calculate Motion, Direction, Speed, Angle

Since I'm programmer but not the mathematician, I would like to share the way I used to calculate all those things.

In return I expect some of you will give me feedback, discussion or challenge, to learn more...
or even make it faster. (I bet you know some trick)

My major purpose to write this script ?
I usually need to develop enemy AI, apply fake physics on object...etc
In order to do that, I found myself need to re-calculate { speed, time, distance, direction, angle } across multiple script.
some of them are costly, e.g. vector3.magnitude, tranformVector...
This make me try to collect the common factor together into a single script with debug tools.

and here is one of the case, that I use that CMotionCache.cs it to debug some advance feature on top of it.

And yes. code here.
>> also can found the missing files append on this post
>> CMotionCache.cs & GizmosExtend.cs

Code (CSharp):

1. using UnityEngine;
2. using Kit;
3.  
4. public class CMotionCache : MonoBehaviour
5. {
6.     [Header("Setting")]
7.     [SerializeField, Tooltip("To define the world up for this object. require normalize vector (length = 1f)")]
8.     Vector3 m_BiasGroundNormal = Vector3.up;
9.     public Vector3 biasGroundNormal => m_BiasGroundNormal; // will normalize at OnEnable.
10.  
11.     [SerializeField] bool pauseOnTimeScaleZero = true;
12.  
13.     [Header("Debug")]
14.     [SerializeField] DebugInfo debugInfo;
15.  
16.     [System.Flags]
17.     private enum eDebugFlag
18.     {
19.         motion = 1 << 0,
20.         rotation = 1 << 1,
21.         speed = 1 << 2,
22.     }
23.  
24.     [System.Serializable]
25.     private class DebugInfo
26.     {
27.         [MaskField(typeof(eDebugFlag))] public eDebugFlag flag;
28.         public Vector2 offset;
29.  
30.         public Color colorMotionVelocity = Color.green;
31.         public Color colorMotionVector = Color.yellow;
32.         public Color colorMotionAngle = Color.cyan;
33.     }
34.  
35.     /// <summary>used for frame lock, prevent double calculation.</summary>
36.     private int m_CacheFrameCount = -1;
37.  
38.     /// <summary>The <see cref="Time.deltaTime"/>, used for calculation.</summary>
39.     public float deltaTime { get; private set; }
40.  
41.     /// <summary>World position on last frame</summary>
42.     public Vector3 lastPosition { get; private set; }
43.  
44.     /// <summary>the rotation on last frame</summary>
45.     public Quaternion lastRotation { get; private set; }
46.  
47.     /// <summary>The last forward from <see cref="lastRotation"/></summary>
48.     public Vector3 lastForward { get; private set; }
49.  
50.     /// <summary>The motion between <see cref="lastPosition"/> and current position</summary>
51.     public Vector3 motionVector { get; private set; }
52.  
53.     /// <summary>The motion vector based on local space <see cref="motionVector"/></summary>
54.     public Vector3 localMotionVector { get; private set; }
55.  
56.     /// <summary>The motion vector project on <see cref="m_BiasGroundNormal"/></summary>
57.     public Vector3 motionVectorIgnoreY { get; private set; }
58.  
59.     /// <summary>The local motion vector project on <see cref="m_BiasGroundNormal"/></summary>
60.     public Vector3 localMotionVectorIgnoreY { get; private set; }
61.  
62.     /// <summary>The motion direction (Normalize)</summary>
63.     public Vector3 motionDirection { get; private set; }
64.  
65.     /// <summary>The moving direction on local space</summary>
66.     public Vector3 localMotionDirection { get; private set; }
67.  
68.     /// <summary>The distance moved from <see cref="lastPosition"/></summary>
69.     public float motionDistance { get; private set; }
70.  
71.     /// <summary>The distance moved from <see cref="lastPosition"/>
72.     /// Ignore Y - (local space), are based on <see cref="m_BiasGroundNormal"/>.</summary>
73.     public float motionDistanceIgnoreY { get; private set; }
74.  
75.     /// <summary>The distance moved from <see cref="lastPosition"/>
76.     /// Ignore Y - (local space), are based on <see cref="m_BiasGroundNormal"/>.</summary>
77.     public float localMotionDistanceIgnoreY { get; private set; }
78.  
79.     /// <summary>The speed moved from <see cref="lastPosition"/> to current position.
80.     /// calculate by engine <see cref="Time.deltaTime"/></summary>
81.     public float motionSpeed { get; private set; }
82.  
83.     /// <summary>The speed project on <see cref="m_BiasGroundNormal"/> as floor,
84.     /// and separate into horizontal direction (XZ)</summary>
85.     public float localMotionSpeedXZ { get; private set; }
86.  
87.     /// <summary>The speed project on <see cref="m_BiasGroundNormal"/> itself
88.     /// and extract the vertical direction (Y)</summary>
89.     public float localMotionSpeedY { get; private set; }
90.  
91.     /// <summary>Velocity - The <see cref="motionSpeed"/> represent in vector</summary>
92.     public Vector3 motionVelocity { get; private set; }
93.  
94.     /// <summary>The steeringRotation between <see cref="lastRotation"/> and current rotation,
95.     /// <seealso cref="Quaternion.FromToRotation(Vector3, Vector3)"/></summary>
96.     public Quaternion steeringRotation { get; private set; }
97.  
98.     /// <summary>The different between <see cref="lastForward"/> and current forward</summary>
99.     public float steeringAngle { get; private set; }
100.  
101.     /// <summary>The angle between
102.     /// moving direction <see cref="motionVector"/>
103.     /// and actually facing direction <see cref="Transform.forward"/>
104.     /// 0 = align forward, -90 = left, +90 = right, -180/180 = backward</summary>
105.     public float angleBetweenMotionLookAt { get; private set; }
106.  
107.     /// <summary>0 = forward, Normalize -180~180 degree to -1f ~ 1f</summary>
108.     public float angleBetweenMotionLookAtNormalize { get; private set; }
109.  
110.     private void OnEnable()
111.     {
112.         ResetParams();
113.         DeltaCalculation();
114.     }
115.  
116.     private void Update()
117.     {
118.         DeltaCalculation();
119.     }
120.  
121.     private void LateUpdate()
122.     {
123.         // Final, update transform record
124.         lastPosition = transform.position;
125.         lastRotation = transform.rotation;
126.     }
127.  
128.     System.Text.StringBuilder sb = new System.Text.StringBuilder(100);
129.     private void OnDrawGizmos()
130.     {
131.         if (debugInfo.flag.HasFlag(eDebugFlag.motion))
132.         {
133.             sb.AppendLine($"Last position : {lastPosition:F1}");
134.             sb.AppendLine($"Motion distance : {motionDistance:F2}");
135.             if (motionVector != Vector3.zero)
136.                 GizmosExtend.DrawDirection(transform.position, motionVector, color: debugInfo.colorMotionVector);
137.         }
138.  
139.         if (debugInfo.flag.HasFlag(eDebugFlag.rotation))
140.         {
141.             sb.AppendLine($"Steering angle : {steeringAngle:F2}");
142.             sb.AppendLine($"Angle between Motion & LookAt : {angleBetweenMotionLookAt:F1}");
143.             if (motionVector != Vector3.zero)
144.                 GizmosExtend.DrawAngleBetween(transform.position, motionVector, transform.forward, biasGroundNormal, 1f, debugInfo.colorMotionAngle);
145.         }
146.  
147.         if (debugInfo.flag.HasFlag(eDebugFlag.speed))
148.         {
149.             sb.AppendLine($"Speed : {motionSpeed:F2}");
150.             sb.AppendLine($"Local Speed XZ : {localMotionSpeedXZ:F1}, Y : {localMotionSpeedY:F1}");
151.             if (motionVelocity != Vector3.zero)
152.                 GizmosExtend.DrawDirection(transform.position, motionVelocity, color: debugInfo.colorMotionVelocity);
153.         }
154.  
155.         if (sb.Length > 0)
156.         {
157.             GizmosExtend.DrawLabel(transform.position,
158.                 sb.ToString(),
159.                 offsetX: debugInfo.offset.x,
160.                 offsetY: debugInfo.offset.y);
161.             sb.Clear();
162.         }
163.     }
164.  
165.     private void DeltaCalculation()
166.     {
167.         if (m_CacheFrameCount == Time.frameCount || Time.deltaTime == 0f)
168.             return;
169.         m_CacheFrameCount = Time.frameCount;
170.  
171.         if (pauseOnTimeScaleZero && Time.timeScale == 0f)
172.             return;
173.  
174.         // time
175.         deltaTime = Time.deltaTime;
176.  
177.         // motion's
178.         motionVector = transform.position - lastPosition;
179.         localMotionVector = transform.InverseTransformVector(motionVector);
180.  
181.         // distance first, we have ZERO cases.
182.         motionDistance = motionVector.magnitude; // Cost
183.         if (motionDistance == 0f)
184.         {
185.             // didn't moved, in this case we can't calculate movement direction.
186.             // Fallback : align to current forward.
187.             motionDirection = localMotionDirection = transform.forward; // Gain
188.             motionVectorIgnoreY = localMotionVectorIgnoreY = Vector3.zero; // Gain
189.             motionDistanceIgnoreY = localMotionDistanceIgnoreY = 0f; // Gain
190.         }
191.         else
192.         {
193.             motionDirection = motionVector.normalized;
194.             localMotionDirection = localMotionVector.normalized; // Gain
195.             motionVectorIgnoreY = Vector3.ProjectOnPlane(motionVector, biasGroundNormal);
196.             motionDistanceIgnoreY = motionVectorIgnoreY.magnitude;
197.             localMotionVectorIgnoreY = new Vector3(localMotionVector.x, 0f, localMotionVector.z); // Gain - no calculation
198.             localMotionDistanceIgnoreY = new Vector2(localMotionVector.x, localMotionVector.z).magnitude; // Gain - Vector2
199.         }
200.  
201.         // Speed
202.         motionSpeed = motionDistance / deltaTime;
203.         motionVelocity = motionDirection * motionSpeed;
204.         localMotionSpeedXZ = motionDistanceIgnoreY / deltaTime;
205.         // math hack, since local's Y is actually moved distance along Y axis with correct sign.
206.         // cost was paid on previous matrix convertion, now we gain some back.
207.         /*
208.         // full version
209.         Vector3 y = Vector3.Project(motionVector, m_BiasGroundNormal);
210.         localMotionSpeedY = y.magnitude / deltaTime * Vector3.Dot(y.normalized, biasGroundNormal);
211.         */
212.         localMotionSpeedY = localMotionVector.y / deltaTime; // Gain
213.  
214.         // Rotation
215.         lastForward = lastRotation * Vector3.forward;
216.         steeringAngle = Vector3.SignedAngle(lastForward, transform.forward, biasGroundNormal);
217.         steeringRotation = Quaternion.FromToRotation(lastForward, transform.forward);
218.         if (motionDirection == Vector3.zero)
219.         {
220.             angleBetweenMotionLookAtNormalize = angleBetweenMotionLookAt = 0f;
221.         }
222.         else
223.         {
224.             angleBetweenMotionLookAt = Vector3.SignedAngle(motionDirection, transform.forward, biasGroundNormal);
225.             angleBetweenMotionLookAtNormalize = angleBetweenMotionLookAt / 180f;
226.         }
227.     }
228.  
229.     public void ResetParams()
230.     {
231.         m_BiasGroundNormal.Normalize();
232.  
233.         lastPosition = transform.position;
234.         lastForward = transform.forward;
235.         lastRotation = transform.rotation;
236.         steeringRotation = Quaternion.identity;
237.  
238.         motionVector = localMotionVector = motionVectorIgnoreY = localMotionVectorIgnoreY = motionDirection = localMotionDirection = motionVelocity = Vector3.zero;
239.      
240.         motionDistance = motionDistanceIgnoreY = localMotionDistanceIgnoreY = motionSpeed = localMotionSpeedXZ = localMotionSpeedY =
241.         steeringAngle = angleBetweenMotionLookAt = angleBetweenMotionLookAtNormalize = deltaTime = motionSpeed = 0f;
242.     }
243. }