using System; using UnityEngine; public class VelocityTracker : MonoBehaviour { public Vector3 FrameAngularVelocity { get { return this.m_frameAngularVelocity; } } public Vector3 FrameLinearVelocity { get { return this.m_frameLinearVelocity; } } public Vector3 TrackedAngularVelocity { get { return this.m_trackedAngularVelocity; } } public Vector3 TrackedLinearVelocity { get { return this.m_trackedLinearVelocity; } } private void Awake() { this.m_position = base.transform.position; this.m_rotation = base.transform.rotation; } private void FixedUpdate() { Vector3 position = base.transform.position; Vector3 deltaPosition = position - this.m_position; this.m_position = position; Quaternion rotation = base.transform.rotation; Vector3 deltaRotation = this.DeltaRotation(rotation, this.m_rotation) * 0.0174532924f; this.m_rotation = rotation; this.AddSample(deltaPosition, deltaRotation); this.m_frameLinearVelocity = this.m_samples[this.m_index].LinearVelocity; this.m_frameAngularVelocity = this.m_samples[this.m_index].AngularVelocity; this.m_trackedLinearVelocity = this.ComputeAverageLinearVelocity().normalized * this.ComputeMaxLinearSpeed(); this.m_trackedAngularVelocity = this.ComputeAverageAngularVelocity(); } private void OnDrawGizmos() { if (!this.m_showGizmos) { return; } Gizmos.color = Color.red; Gizmos.DrawRay(base.transform.position, this.TrackedLinearVelocity); } private Vector3 DeltaRotation(Quaternion final, Quaternion initial) { Vector3 eulerAngles = final.eulerAngles; Vector3 eulerAngles2 = initial.eulerAngles; Vector3 result = new Vector3(Mathf.DeltaAngle(eulerAngles2.x, eulerAngles.x), Mathf.DeltaAngle(eulerAngles2.y, eulerAngles.y), Mathf.DeltaAngle(eulerAngles2.z, eulerAngles.z)); return result; } private void AddSample(Vector3 deltaPosition, Vector3 deltaRotation) { this.m_index = (this.m_index + 1) % this.m_samples.Length; this.m_count = Mathf.Min(this.m_count + 1, this.m_samples.Length); float time = Time.time; Vector3 linearVelocity = deltaPosition / Time.deltaTime; Vector3 angularVelocity = deltaRotation / Time.deltaTime; this.m_samples[this.m_index] = new VelocityTracker.Sample { Time = time, LinearVelocity = linearVelocity, AngularVelocity = angularVelocity }; this.m_samples[this.m_index].SquaredLinearSpeed = this.ComputeAverageLinearVelocity().sqrMagnitude; } private int Count() { return Mathf.Min(this.m_count, this.m_samples.Length); } private int IndexPrev(int index) { return (index != 0) ? (index - 1) : (this.m_count - 1); } private bool IsSampleValid(int index, float windowSize) { float num = Time.time - this.m_samples[index].Time; return windowSize - num >= 0.0001f || index == this.m_index; } private Vector3 ComputeAverageAngularVelocity() { int num = this.m_index; int num2 = this.Count(); int num3 = 0; Vector3 vector = Vector3.zero; for (int i = 0; i < num2; i++) { if (!this.IsSampleValid(num, 0.0222222228f)) { break; } num3++; vector += this.m_samples[num].AngularVelocity; num = this.IndexPrev(num); } if (num3 > 1) { vector /= (float)num3; } return vector; } private Vector3 ComputeAverageLinearVelocity() { int num = this.m_index; int num2 = this.Count(); int num3 = 0; Vector3 vector = Vector3.zero; for (int i = 0; i < num2; i++) { if (!this.IsSampleValid(num, 0.0444444455f)) { break; } num3++; vector += this.m_samples[num].LinearVelocity; num = this.IndexPrev(num); } if (num3 > 1) { vector /= (float)num3; } return vector; } private float ComputeMaxLinearSpeed() { int num = this.m_index; int num2 = this.Count(); float num3 = 0f; for (int i = 0; i < num2; i++) { if (!this.IsSampleValid(num, 0.08888889f)) { break; } num3 = Mathf.Max(num3, this.m_samples[num].SquaredLinearSpeed); num = this.IndexPrev(num); } return (num3 <= Mathf.Epsilon) ? 0f : Mathf.Sqrt(num3); } public const float WINDOW_TIME = 0.0111111114f; public const float WINDOW_EPSILON = 0.0001f; public const float LINEAR_SPEED_WINDOW = 0.08888889f; public const float LINEAR_VELOCITY_WINDOW = 0.0444444455f; public const float ANGULAR_VELOCITY_WINDOW = 0.0222222228f; public const int MAX_SAMPLES = 45; [SerializeField] public bool m_showGizmos = true; private int m_index = -1; private int m_count; private VelocityTracker.Sample[] m_samples = new VelocityTracker.Sample[45]; private Vector3 m_position = Vector3.zero; private Quaternion m_rotation = Quaternion.identity; private Vector3 m_frameLinearVelocity = Vector3.zero; private Vector3 m_frameAngularVelocity = Vector3.zero; private Vector3 m_trackedLinearVelocity = Vector3.zero; private Vector3 m_trackedAngularVelocity = Vector3.zero; private struct Sample { public float Time; public float SquaredLinearSpeed; public Vector3 LinearVelocity; public Vector3 AngularVelocity; } }