注释掉算法

Assets/BowArrow/Scripts/Bluetooth/o09Axis.cs

@@ -730,53 +730,72 @@ public class o09Axis
         return platformID == 1;
     }
 
-    o09AxisCSBridge axisCSBridge = new o09AxisCSBridge();
+    // o09AxisCSBridge axisCSBridge = new o09AxisCSBridge();
 
     public Quaternion update(Vector3 AccOld, Vector3 GyrOld, Vector3 MagOld, long TimeGapOld) {
-        if (axisCSBridge != null) return axisCSBridge.Update_f(AccOld, GyrOld, MagOld, TimeGapOld);
-        else if (IsWindows()) return Update_f(AccOld, GyrOld, MagOld, TimeGapOld);
+        // if (axisCSBridge != null) {
+        //     return axisCSBridge.Update_f(AccOld, GyrOld, MagOld, TimeGapOld);
+        // }
+        if (IsWindows()) return Update_f(AccOld, GyrOld, MagOld, TimeGapOld);
         else return SO_Update_f(AccOld, GyrOld, MagOld, TimeGapOld);
     }
 
     private void setIdentity() {
-        if (axisCSBridge != null) axisCSBridge.SetIdentity();
-        else if (IsWindows()) SetIdentity();
+        // if (axisCSBridge != null) {
+        //     axisCSBridge.SetIdentity(); 
+        //     return;
+        // }
+        if (IsWindows()) SetIdentity();
         else SO_SetIdentity();
     }
 
     public Vector3 getGyrOld() {
-        if (axisCSBridge != null) return axisCSBridge.GetGyrOld_f();
-        else if (IsWindows()) return GetGyrOld_f();
+        // if (axisCSBridge != null) {
+        //     return axisCSBridge.GetGyrOld_f();
+        // }
+        if (IsWindows()) return GetGyrOld_f();
         else return SO_GetGyrOld_f();
     }
 
     public State getLastState() {
-        if (axisCSBridge != null) return axisCSBridge.GetLastState_f();
-        else if (IsWindows()) return GetLastState_f();
+        // if (axisCSBridge != null) {
+        //     return axisCSBridge.GetLastState_f();
+        // }
+        if (IsWindows()) return GetLastState_f();
         else return SO_GetLastState_f();
     }
 
     private Vector3 getAccIdentity() {
-        if (axisCSBridge != null) return axisCSBridge.GetAccIdentity_f();
-        else if (IsWindows()) return GetAccIdentity_f();
+        // if (axisCSBridge != null) {
+        //     return axisCSBridge.GetAccIdentity_f();
+        // }
+        if (IsWindows()) return GetAccIdentity_f();
         else return SO_GetAccIdentity_f();
     }
 
     private Vector3 getMagIdentity() {
-        if (axisCSBridge != null) return axisCSBridge.GetMagIdentity_f();
-        else if (IsWindows()) return GetMagIdentity_f();
+        // if (axisCSBridge != null) {
+        //     return axisCSBridge.GetMagIdentity_f();
+        // }
+        if (IsWindows()) return GetMagIdentity_f();
         else return SO_GetMagIdentity_f();
     }
 
     private void setAccIdentity(Vector3 value) {
-        if (axisCSBridge != null) axisCSBridge.SetAccIdentity(new Vector3D(value));
-        else if (IsWindows()) SetAccIdentity(new Vector3D(value));
+        // if (axisCSBridge != null) {
+        //     axisCSBridge.SetAccIdentity(new Vector3D(value)); 
+        //     return;
+        // }
+        if (IsWindows()) SetAccIdentity(new Vector3D(value));
         else SO_SetAccIdentity(new Vector3D(value));
     }
 
     private  void setMagIdentity(Vector3 value) {
-        if (axisCSBridge != null) axisCSBridge.SetMagIdentity(new Vector3D(value));
-        else if (IsWindows()) SetMagIdentity(new Vector3D(value));
+        // if (axisCSBridge != null) {
+        //     axisCSBridge.SetMagIdentity(new Vector3D(value)); 
+        //     return;
+        // }
+        if (IsWindows()) SetMagIdentity(new Vector3D(value));
         else SO_SetMagIdentity(new Vector3D(value));
     }
 

Assets/BowArrow/Scripts/Bluetooth/o09AxisCS.cs

@@ -5,143 +5,143 @@ using UnityEngine;
 
 public class o09AxisCS
 {
-    static public Vector3 AccIdentity = new Vector3(0, -1, 0);
-    static public Vector3 MagIdentity = new Vector3(-1, 2, 0).normalized;
-    public class State
-    {
-        public long TimeGap;
-        public Vector3 Acc = AccIdentity;
-        public Vector3 AccSmooth = AccIdentity;
-        public double AccVariance = 1;
-
-        public Vector3 Gyr;
-        public Vector3 Mag = MagIdentity;
-        public Vector3 MagSmooth = MagIdentity;
-        public Quaternion Qua = Quaternion.identity;
-        public Quaternion QuaSmooth = Quaternion.identity;
-        public double Variance = 1;
-    }
-
-    o0Project.Variance HardwareVarianceGyr = new o0Project.Variance(1000);
-    o0Project.Variance HardwareVarianceAcc = new o0Project.Variance(1000);
-    o0Project.Variance HardwareVarianceMag = new o0Project.Variance(1000);
-
-    public List<State> States = new List<State>();
-
-    public Vector3 AccOld;
-    public Vector3 GyrOld;
-    public Vector3 MagOld;
-
-    public float x;
-    public float y;
-    public float z;
-
-    long TimeGapOld;
-    o0Aien.o0WeightedAverageFilterVector3 AccFilter = new o0Aien.o0WeightedAverageFilterVector3(5);
-    o0Aien.o0WeightedAverageFilterVector3 MagFilter = new o0Aien.o0WeightedAverageFilterVector3(10);
+    // static public Vector3 AccIdentity = new Vector3(0, -1, 0);
+    // static public Vector3 MagIdentity = new Vector3(-1, 2, 0).normalized;
+    // public class State
+    // {
+    //     public long TimeGap;
+    //     public Vector3 Acc = AccIdentity;
+    //     public Vector3 AccSmooth = AccIdentity;
+    //     public double AccVariance = 1;
+
+    //     public Vector3 Gyr;
+    //     public Vector3 Mag = MagIdentity;
+    //     public Vector3 MagSmooth = MagIdentity;
+    //     public Quaternion Qua = Quaternion.identity;
+    //     public Quaternion QuaSmooth = Quaternion.identity;
+    //     public double Variance = 1;
+    // }
+
+    // o0Project.Variance HardwareVarianceGyr = new o0Project.Variance(1000);
+    // o0Project.Variance HardwareVarianceAcc = new o0Project.Variance(1000);
+    // o0Project.Variance HardwareVarianceMag = new o0Project.Variance(1000);
+
+    // public List<State> States = new List<State>();
+
+    // public Vector3 AccOld;
+    // public Vector3 GyrOld;
+    // public Vector3 MagOld;
+
+    // public float x;
+    // public float y;
+    // public float z;
+
+    // long TimeGapOld;
+    // o0Aien.o0WeightedAverageFilterVector3 AccFilter = new o0Aien.o0WeightedAverageFilterVector3(5);
+    // o0Aien.o0WeightedAverageFilterVector3 MagFilter = new o0Aien.o0WeightedAverageFilterVector3(10);
          
-    public Quaternion update(Vector3 AccOld, Vector3 GyrOld, Vector3 MagOld, long TimeGapOld)
-    {
-        o0UIRawImageTester.UpdateAllOffset();
-
-        var Acc = this.AccOld;
-        var Gyr = (this.GyrOld + GyrOld)/2;
-        var Mag = this.MagOld;
-        float TimeGap = this.TimeGapOld;
-        this.AccOld = AccOld;
-        this.GyrOld = GyrOld;
-        this.MagOld = MagOld;
-        this.TimeGapOld = TimeGapOld;
-        var Last = States.LastOrDefault() ?? new State();
-        if (this.TimeGapOld <= 0)
-            return Last.Qua;
-        States.Add(new State());
-        if (States.Count > 200)
-            States.RemoveAt(0);
-        var state = States.Last();
-        state.Acc = Acc;
-
-        state.AccSmooth = AccFilter.Update(Acc);
+    // public Quaternion update(Vector3 AccOld, Vector3 GyrOld, Vector3 MagOld, long TimeGapOld)
+    // {
+    //     o0UIRawImageTester.UpdateAllOffset();
+
+    //     var Acc = this.AccOld;
+    //     var Gyr = (this.GyrOld + GyrOld)/2;
+    //     var Mag = this.MagOld;
+    //     float TimeGap = this.TimeGapOld;
+    //     this.AccOld = AccOld;
+    //     this.GyrOld = GyrOld;
+    //     this.MagOld = MagOld;
+    //     this.TimeGapOld = TimeGapOld;
+    //     var Last = States.LastOrDefault() ?? new State();
+    //     if (this.TimeGapOld <= 0)
+    //         return Last.Qua;
+    //     States.Add(new State());
+    //     if (States.Count > 200)
+    //         States.RemoveAt(0);
+    //     var state = States.Last();
+    //     state.Acc = Acc;
+
+    //     state.AccSmooth = AccFilter.Update(Acc);
         
-        state.Gyr = Gyr;
-        state.Mag = Mag;/**/
-        state.MagSmooth = MagFilter.Update(Mag);
-        if (States.Count <=1)
-            return Quaternion.identity;
+    //     state.Gyr = Gyr;
+    //     state.Mag = Mag;/**/
+    //     state.MagSmooth = MagFilter.Update(Mag);
+    //     if (States.Count <=1)
+    //         return Quaternion.identity;
 
-        HardwareVarianceGyr.Update((Gyr).magnitude);//每毫秒方差2.331017E-09 度左右   0.00000002331017
-        HardwareVarianceAcc.Update(Vector3.Angle(state.Acc, Last.Acc));//方差0.0012度左右
-        HardwareVarianceMag.Update(Vector3.Angle(state.Mag, Last.Mag));//方差3.5度左右
+    //     HardwareVarianceGyr.Update((Gyr).magnitude);//每毫秒方差2.331017E-09 度左右   0.00000002331017
+    //     HardwareVarianceAcc.Update(Vector3.Angle(state.Acc, Last.Acc));//方差0.0012度左右
+    //     HardwareVarianceMag.Update(Vector3.Angle(state.Mag, Last.Mag));//方差3.5度左右
 
-        var LastQuaternion = Last.Qua;
+    //     var LastQuaternion = Last.Qua;
 
-        var newQua = new Quaternion();
-        newQua.eulerAngles = Gyr * TimeGap;
-        var quaGyr = LastQuaternion * newQua;
+    //     var newQua = new Quaternion();
+    //     newQua.eulerAngles = Gyr * TimeGap;
+    //     var quaGyr = LastQuaternion * newQua;
 
-        double AccLengthToAngle = 5;//1倍引力差相当于多少度方差
-        double MagLengthToAngle = 5;//1倍磁力差相当于多少度方差
+    //     double AccLengthToAngle = 5;//1倍引力差相当于多少度方差
+    //     double MagLengthToAngle = 5;//1倍磁力差相当于多少度方差
 
-        double GyrVariance = Last.Variance + Math.Pow((Gyr * TimeGap).magnitude * 0.3,3);// 指数4 = 方差2 * 欧拉角旋转误差2     移动导致累计误差
-        double AccVariance = Math.Max((Gyr * TimeGap).magnitude, Vector3.Angle(state.AccSmooth, Last.AccSmooth)) * 1 + Math.Pow(Math.Abs(state.AccSmooth.magnitude - 9.8) / 9.8 * AccLengthToAngle,4);
-        double MagVariance = Math.Max((Gyr * TimeGap).magnitude, Vector3.Angle(state.MagSmooth, Last.MagSmooth)) * 1 + Math.Pow(Math.Abs(state.MagSmooth.magnitude - 1) / 1 * MagLengthToAngle,4);/**/
+    //     double GyrVariance = Last.Variance + Math.Pow((Gyr * TimeGap).magnitude * 0.3,3);// 指数4 = 方差2 * 欧拉角旋转误差2     移动导致累计误差
+    //     double AccVariance = Math.Max((Gyr * TimeGap).magnitude, Vector3.Angle(state.AccSmooth, Last.AccSmooth)) * 1 + Math.Pow(Math.Abs(state.AccSmooth.magnitude - 9.8) / 9.8 * AccLengthToAngle,4);
+    //     double MagVariance = Math.Max((Gyr * TimeGap).magnitude, Vector3.Angle(state.MagSmooth, Last.MagSmooth)) * 1 + Math.Pow(Math.Abs(state.MagSmooth.magnitude - 1) / 1 * MagLengthToAngle,4);/**/
 
-        state.Variance = GyrVariance;
-        state.Variance = state.Variance * (AccVariance+ MagVariance) / (state.Variance + (AccVariance + MagVariance));
+    //     state.Variance = GyrVariance;
+    //     state.Variance = state.Variance * (AccVariance+ MagVariance) / (state.Variance + (AccVariance + MagVariance));
 
-        if (double.IsNaN(GyrVariance))
-            GyrVariance = double.MinValue;
-        if (double.IsNaN(AccVariance))
-            AccVariance = double.MinValue;
-        if (double.IsNaN(MagVariance))
-            MagVariance = double.MinValue;
-        if (double.IsNaN(state.Variance))
-            state.Variance = double.MinValue;
+    //     if (double.IsNaN(GyrVariance))
+    //         GyrVariance = double.MinValue;
+    //     if (double.IsNaN(AccVariance))
+    //         AccVariance = double.MinValue;
+    //     if (double.IsNaN(MagVariance))
+    //         MagVariance = double.MinValue;
+    //     if (double.IsNaN(state.Variance))
+    //         state.Variance = double.MinValue;
 
-        var quaAccMag = o0Project.o0.FormQuaternion(AccIdentity, MagIdentity, state.AccSmooth, state.MagSmooth, (float)(AccVariance / (AccVariance + MagVariance)));
+    //     var quaAccMag = o0Project.o0.FormQuaternion(AccIdentity, MagIdentity, state.AccSmooth, state.MagSmooth, (float)(AccVariance / (AccVariance + MagVariance)));
 
-        var quaMinRate = GyrVariance / (GyrVariance + Math.Max(AccVariance, MagVariance));
-        var quaMaxRate = GyrVariance / (GyrVariance + Math.Min(AccVariance, MagVariance));
-        Quaternion quaFirst = Quaternion.Slerp(quaGyr, quaAccMag, (float)quaMinRate).normalized;
-        if (float.IsNaN(quaFirst.w))
-            quaFirst = Last.Qua;
+    //     var quaMinRate = GyrVariance / (GyrVariance + Math.Max(AccVariance, MagVariance));
+    //     var quaMaxRate = GyrVariance / (GyrVariance + Math.Min(AccVariance, MagVariance));
+    //     Quaternion quaFirst = Quaternion.Slerp(quaGyr, quaAccMag, (float)quaMinRate).normalized;
+    //     if (float.IsNaN(quaFirst.w))
+    //         quaFirst = Last.Qua;
 
-        var quaSecondRate = (quaMaxRate - quaMinRate) / (1 - quaMinRate);
+    //     var quaSecondRate = (quaMaxRate - quaMinRate) / (1 - quaMinRate);
 
-        state.Qua = AccVariance < MagVariance ? o0Project.o0.FormQuaternion(quaFirst, AccIdentity, state.AccSmooth, (float)quaSecondRate) : o0Project.o0.FormQuaternion(quaFirst, MagIdentity, state.MagSmooth, (float)quaSecondRate);
+    //     state.Qua = AccVariance < MagVariance ? o0Project.o0.FormQuaternion(quaFirst, AccIdentity, state.AccSmooth, (float)quaSecondRate) : o0Project.o0.FormQuaternion(quaFirst, MagIdentity, state.MagSmooth, (float)quaSecondRate);
         
-        if (float.IsNaN(state.Qua.w))
-            state.Qua = Last.Qua;/**/
-        state.QuaSmooth = Quaternion.Slerp(Last.QuaSmooth, state.Qua, 0.3f);
-
-        var frontV = Last.Qua * Vector3.forward;
-        var upV = Last.Qua * Vector3.up;
-
-        x = (Mathf.Atan(upV.y / upV.z) / Mathf.PI * 180 + (upV.z < 0 ? 90:270));
-        y = (Mathf.Atan(frontV.z / frontV.x) / Mathf.PI * 180+(frontV.x < 0 ? 90:270));
-        z = (Mathf.Atan(upV.y / upV.x) / Mathf.PI * 180 + (upV.x < 0 ? 90:270));
-
-        return state.Qua;
-    }
-
-    public void SetIdentity()
-    {
-        Quaternion qua = default;
-        AccIdentity = AccOld;
-        MagIdentity = MagOld;
-        qua = o0Project.o0.FormQuaternion(Quaternion.identity, Vector3.down,AccIdentity, 1);
-        AccIdentity=qua*AccIdentity;
-        MagIdentity = qua*MagIdentity;
-        States.Last().Qua = Quaternion.identity;
-        States.Last().Qua = qua*States.Last().Qua;//Quaternion.identity;
-        States.Last().Variance = 0.0000001;
-    }
-
-    public State getLastState() {
-        return this.States.Last();
-    }
-
-    public Vector3 getGyrOld() {
-        return GyrOld;
-    }
+    //     if (float.IsNaN(state.Qua.w))
+    //         state.Qua = Last.Qua;/**/
+    //     state.QuaSmooth = Quaternion.Slerp(Last.QuaSmooth, state.Qua, 0.3f);
+
+    //     var frontV = Last.Qua * Vector3.forward;
+    //     var upV = Last.Qua * Vector3.up;
+
+    //     x = (Mathf.Atan(upV.y / upV.z) / Mathf.PI * 180 + (upV.z < 0 ? 90:270));
+    //     y = (Mathf.Atan(frontV.z / frontV.x) / Mathf.PI * 180+(frontV.x < 0 ? 90:270));
+    //     z = (Mathf.Atan(upV.y / upV.x) / Mathf.PI * 180 + (upV.x < 0 ? 90:270));
+
+    //     return state.Qua;
+    // }
+
+    // public void SetIdentity()
+    // {
+    //     Quaternion qua = default;
+    //     AccIdentity = AccOld;
+    //     MagIdentity = MagOld;
+    //     qua = o0Project.o0.FormQuaternion(Quaternion.identity, Vector3.down,AccIdentity, 1);
+    //     AccIdentity=qua*AccIdentity;
+    //     MagIdentity = qua*MagIdentity;
+    //     States.Last().Qua = Quaternion.identity;
+    //     States.Last().Qua = qua*States.Last().Qua;//Quaternion.identity;
+    //     States.Last().Variance = 0.0000001;
+    // }
+
+    // public State getLastState() {
+    //     return this.States.Last();
+    // }
+
+    // public Vector3 getGyrOld() {
+    //     return GyrOld;
+    // }
 }

Assets/BowArrow/Scripts/Bluetooth/o09AxisCSBridge.cs

@@ -1,45 +1,45 @@
 using UnityEngine;
 
 public class o09AxisCSBridge {
-    o09AxisCS o09AxisCS = new o09AxisCS();
-    public Quaternion Update_f(Vector3 AccOld, Vector3 GyrOld, Vector3 MagOld, long TimeGapOld) {
-        return o09AxisCS.update(AccOld, GyrOld, MagOld, TimeGapOld);
-    }
-    public void SetIdentity() {
-        o09AxisCS.SetIdentity();
-    }
-    public Vector3 GetGyrOld_f() {
-        return o09AxisCS.getGyrOld();
-    }
-    public o09Axis.State GetLastState_f() {
-        o09AxisCS.State s = o09AxisCS.getLastState();
-        o09Axis.State state = new o09Axis.State();
-        state.TimeGap = System.Convert.ToInt32(s.TimeGap);
-        state.Acc = s.Acc;
-        state.AccSmooth = s.AccSmooth;
-        state.AccVariance = (float) s.AccVariance;
-        state.Gyr = s.Gyr;
-        state.Mag = s.Mag;
-        state.MagSmooth = s.MagSmooth;
-        state.Qua = s.Qua;
-        state.QuaSmooth = s.QuaSmooth;
-        state.Variance = (float) s.Variance;
-        return state;
-    }
-    public Vector3 GetAccIdentity_f() {
-        return o09AxisCS.AccIdentity;
-    }
-    public Vector3 GetMagIdentity_f() {
-        return o09AxisCS.MagIdentity;
-    }
-    public void SetAccIdentity(o09Axis.Vector3D value) {
-        o09AxisCS.AccIdentity.x = (float) value.x;
-        o09AxisCS.AccIdentity.y = (float) value.y;
-        o09AxisCS.AccIdentity.z = (float) value.z;
-    }
-    public void SetMagIdentity(o09Axis.Vector3D value) {
-        o09AxisCS.MagIdentity.x = (float) value.x;
-        o09AxisCS.MagIdentity.y = (float) value.y;
-        o09AxisCS.MagIdentity.z = (float) value.z;
-    }
+    // o09AxisCS o09AxisCS = new o09AxisCS();
+    // public Quaternion Update_f(Vector3 AccOld, Vector3 GyrOld, Vector3 MagOld, long TimeGapOld) {
+    //     return o09AxisCS.update(AccOld, GyrOld, MagOld, TimeGapOld);
+    // }
+    // public void SetIdentity() {
+    //     o09AxisCS.SetIdentity();
+    // }
+    // public Vector3 GetGyrOld_f() {
+    //     return o09AxisCS.getGyrOld();
+    // }
+    // public o09Axis.State GetLastState_f() {
+    //     o09AxisCS.State s = o09AxisCS.getLastState();
+    //     o09Axis.State state = new o09Axis.State();
+    //     state.TimeGap = System.Convert.ToInt32(s.TimeGap);
+    //     state.Acc = s.Acc;
+    //     state.AccSmooth = s.AccSmooth;
+    //     state.AccVariance = (float) s.AccVariance;
+    //     state.Gyr = s.Gyr;
+    //     state.Mag = s.Mag;
+    //     state.MagSmooth = s.MagSmooth;
+    //     state.Qua = s.Qua;
+    //     state.QuaSmooth = s.QuaSmooth;
+    //     state.Variance = (float) s.Variance;
+    //     return state;
+    // }
+    // public Vector3 GetAccIdentity_f() {
+    //     return o09AxisCS.AccIdentity;
+    // }
+    // public Vector3 GetMagIdentity_f() {
+    //     return o09AxisCS.MagIdentity;
+    // }
+    // public void SetAccIdentity(o09Axis.Vector3D value) {
+    //     o09AxisCS.AccIdentity.x = (float) value.x;
+    //     o09AxisCS.AccIdentity.y = (float) value.y;
+    //     o09AxisCS.AccIdentity.z = (float) value.z;
+    // }
+    // public void SetMagIdentity(o09Axis.Vector3D value) {
+    //     o09AxisCS.MagIdentity.x = (float) value.x;
+    //     o09AxisCS.MagIdentity.y = (float) value.y;
+    //     o09AxisCS.MagIdentity.z = (float) value.z;
+    // }
 }