smart-bow-infrared/Assets/BowArrow/Scripts/Bluetooth/o0DistanceToAxis.cs

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using o0.Geometry;
using o0.Num;
using UnityEngine;

namespace o0.Bow
{
    public class DistanceToAxis//测试版
    {
        public DistanceToAxis()
        {
            DistanceQueue = new SortedQueue<double>();
            Distance = 0.6;
        }
        Vector<double> LastAcc;
        Vector<double> LockAcc;//静止状态下加速计向量，仅通过陀螺仪短时校准，长时校准不保证准确性，也无必要性
        double LockMS;
        double LastGapMS;
        Vector<double> Speed;//每秒
        SortedQueue<double> DistanceQueue;
        double Distance;
        /////////////////////////////////////////////////Acc长度以1为标准
        public void Update(Vector<double> Gyr, Vector<double> Acc, Vector<double> Mag, double GapMS)//务必仅在射箭瞄准游戏过程中调用
        {
            if(LastAcc == default)
            {
                LastAcc = Acc;
                return;
            }

            if (Gyr.Length * 1000 < 1 // 陀螺仪计算的每秒移动小于1度
                && (LastAcc - Acc).Length / GapMS * 1000 < 1// 加速计计算的每秒移动小于1度
                && Acc.Length < 1.05
                && Acc.Length > 0.95
                && LastAcc.Length < 1.05
                && LastAcc.Length > 0.95)//则当做静止状态，此时记录加速计长度
            {
                LockAcc = Acc;
                LockMS = 0;
                LastGapMS = GapMS;
                Speed = Vector<double>.Zero;


                //Debug.Log(Gyr.Length * 1000 + " | " + Acc.Length + " | " + (LastAcc - Acc).Length / GapMS * 1000);

                LastAcc = Acc;
                return;
            }
            if(LockAcc != default)
            {
                var GyrOperator = -Geometry.Quaternion.Euler((Gyr * GapMS).To<float>());
                LockAcc = (GyrOperator * LockAcc.To<float>()).To<double>();
                Speed = (GyrOperator * Speed.To<float>()).To<double>();
                Speed += (Acc - LockAcc) * 9.8 * (LastGapMS + GapMS) / 1000 / 2;

                if (LockMS < 250 && Gyr.Length * 1000 > 5)
                {
                    var distance = Speed.Length / (Gyr.Length * 1000) * 180 / Math.PI;//臂长
                    if(distance < 1)
                    {
                        if (DistanceQueue.Count > 100)
                            DistanceQueue.Dequeue();
                        DistanceQueue.Enqueue(distance);
                        if(DistanceQueue.Count > 30)
                        {
                            var mid = DistanceQueue.Count >> 1;
                            var max = mid + 10;
                            var list = new List<double>();
                            for (var i = mid - 9; i < max; ++i)
                                list.Add(DistanceQueue[i]);
                            Distance = list.Mean();
                        }
                    }
                    //Debug.Log(Distance);
                }

                // TestVector.SetAcc((Acc - LockAcc).ToUnityVector(), 1);
                // TestVector.SetMag(Speed.ToUnityVector(), 1);
                //Debug.Log(Gyr.Length);

                LastGapMS = GapMS;
                LockMS += GapMS;
            }
            LastAcc = Acc;
            return;
        }


        //默认轴在后方 z为负
        //////// 米/秒                      度/毫秒
        public Vector<double> GyrToSpeed(Vector<double> Gyr, double GapMS)
        {
            var GyrOperator = Geometry.Quaternion.Euler((Gyr * GapMS).To<float>()).Inversed;
            var axis = Vector<double>.Back * Distance;
            axis = (GyrOperator * axis.To<float>() - axis.To<float>()).To<double>() / GapMS * 1000;
            // TestVector.SetAcc(axis.ToUnityVector(), 2);
            //Debug.Log(axis);
            return axis;
        }/**/
        Vector<double> LastSpeed = default;
        public Vector<double> AccCorrection(Vector<double> Gyr, Vector<double> Acc, double GapMS)
        {
            Vector<double> Speed = GyrToSpeed(Gyr, GapMS);
            if (LastSpeed == default)
            {
                LastSpeed = Speed;
                //Debug.Log(Acc.z);
                //var Speed = 
                return Acc;
            }
            var GyrOperator = Geometry.Quaternion.Euler((Gyr * GapMS).To<float>()).Inversed;
            var RotatedLastSpeed = GyrOperator * LastSpeed.To<float>();
            var AccCorrection = (Speed - RotatedLastSpeed.To<double>()) / 9.8 / GapMS * 1000;
            // TestVector.SetMag(AccCorrection.ToUnityVector(), 2);
            // TestVector.SetAcc((Acc + AccCorrection).ToUnityVector(), 3);
            LastSpeed = Speed;
            return Acc + AccCorrection;//因为Acc是反的
        }
    }
}