yichael_smartbow/Assets/BowArrow/Scripts/Bluetooth/AimHandler.cs

using System;
using UnityEngine;
using System.Collections.Generic;
using System.Linq;
using UnityEngine.UI;
using Newtonsoft.Json;
using o0._9Axis;

/* 瞄准处理器 */
public class AimHandler : MonoBehaviour
{
    Transform controlObj {
        get {
            if (CameraToLook.ins) {
                return CameraToLook.ins.transform;
            }
            return null;
        }
    }
    [SerializeField] Button SetIdentityButton;
    [SerializeField] Button MagCalibrationButton;
    [SerializeField] Button GyrCalibrationButton;
    [SerializeField] Text MagScaleText;
    [SerializeField] Text GyrScaleText;

    //椭圆对象
    [SerializeField] Ellipse ellipseScript;
    [SerializeField] GameObject AccObj;
    [SerializeField] GameObject MagObj;

    [SerializeField] GameObject AccMesh;
    [SerializeField] GameObject GryMesh;
    [SerializeField] GameObject MagMesh;
    [SerializeField] GameObject AMesh;
    [SerializeField] Transform DebugTexts;
    [SerializeField] Transform DrawImage;
    
    long TimeGap = default;
    Vector3 Acc = default;
    Vector3 Gyr = default;
    Vector3 Mag = default;
    public o09Axis _9Axis = new o09Axis();

    Vector3 cMaxVector = new Vector3(0,0,0);
    Vector3 cMinVector = new Vector3(0, 0, 0);

    [o0.Serialize]
    public MagnetometerAutoCalibrater MagCalibrater;
    o0GyrCalibrater GyrCalibrater;

    //陀螺仪校准进度记录
    [NonSerialized] public int gyrCalibrateCompleteCount = 0;
    [NonSerialized] public int gyrCalibrateTotalCount = 2000;

    [NonSerialized] public long msOld = 0;

    public static AimHandler ins; 

    void Start()
    {
        ins = this;
        BluetoothDispatcher.aim = OnDataReceived;

        //初始化
        _9Axis.LoadIdentity();

        _9Axis.AccMesh = AccMesh;
        _9Axis.GryMesh = GryMesh;
        _9Axis.MagMesh = MagMesh;

        for (var i = 0; i < 9; ++i)
        {
            _9Axis.Tester.Add(DrawImage.Find(i.ToString()).gameObject.AddComponent<o0UIRawImageTester>());
        }

        for (var i = 0; i < 15; ++i)
        {
            _9Axis.TextTester.Add(DebugTexts.transform.Find("Text" + i.ToString()).gameObject.GetComponent<Text>());
        }

        if (SetIdentityButton) 
        {
            SetIdentityButton.onClick.AddListener(DoIdentity);
        }
        
        // try 
        // {  
        //     string magDataStr = PlayerPrefs.GetString("o0MagneticCalibrater");
        //     MagCalibrater = JsonConvert.DeserializeObject<o0MagneticCalibraterEllipsoidFitting>(magDataStr);
        // } 
        // catch(Exception) 
        // {
        //     MagCalibrater = null;
        // }

        if (MagCalibrater == null) 
        {
            // MagCalibrater = new o0MagneticCalibraterEllipsoidFitting();
            MagCalibrater = new MagnetometerAutoCalibrater();
        }
        if (MagCalibrationButton)
        {
            MagCalibrationButton.onClick.AddListener(delegate {
                // CalibrateMag(!MagCalibrater.Calibration);
            });
        }

        try {
            string gyrDataStr = PlayerPrefs.GetString("o0GyrCalibrater");
            GyrCalibrater = JsonConvert.DeserializeObject<o0GyrCalibrater>(gyrDataStr);
            if (GyrCalibrater._Average != Vector3.zero) GyrScaleText.text = "已校准";
        } catch(Exception) {
            GyrCalibrater = null;
        }
        if (GyrCalibrater == null) 
        {
            GyrCalibrater = new o0GyrCalibrater();
        }
        if (GyrCalibrationButton)
        {
            GyrCalibrationButton.onClick.AddListener(delegate() {
                CalibrateGyr(!GyrCalibrater.Calibration);
            });
        }
    }

    public void CalibrateGyr(bool calibration) {
        try {
            GyrCalibrater.Calibration = calibration;
            if (calibration)
            {
                GyrCalibrationButton.GetComponentInChildren<Text>().text = "停止陀螺仪校准";
            }
            else
            {
                GyrCalibrationButton.GetComponentInChildren<Text>().text = "开始陀螺仪校准";
                PlayerPrefs.SetString("o0GyrCalibrater", JsonConvert.SerializeObject(GyrCalibrater));            
            }
        } catch (Exception e) { Debug.LogError(e.Message); }
    }

    [NonSerialized] public bool isCalibrateMagPerfect = false;
     public void CalibrateMag(bool calibration) {
        // try {
        //     if (calibration)
        //     {
        //         PointCorrector.ins = new PointCorrector(this.ellipseScript, this.MagCalibrater);
        //         MagCalibrater.Calibration = calibration;
        //         MagCalibrationButton.GetComponentInChildren<Text>().text = "停止地磁计校准";
        //         this.cMaxVector = new Vector3(float.MinValue, float.MinValue, float.MinValue);
        //         this.cMinVector = new Vector3(float.MaxValue, float.MaxValue, float.MaxValue);
        //         this.ellipseScript.ellipseTran.gameObject.SetActive(false);
        //     }
        //     else
        //     {
        //         PointCorrector.ins = null;
        //         List<Vector3> list = MagCalibrater.getRecords();

        //         //停止校准时候，看看数组值
        //         float maxDistance = 0f,ratio = 1f;
        //         Vector3 maxVector3 = new Vector3(0,0,0);
        //         List<Vector3> endRecords = new List<Vector3>();
        //         foreach (Vector3 i in list)
        //         {
        //             Vector3 v = i - MagCalibrater._Center;
        //             if (Math.Abs(v.magnitude) > maxDistance)
        //             {
        //                 maxVector3 = v;
        //                 maxDistance = Math.Abs(v.magnitude);
        //                 if(Math.Abs(v.magnitude) < Math.Abs(MagCalibrater._Radius.magnitude))
        //                     ratio = Math.Abs(v.magnitude) / Math.Abs(MagCalibrater._Radius.magnitude);
        //                 else
        //                     ratio = Math.Abs(MagCalibrater._Radius.magnitude) / Math.Abs(v.magnitude);
        //             }
        //         }
        //         Debug.LogWarning(maxDistance + " == " + Math.Abs(MagCalibrater._Radius.magnitude) + " == " + MagCalibrater._Radius + " = " + maxVector3);
        //         //如果比例效果不理想。可以设置为ratio=0.5f 
        //         foreach (Vector3 i in list)
        //         {
        //             //- MagCalibrater._Center
        //             Vector3 v = i ;
        //             v *= ratio;
        //             if(endRecords.Count>3000)
        //             {
        //                 endRecords.RemoveAt(0);
        //             }
        //             endRecords.Add(v);
        //         }
        //         this.ellipseScript.ClearAndUpdatePointArray();
        //         // this.ellipseScript.DrawPointCloud(endRecords);

        //         MagCalibrater.Calibration = calibration;

        //         this.ellipseScript.ellipseTran.gameObject.SetActive(true);
        //         //绘制椭圆形
        //         if (MagCalibrater._Radius != this.ellipseScript.ellipseTran.localScale)
        //         {
        //             this.ellipseScript.setEllipseLocalScaleAndCenter(MagCalibrater._Radius, MagCalibrater._Center * ratio);
        //             //设置绘制图像相机的对应位置
        //             this.ellipseScript.setCameraPos(MagCalibrater._Center * ratio);
        //             this.ellipseScript.setCameraSize(MagCalibrater._Radius);
        //         }

        //         MagCalibrationButton.GetComponentInChildren<Text>().text = "开始地磁计校准";
        //         PlayerPrefs.SetString("o0MagneticCalibrater", JsonConvert.SerializeObject(MagCalibrater));

        //         #region 看校准是否完美，求Records方差
        //         if (list == null || list.Count == 0) isCalibrateMagPerfect = false;
        //         else
        //         {
        //             double sumV = 0;
        //             double[] listArray = new double[list.Count];
        //             for (int i = 0; i < list.Count; i++)
        //             {
        //                 listArray[i] = MagCalibrater.Update(list[i]).magnitude;
        //                 sumV += listArray[i];
        //             }
        //             double avarageV = sumV / listArray.Length;
        //             double varianceV = 0;
        //             foreach (double vvv in listArray)
        //             {
        //                 varianceV += Math.Pow(vvv - avarageV, 2);
        //             }
        //             varianceV /= listArray.Length;
        //             isCalibrateMagPerfect = varianceV < 0.001;
        //             PopupMgr.ins.ShowTip(TextAutoLanguage2.GetTextByKey("tip_mag-calibrate-variance-equal") + varianceV);
        //         }
        //         #endregion
        //     }
        // } catch (Exception e) { Debug.LogError(e.Message); }
    }

    //转换读取的数据，无符号->有符号
    float TwoByteToFloat(byte b1, byte b2) 
    {
        ushort twoByte = (ushort) (b1 * 256 + b2);
        short shortNum = (short) twoByte;
        return (float) shortNum; 
    }

    public void OnDataReceived(byte[] bytes)
    {
        // Debug.Log("瞄准模块数据长度" + bytes.Length);
        if (bytes.Length != 27)
        {
            if (bytes.Length == 2) {
                if (bytes[0] == 0x66 && bytes[1] == 0x31) {
                    if (SB_EventSystem.ins && SB_EventSystem.ins.simulateMouseIsAwaked) {
                        //鼠标居中
                        SB_EventSystem.ins.MakeMouseToScreenCenter(); 
                        //视角回正
                        DoIdentity();
                    } else {
                        AutoResetView.DoIdentity();
                    }
                } else if (bytes[0] == 0x66 && bytes[1] == 0x32) {
                    if (SB_EventSystem.ins) {
                    // if (SB_EventSystem.ins && !CommonConfig.SpecialVersion1) {
                        //唤起/隐藏虚拟鼠标
                        SB_EventSystem.ins.AwakenSimulateMouse(); 
                    }
                } else if (bytes[1] == 10) {
                    //显示电量
                    DeviceBatteryView.ins.RenderBattery(1, bytes[0]); 
                }
            } else if (bytes[0] == 0x5b) {
                if (SB_EventSystem.ins && SB_EventSystem.ins.simulateMouseIsAwaked) {
                    //点击鼠标
                    SB_EventSystem.ins.ClickMouse(); 
                } else {
                    //红外射击检测
                    ShootCheck.ins.ShootByInfrared(bytes); 
                }
            }
            return;
        }
        // if (DeviceBatteryView.ins) {
        //     DeviceBatteryView.ins.labelTemperature.text = (TwoByteToFloat(bytes[5], bytes[6]) / 100).ToString("#0.00") + "℃";
        // }
        if (bytes[7] == 0 && bytes[8] == 0 && bytes[9] == 0 && bytes[10] == 0 && bytes[11] == 0 && bytes[12] == 0)
            return;
        if (bytes[19] == 0 && bytes[20] == 0 && bytes[21] == 0 && bytes[22] == 0 && bytes[23] == 0 && bytes[24] == 0)
            return;

       //if (ban9AxisCalculate) //如果箭射出后禁止九轴计算，就缓存最新几帧九轴数据
       //{
       //    
       //    if (cached9AxisFrames.Count < 2)
       //    {
       //        cached9AxisFrames.Enqueue(bytes);
       //    }
       //    else
       //    {
       //        cached9AxisFrames.Dequeue();
       //        cached9AxisFrames.Enqueue(bytes);
       //    }
       //    return;
       //}

        float ax = TwoByteToFloat(bytes[7], bytes[8]);
        float ay = TwoByteToFloat(bytes[9], bytes[10]);
        float az = TwoByteToFloat(bytes[11], bytes[12]);

        float roll = TwoByteToFloat(bytes[13], bytes[14]);
        float pitch = TwoByteToFloat(bytes[15], bytes[16]);
        float yaw = TwoByteToFloat(bytes[17], bytes[18]);

        float x = TwoByteToFloat(bytes[19], bytes[20]);
        float y = TwoByteToFloat(bytes[21], bytes[22]);
        float z = TwoByteToFloat(bytes[23], bytes[24]);

        float mxr = TwoByteToFloat(bytes[20], bytes[19]);
        float myr = TwoByteToFloat(bytes[22], bytes[21]);
        float mzr = TwoByteToFloat(bytes[24], bytes[23]);

        if (CommonConfig.devicePlan == 0) {
            Acc = new Vector3(-az, ay, -ax) / 32768 * 16;
            Gyr = new Vector3(yaw, -pitch, roll) / 32768 * 2;
            Mag = new Vector3(-z, y, x) / 32768 * 256; //旧版
            mag0o = UnityVectorTo0o(Mag);
            if (!MagCalibrater.Update(mag0o)) {
                isCalibrateMagPerfect = false;
                return;
            }
            isCalibrateMagPerfect = true;
            mag0o = MagCalibrater.EllipsoidFitting.Map(mag0o);
            Mag = o0VectorToUnity(mag0o);
        } else if (CommonConfig.devicePlan == 1) {
            Acc = new Vector3(ax, ay, az) / 32768 * 16;
            Gyr = new Vector3(roll, pitch, yaw) / 32768 * 2;
            Mag = new Vector3(z, x, -y) / 32768 * 256;//第一个6+3硬件
        } else if (CommonConfig.devicePlan == 2) {
            Acc = new Vector3(-az, ax, -ay) / 32768 * 16;
            Gyr = new Vector3(-yaw, roll, -pitch) / 32768 * 2;
            Mag = new Vector3(mzr, mxr, -myr) / 32768 * 256;//第二个6+3硬件
        } 

        AccObj.transform.GetChild(0).localPosition = Acc;

        Gyr = GyrCalibrater.Update(Gyr);

        if (GyrCalibrater.Calibration) 
        {
            if (gyrCalibrateCompleteCount < gyrCalibrateTotalCount) {
                gyrCalibrateCompleteCount++;
            }
        }
        if (GyrScaleText && GyrCalibrater.Calibration) 
        {    
            GyrScaleText.text = "" + (_9Axis.getGyrOld() * 1000000).ToString();
        }

        // if(Mag.x > -128 && Mag.y > -128 && Mag.z > -128 && Mag.x < 128 && Mag.y < 128 && Mag.z < 128)
        // {   
        //     //绘制地磁计点
        //     if (MagCalibrater.Calibration)
        //     {
        //         this.ellipseScript.AddAndUpdatePointArray(Mag);

        //         if (Mag.x > cMaxVector.x) cMaxVector.x = Mag.x;
        //         if (Mag.y > cMaxVector.y) cMaxVector.y = Mag.y;
        //         if (Mag.z > cMaxVector.z) cMaxVector.z = Mag.z;
        //         if (Mag.x < cMinVector.x) cMinVector.x = Mag.x;
        //         if (Mag.y < cMinVector.y) cMinVector.y = Mag.y;
        //         if (Mag.z < cMinVector.z) cMinVector.z = Mag.z;

        //         Vector3 centerPoint = (this.cMaxVector + this.cMinVector) / 2;

        //         //设置绘制图像相机的对应位置
        //         this.ellipseScript.setCameraPos(centerPoint);
        //         this.ellipseScript.setCameraSize(this.cMaxVector - this.cMinVector);

        //         PointCorrector.ins?.Update(centerPoint);
        //     }
        //     Mag = MagCalibrater.Update(Mag);

        //     if (MagScaleText)
        //     {
        //         MagScaleText.text = MagCalibrater._Radius.ToString();
        //     }
           
        // }
        MagObj.transform.GetChild(0).localPosition = Mag;

        var ms = (((long)bytes[1]) *60 + bytes[2])*1000 + (long)TwoByteToFloat(bytes[3], bytes[4]);
        if(msOld == default)
        {
            msOld = ms;
            return;
        }
        TimeGap = ms - msOld;
        msOld = ms;

        GapMs = TimeGap;
        gyr0o = UnityVectorTo0o(Gyr);
        acc0o = UnityVectorTo0o(Acc);
        
        distanceToAxis.Update(gyr0o, acc0o, mag0o, GapMs);
        acc0o = distanceToAxis.AccCorrection(gyr0o, acc0o, GapMs);/**///轴心偏离矫正
        Acc = o0VectorToUnity(acc0o);

        var GyrOperator = new Quaternion();
        GyrOperator.eulerAngles = Gyr * TimeGap;
        GryMesh.transform.localRotation *= GyrOperator;

        AMesh.transform.localRotation = newRotation = _9Axis.update(Acc, Gyr, Mag, TimeGap);

        if (BowQuatDebug.ins) BowQuatDebug.ins.ShowModuleQuat(newRotation.eulerAngles);

        if (SB_EventSystem.ins && SB_EventSystem.ins.simulateMouseIsAwaked) {
            SB_EventSystem.ins.MoveSimulateMouse(newRotation);
        }
    }

    o0.Bow.DistanceToAxis distanceToAxis = new o0.Bow.DistanceToAxis();
    double GapMs;
    o0.Geometry.Vector<double> gyr0o;
    o0.Geometry.Vector<double> acc0o;
    o0.Geometry.Vector<double> mag0o;
    o0.Geometry.Vector<double> UnityVectorTo0o(Vector3 src) {
        return new o0.Geometry.Vector<double>(double.Parse(src.x.ToString()), double.Parse(src.y.ToString()), double.Parse(src.z.ToString()));
    }
    Vector3 o0VectorToUnity(o0.Geometry.Vector<double> src) {
        return new Vector3(float.Parse(src.x.ToString()), float.Parse(src.y.ToString()), float.Parse(src.z.ToString()));
    }

    [NonSerialized] public bool lerpForRotation = true;
    [NonSerialized] public float lerpTimeRate = 7;
    public void Update()
    {
        if (controlObj && !ban9AxisCalculate)
        {
            if (lerpForRotation)
            {
                controlObj.localRotation = Quaternion.Lerp(controlObj.localRotation, newRotation, Time.deltaTime * lerpTimeRate);   
            } 
            else 
            {
                controlObj.localRotation = newRotation;
            }
        }
    }

    private bool ban9AxisCalculate = false;
    private Queue<byte[]> cached9AxisFrames = new Queue<byte[]>();
    public void Ban9AxisCalculate(bool ban) {
        ban9AxisCalculate = ban;
        if (!ban) {
            msOld = default;
            // try
            // {
            //     if (StatesBackDebug.ins) _9Axis.DeleteStatesFromTail(StatesBackDebug.ins.val);
            // }
            // catch (Exception) { }
            //恢复九轴计算时，把缓存的最新几帧计算了
            // bool isFirstFrame = true;
            while (cached9AxisFrames.Count > 0)
            {
                try
                {
                    OnDataReceived(cached9AxisFrames.Dequeue());
                    //最初的一帧是用来顶掉msOld的，不会进state数组，因此不需要设置方差
                    // if (!isFirstFrame) {
                    //     _9Axis.SetAccMagVariance(10000);
                    // }
                    // isFirstFrame = false;
                }
                catch (Exception) { }
            }
            //立马应用到控制物体中
            if (controlObj) controlObj.localRotation = newRotation;
        }
    }

    Quaternion newRotation = Quaternion.identity;

    public void DoIdentity()
    {
        _9Axis.SetIdentityAndSave();
        Quaternion qua = _9Axis.getLastState().Qua;
        newRotation = qua;
        if (controlObj) controlObj.localRotation = qua;
    }
}