回溯找稳定的点

Assets/BowArrow/Scripts/Bluetooth/BluetoothAim.cs

@@ -465,6 +465,17 @@ class AimHandler
         msOld = ms;
 
         AMesh.transform.localRotation = newRotation = _9Axis.Update(Acc * 10, Gyr, Mag, TimeGap);
+        Quaternion nextRotation = controlObj.localRotation;
+        filter.Update(ref nextRotation, newRotation);
+        newRotation = nextRotation;
+        if (ArmBow.ins != null) {
+            for (int i = ArmBow.ins.recordRotations.Length - 1; i > 0 ; i--)
+            {
+                ArmBow.ins.recordRotations[i] = ArmBow.ins.recordRotations[i - 1];
+            }
+            ArmBow.ins.recordRotations[0] = newRotation;
+            ArmBow.ins.recordCount++;
+        }
 
         receiveDataCount++;
         if (!hasAutoIdentity && receiveDataCount == 5) {
@@ -485,9 +496,9 @@ class AimHandler
     {
         if (hasAutoIdentity && controlObj != null)
         {
-            Quaternion nowRotation = controlObj.localRotation;
-            filter.Update(ref nowRotation, newRotation);
-            controlObj.localRotation = Quaternion.Lerp(controlObj.localRotation, nowRotation, Time.deltaTime * 6);
+            // 最终结果进行lerp
+            // controlObj.localRotation = Quaternion.Lerp(controlObj.localRotation, newRotation, Time.deltaTime * 6);
+            controlObj.localRotation = newRotation;
             // GameObject.Find("Canvas/RPY_LOG").GetComponent<Text>().text = 
             //     "roll: " + controlObj.localEulerAngles.x +
             //     "\npitch: " + controlObj.localEulerAngles.y +

Assets/BowArrow/Scripts/Game/ArmBow.cs

@@ -14,6 +14,9 @@ public class ArmBow : MonoBehaviour
     private bool readying = false;
 
     public static ArmBow ins;
+    public Quaternion[] recordRotations = new Quaternion[100];
+    public float[] recordRotationVars = new float[99];
+    public int recordCount = 0;
 
     void Start()
     {
@@ -113,7 +116,30 @@ public class ArmBow : MonoBehaviour
     public void shoot() {
         // Vector3 rayHitPoint = CrossHair.ins.getRayHitPoint();
 
-        GameObject arrowCopy = GameObject.Instantiate(this.arrow, this.bowCamera.transform.position, this.bowCamera.transform.rotation);
+        Quaternion best_rotation = this.bowCamera.transform.rotation;
+        if (recordCount >= recordRotations.Length) {
+            int single_check_count = 6;
+            float min_wave = float.MaxValue;
+            for (int i = 0; i < recordRotationVars.Length; i++)
+            {
+                recordRotationVars[i] = Quaternion.Angle(recordRotations[i], recordRotations[i + 1]);
+                if (i >= single_check_count - 1) 
+                {
+                    float wave = 0;
+                    for (int j = i; j > i - single_check_count; j--)
+                    {
+                        wave += recordRotationVars[j];
+                    }
+                    if (wave < min_wave)
+                    {
+                        min_wave = wave;
+                        best_rotation = recordRotations[i - single_check_count + 1];
+                    }
+                }
+            }
+        }
+
+        GameObject arrowCopy = GameObject.Instantiate(this.arrow, this.bowCamera.transform.position, best_rotation);
 
         Vector3 s1 = arrowCopy.transform.localScale;
         Vector3 s2 = bowCamera.transform.localScale;