feat: hw2

Assets/Cainos/Common/Script/Utils/SecondOrderDynamics.cs

@@ -0,0 +1,133 @@
+//https://www.youtube.com/watch?v=KPoeNZZ6H4s
+
+using UnityEngine;
+
+namespace Cainos.Common
+{
+    [System.Serializable]
+    public struct SecondOrderDynamics
+    {
+        private Vector3 xp;
+        private Vector3 y, yd;
+        private float k1, k2, k3;
+        private Vector3 xd;
+
+        private float k2_stable;
+
+        [SerializeField] private float f;
+        [SerializeField] private float d;
+        [SerializeField] private float r;
+
+        public float Frequency
+        {
+            get { return f; }
+            set
+            {
+                if (value <= 0.01f) value = 0.01f;
+
+                f = value;
+                UpdateInnerParams();
+            }
+        }
+
+
+        public float Damping
+        {
+            get { return d; }
+            set
+            {
+                if (value <= 0.01f) value = 0.01f;
+
+                d = value;
+                UpdateInnerParams();
+            }
+        }
+
+        public float Response
+        {
+            get { return r; }
+            set
+            {
+                r = value;
+                UpdateInnerParams();
+            }
+        }
+
+        public SecondOrderDynamics(float frequency, float damping, float response)
+        {
+            this.f = 1.0f;
+            this.d = 0.0f;
+            this.r = 0.0f;
+            xp = Vector3.zero;
+            y = Vector3.zero;
+            xd = Vector3.zero;
+            yd = Vector3.zero;
+            k1 = 0.0f;
+            k2 = 0.0f;
+            k3 = 0.0f;
+            k2_stable = 0.0f;
+
+            Reset(frequency, damping, response, Vector3.zero);
+        }
+
+        public void Reset(float frequency, float damping, float response, Vector3 x0)
+        {
+            f = frequency;
+            d = damping;
+            r = response;
+
+            xp = x0;
+            y = x0;
+            xd = Vector3.zero;
+            yd = Vector3.zero;
+
+            UpdateInnerParams();
+        }
+
+        public void Reset(Vector3 x0)
+        {
+            Reset(f, d, r, x0);
+        }
+
+        public void Reset(Vector2 x0)
+        {
+            Reset(f, d, r, x0);
+        }
+
+        public void Reset(float x0)
+        {
+            Reset(f, d, r, Vector3.one * x0);
+        }
+
+        public Vector3 Update(Vector3 x, float t)
+        {
+            if (t < Mathf.Epsilon) return y;
+
+            xd = (x - xp) / t;
+            xp = x;
+
+            k2_stable = Mathf.Max(k2, 1.1f * (t * t * 0.25f + t * k1 * 0.5f));
+            y += t * yd;
+            yd += t * (x + k3 * xd - y - k1 * yd) / k2_stable;
+
+            return y;
+        }
+
+        public Vector2 Update(Vector2 x, float t)
+        {
+            return Update(new Vector3(x.x,x.y,0.0f), t);
+        }
+
+        public float Update(float x, float t)
+        {
+            return Update(Vector3.one * x, t).x;
+        }
+
+        private void UpdateInnerParams()
+        {
+            k1 = d / (Mathf.PI * f);
+            k2 = 1.0f / ((2.0f * Mathf.PI * f) * (2.0f * Mathf.PI * f));
+            k3 = r * d / (2.0f * Mathf.PI * f);
+        }
+    }
+}