slambb_smartbowsdkandbgbox/Assets/InfraredDemo/Scripts/InfraredDemo.cs

using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using UnityEngine.UI;
using SmartBowSDK;
using InfraredManager;
using ZIM;
using SLAMUVC;
using System;

/// <summary>
/// 红外 + 蓝牙的使用示例脚本
/// </summary>
public class InfraredDemo : MonoBehaviour
{
    public static InfraredDemo _ins;

    public static double MapValue(double value, double originalMin, double originalMax, double targetMin, double targetMax)
    {
        // 线性插值公式
        return targetMin + (value - originalMin) * (targetMax - targetMin) / (originalMax - originalMin);
    }


    private void Awake()
    {
        _ins = this;
    }
    void Start()
    {
        InitBluetoothModule();
        InitInfraredCamera();
    }

    void Update()
    {
        UpdateBluetoothModule();
        UpdateBluetoothModule6Axis();
        UpdateInfraredCamera();
    }

    #region 红外摄像
    public static bool running { get => infraredCameraHelper != null; }
    public static InfraredCameraHelper infraredCameraHelper { get; set; }

    [SerializeField] RawImage _cameraRender;
    [SerializeField] RawImage _imageScreenLocateManual;
    [SerializeField] List<RectTransform> _crosshairsInCamera;

    [SerializeField] Slider _sliderBrightness;
    [SerializeField] Slider _sliderContrast;
    [SerializeField] Slider _sliderShakeFilter;

    [SerializeField] Button _btnReset;
    [SerializeField] Button _btnScreenLocateManual;
    [SerializeField] Button _btnScreenLocateManualAuto;

    [SerializeField] Dropdown _dropdownResolution;

    [SerializeField] Text _fpsText;
    int frames;
    float lastCheckTime;

    //屏幕上ui的点
    List<Vector2> _screenLocatePoints = new();
    //屏幕内的点
    List<Vector2> _locatePointList = new();

    //public ParamFloatValue brightness = new ParamFloatValue("ic_brightness", 50.0f);
    // public ParamFloatValue contrast = new ParamFloatValue("ic_contrast", 50.0f);
    public ParamFloatValue shakeFilterValue = new ParamFloatValue("ic_shakeFilterValue2", 6.0f);
    public ParamFloatValue resoution = new ParamFloatValue("ic_resoution2", 2);


    string[] sliderNameArray = new string[]{
        "亮度", //
        "对比度"};

    string[] sliderStrArray = new string[]{
        "PU_BRIGHTNESS",
        "PU_CONTRAST"};

    UVCManager.CameraInfo currentCameraInfo;
    void InitInfraredCamera()
    {
        //SDK创建
        Vector2 saveResolution = GetResolution((int)resoution.Get());
        infraredCameraHelper = InfraredCameraHelper.GetInstance();
        infraredCameraHelper.Create((int)saveResolution.x, (int)saveResolution.y);

        //位置更新
        infraredCameraHelper.OnPositionUpdate += (Vector2 point) =>
        {
            Ray ray = Camera.main.ScreenPointToRay(point);
            Vector3 rayEndPoint = ray.GetPoint(200);
            Bow.Instance.transform.LookAt(rayEndPoint);
        };
        infraredCameraHelper.OnUVCIsReady += (UVCManager.CameraInfo camera) =>
        {

            if (currentCameraInfo != null) return;
            currentCameraInfo = camera;
            //生成控制摄像机的参数滑条
            Debug.Log("初始化摄像机！");
            //参数面板
            for (int i = 0; i < sliderStrArray.Length; i++)
            {
                string typeStr = sliderStrArray[i];
                UVCCtrlInfo _UVCCtrlInfo = camera.GetInfo(typeStr);
                int _curValue = _UVCCtrlInfo.current;
                //指定默认值
                //5、UVC亮度 - 50
                //6、UVC对比度 - 50
                if (typeStr == "PU_BRIGHTNESS")
                {
                    _sliderBrightness.minValue = _UVCCtrlInfo.min;
                    _sliderBrightness.maxValue = _UVCCtrlInfo.max;
                    _sliderBrightness.wholeNumbers = true;
                    SetBrightness(typeStr, _curValue);
                    _sliderBrightness.onValueChanged.AddListener((newValue) =>
                    {

                        SetBrightness(typeStr, newValue);
                    });
                }
                else if (typeStr == "PU_CONTRAST")
                {
                    _sliderContrast.minValue = _UVCCtrlInfo.min;
                    _sliderContrast.maxValue = _UVCCtrlInfo.max;
                    _sliderContrast.wholeNumbers = true;
                    SetContrast(typeStr, _curValue);
                    _sliderContrast.onValueChanged.AddListener((newValue) =>
                    {
                        SetContrast(typeStr, newValue);
                    });
                }
            }
            SetShakeFilterValue(shakeFilterValue.Get());
            _sliderShakeFilter.onValueChanged.AddListener(SetShakeFilterValue);
            //功能按钮
            _btnReset.onClick.AddListener(OnClick_Reset);
            _btnScreenLocateManual.onClick.AddListener(OnClick_ScreenLocateManual);
            _btnScreenLocateManualAuto.onClick.AddListener(OnClick_ScreenLocateAuto);
            //分辨率修改
            _dropdownResolution.onValueChanged.AddListener(v =>
            {
                SetResolution(v);
            });
            //算法红外阈值
            infraredCameraHelper.SetInfraredLocateBrightnessThreshold(0.8f);

        };
        //屏幕变化时候
        infraredCameraHelper.OnUVCPosUpdate += (list) =>
        {
            Debug.Log("OnUVCPosUpdate");
            SetLocatePointsToCameraRender(list, 1, 1);
        };

    }

    void UpdateInfraredCamera()
    {
        if (infraredCameraHelper == null) return;

        if (infraredCameraHelper.GetCameraTexture())
        {
            if (_cameraRender.texture == null || infraredCameraHelper.GetCameraTexture().GetNativeTexturePtr() != _cameraRender.texture.GetNativeTexturePtr())
                _cameraRender.texture = infraredCameraHelper.GetCameraTexture();

        }
        //手动定位
        if (infraredCameraHelper.IsScreenLocateManualDoing())
        {
            //引导提示
            string tipText = "单击屏幕 左下角";
            if (_screenLocatePoints.Count == 0) _locatePointList.Clear();
            if (_screenLocatePoints.Count == 1) tipText = "单击屏幕 右下角";
            if (_screenLocatePoints.Count == 2) tipText = "单击屏幕 右上角";
            if (_screenLocatePoints.Count == 3) tipText = "单击屏幕 左上角";
            _imageScreenLocateManual.transform.Find("Text").GetComponent<Text>().text = tipText;
            //点击检测
            if (Input.GetMouseButtonDown(0))
            {
                Vector2 mouse = Input.mousePosition;
                float u = Mathf.Clamp01(mouse.x / Screen.width);
                float v = Mathf.Clamp01(mouse.y / Screen.height);
                u = Math.Clamp(u, 0, 1);
                v = Math.Clamp(v, 0, 1);
                _locatePointList.Add(new Vector2(u, v));
                float texWidth = _imageScreenLocateManual.texture.width;
                float texHeight = _imageScreenLocateManual.texture.height;
                Vector2 texPoint = new Vector2(u * texWidth, v * texHeight);
                _screenLocatePoints.Add(texPoint);
            }
            //定位点显示
            Transform pointsTF = _imageScreenLocateManual.transform.Find("Points");
            for (int i = 0; i < pointsTF.childCount; i++)
            {
                Transform pointTF = pointsTF.GetChild(i);
                if (i < _screenLocatePoints.Count)
                {
                    float texWidth = _imageScreenLocateManual.texture.width;
                    float texHeight = _imageScreenLocateManual.texture.height;
                    Vector2 texSize = new Vector2(texWidth, texHeight);
                    Vector2 pos = _screenLocatePoints[i];
                    pointTF.localPosition = pos.pixelToLocalPosition_AnchorCenter(texSize, _imageScreenLocateManual.rectTransform.rect);
                    pointTF.gameObject.SetActive(true);
                }
                else pointTF.gameObject.SetActive(false);
            }
            //完成定位
            if (_screenLocatePoints.Count == 4)
            {
                _imageScreenLocateManual.gameObject.SetActive(false);
                SetLocatePointsToCameraRender(_screenLocatePoints, _imageScreenLocateManual.texture.width, _imageScreenLocateManual.texture.height);
                infraredCameraHelper.QuitScreenLocateManual(_locatePointList);
            }
        }
        //在相机画面显示准心
        if (ScreenLocate.Main)
        {
            var _sl = ScreenLocate.Main;
            var buffer = _sl.infraredSpotBuffer;
            if (buffer != null)
            {
                for (int i = 0; i < buffer.Length; i++)
                {
                    if (buffer[i].CameraLocation != null)
                    {
                        //添加一个偏移量，使得最后输出的准心是指向正中心
                        Vector2 newPoint2 = _sl.GetOffsetCameraLocation(buffer[i].CameraLocation.Value);
                        // 检测到光点 
                        var pos = newPoint2.pixelToLocalPosition_AnchorCenter(_sl.mUVCCameraInfo.Size, _cameraRender.rectTransform.rect);
                        _crosshairsInCamera[i].gameObject.SetActive(true);
                        _crosshairsInCamera[i].anchoredPosition = pos;

                    }
                    else
                        _crosshairsInCamera[i].gameObject.SetActive(false);
                }
            }
        }
        //性能检测
        ++frames;
        float timeNow = Time.realtimeSinceStartup;
        const float UpdateInterval = 0.5f;
        if (timeNow > lastCheckTime + UpdateInterval)
        {
            float fps = (float)(frames / (timeNow - lastCheckTime));
            frames = 0;
            lastCheckTime = timeNow;
            _fpsText.text = "FPS:" + fps.ToString("f2");
        }
    }

    public void SetLocatePointsToCameraRender(List<Vector2> points, float w, float h)
    {
        Transform pointsTF2 = _cameraRender.transform.Find("Points");

        if (pointsTF2.childCount == points.Count)
        {
            Vector2 texSize = new Vector2(w, h);
            for (int i = 0; i < pointsTF2.childCount; i++)
            {
                Transform pointTF = pointsTF2.GetChild(i);
                Vector2 pos = points[i];
                pointTF.localPosition = pos.pixelToLocalPosition_AnchorCenter(texSize, _cameraRender.rectTransform.rect);
                pointTF.gameObject.SetActive(true);
            }
        }
    }
    /// <summary>
    /// 亮度
    /// </summary>
    /// <param name="typeStr"></param>
    /// <param name="v"></param>
    void SetBrightness(string typeStr, float v)
    {
        var _value = Mathf.FloorToInt(v);
        currentCameraInfo.SetValue(typeStr, _value);
        //brightness.Set(v);
        _sliderBrightness.SetValueWithoutNotify(v);
        _sliderBrightness.transform.Find("Value").GetComponent<Text>().text = v.ToString("f1");
    }
    /// <summary>
    ///  对比度
    /// </summary>
    /// <param name="typeStr"></param>
    /// <param name="v"></param>
    void SetContrast(string typeStr, float v)
    {
        var _value = Mathf.FloorToInt(v);
        currentCameraInfo.SetValue(typeStr, _value);
        //contrast.Set(v);
        _sliderContrast.SetValueWithoutNotify(v);
        _sliderContrast.transform.Find("Value").GetComponent<Text>().text = v.ToString("f1");
    }
    void SetShakeFilterValue(float v)
    {
        shakeFilterValue.Set(v);
        _sliderShakeFilter.SetValueWithoutNotify(shakeFilterValue.Get());
        _sliderShakeFilter.transform.Find("Value").GetComponent<Text>().text = shakeFilterValue.Get().ToString("f1");
        InfraredCameraHelper.GetInstance().SetShakeFilterValue(shakeFilterValue.Get());
    }
    void SetResolution(int optionIndex)
    {
        resoution.Set(optionIndex);
        _dropdownResolution.SetValueWithoutNotify(optionIndex);
        switch (optionIndex)
        {
            case 0:
                infraredCameraHelper.SetCameraResolutionNew(1280, 720);
                break;
            case 1:
                infraredCameraHelper.SetCameraResolutionNew(640, 360);
                break;
            case 2:
                infraredCameraHelper.SetCameraResolutionNew(320, 240);
                break;
            case 3:
                infraredCameraHelper.SetCameraResolutionNew(160, 120);
                break;
        }
    }
    Vector2 GetResolution(int optionIndex)
    {
        _dropdownResolution.SetValueWithoutNotify(optionIndex);
        Vector2 vect = new Vector2(320, 240);
        switch (optionIndex)
        {
            case 0:
                vect = new Vector2(1280, 720);
                break;
            case 1:
                vect = new Vector2(640, 360);
                break;
            case 2:
                vect = new Vector2(320, 240);
                break;
            case 3:
                vect = new Vector2(160, 120);
                break;
        }
        return vect;
    }
    void OnClick_Reset()
    {
        SetBrightness("PU_BRIGHTNESS", 50);
        SetContrast("PU_CONTRAST", 50);
        SetShakeFilterValue(6);
    }
    void OnClick_ScreenLocateManual()
    {
        if (infraredCameraHelper.IsScreenLocateManualDoing()) return;
        Texture2D texture2D = infraredCameraHelper.EnterScreenLocateManual();
        if (texture2D == null)
        {
            infraredCameraHelper.QuitScreenLocateManual(null);
            return;
        }
        _imageScreenLocateManual.texture = texture2D;
        //_imageScreenLocateManual.material = infraredCameraHelper.GetCameraMaterial();
        _screenLocatePoints.Clear();
        _imageScreenLocateManual.gameObject.SetActive(true);
    }

    void OnClick_ScreenLocateAuto()
    {
        infraredCameraHelper.EnterScreenLocateManualAuto();
    }
    public void OnClick_SetAdjustPointsOffset()
    {
        var _sl = ScreenLocate.Main;
        var buffer = _sl.infraredSpotBuffer;
        if (buffer != null)
        {
            for (int i = 0; i < buffer.Length; i++)
            {
                if (buffer[i].CameraLocation != null)
                {
                    Debug.Log("CameraLocation:" + buffer[i].CameraLocation.Value);
                    var centerOffset = infraredCameraHelper.GetCenterOffset(buffer[i].CameraLocation.Value, "CameraLocation");
                    Debug.Log("CenterOffset: " + centerOffset);
                    var uvCenterOffset = infraredCameraHelper.GetCenterOffset(buffer[i].ScreenUV.Value, "ScreenUV");
                    Debug.Log("UvCenterOffset: " + uvCenterOffset);
                }
            }
        }


    }
    #endregion

    #region 蓝牙模块
    [SerializeField] Button _btnConnect;
    [SerializeField] Text _textMacAddress;
    [SerializeField] Text _textBattery;
    SmartBowHelper OriginSmartBowHelper;

    //六轴部分按钮操作
    [SerializeField] Button _btnConnect6Axis;
    [SerializeField] Text _textMacAddress6Axis;
    [SerializeField] Text _textBattery6Axis;
    SmartBowHelper BGBoxSmartBowHelper;
    [SerializeField] GameObject _BGBoxObj;

    void InitBluetoothModule()
    {
        SmartBowLogger.isDebug = true;

        OriginSmartBowHelper = SmartBowHelper.NewInstance();
        OriginSmartBowHelper.OnBluetoothModuleInited += OnBluetoothModuleInited;
        OriginSmartBowHelper.OnBluetoothError += OnBluetoothError;
        OriginSmartBowHelper.OnShooting += OnShooting;
        _btnConnect.onClick.AddListener(OnClick_Connect);
     


        //6轴部分代码
        BGBoxSmartBowHelper = SmartBowHelper.NewInstance();
        BGBoxSmartBowHelper.OnBluetoothModuleInited += OnBluetoothModuleInited6Axis;
        BGBoxSmartBowHelper.OnBluetoothError += OnBluetoothError6Axis;
        BGBoxSmartBowHelper.OnSixAxisRotationUpdate += (eulerAngles) =>
        {
            _BGBoxObj.transform.eulerAngles = eulerAngles;
        };
        //识别当前手柄
        BGBoxSmartBowHelper.SetFilters("BGBox_202012");
        //设置手柄解析
        BGBoxSmartBowHelper.SetSensorAxisType(SensorAxisType.SixAxisSensor);
        _btnConnect6Axis.onClick.AddListener(OnClick_ConnectBGBox);

        if (BluetoothWindows.IsWindows()) {
            BleWinHelper.RegisterTo(OriginSmartBowHelper.gameObject, OriginSmartBowHelper.CreateBluetoothWindows());
            BleWinHelper.RegisterTo(BGBoxSmartBowHelper.gameObject, BGBoxSmartBowHelper.CreateBluetoothWindows());
        }
        //Debug.Log("InitBluetoothModule");
    }
    void UpdateBluetoothModule()
    {
        //更新显示蓝牙状态
        BluetoothStatusEnum bluetoothStatus = OriginSmartBowHelper.GetBluetoothStatus();
        Text btnConnectText = _btnConnect.GetComponentInChildren<Text>();
        if (bluetoothStatus == BluetoothStatusEnum.None) btnConnectText.text = "<color=blue>未连接</color>（点击连接）";
        if (bluetoothStatus == BluetoothStatusEnum.Connecting) btnConnectText.text = "<color=#FF670D>连接中</color>";
        if (bluetoothStatus == BluetoothStatusEnum.Connected)
        {
            if (OriginSmartBowHelper.IsBluetoothModuleInited()) btnConnectText.text = "<color=green>已连接</color>（点击断开）";
            else btnConnectText.text = "<color=green>已连接</color><color=blue>（正在初始化）</color>";
        }
        //更新显示电量
        int battery = OriginSmartBowHelper.GetBattery();
        _textBattery.text = battery > 0 ? $"电量值:{battery}" : "未获得电量值";
        //更新显示Mac地址
        string macAddress = OriginSmartBowHelper.GetMacAddress();
        _textMacAddress.text = macAddress != null ? $"Mac地址:{macAddress}" : "未获得Mac地址";
    }
    void OnBluetoothModuleInited()
    {

        OriginSmartBowHelper.StartRotationSensor();
        OriginSmartBowHelper.StartShootingSensor();
    }
    void OnBluetoothError(BluetoothError error, string message)
    {
        Debug.Log("OnBluetoothError:" + error);
        if (error == BluetoothError.ScanNotFoundTargetDevice)
        {
            TipText.Show("连接失败，未发现目标设备！");
            return;
        }
        TipText.Show(message);
    }
    void OnShooting(float speed)
    {
        Bow.Instance.Shoot();
    }
    void OnClick_Connect()
    {
        if (OriginSmartBowHelper.GetBluetoothStatus() == BluetoothStatusEnum.Connecting) return;
        if (OriginSmartBowHelper.GetBluetoothStatus() == BluetoothStatusEnum.None)
        {
            OriginSmartBowHelper.Connect("test", 1);
            return;
        }
        if (OriginSmartBowHelper.GetBluetoothStatus() == BluetoothStatusEnum.Connected)
        {
            OriginSmartBowHelper.Disconnect();
            return;
        }
    }


    /// <summary>
    /// 更新BGBox信息
    /// </summary>
    void UpdateBluetoothModule6Axis()
    {
        //更新显示蓝牙状态
        BluetoothStatusEnum bluetoothStatus = BGBoxSmartBowHelper.GetBluetoothStatus();
        Text btnConnectText = _btnConnect6Axis.GetComponentInChildren<Text>();
        if (bluetoothStatus == BluetoothStatusEnum.None) btnConnectText.text = "<color=blue>未连接</color>（点击连接）";
        if (bluetoothStatus == BluetoothStatusEnum.Connecting) btnConnectText.text = "<color=#FF670D>连接中</color>";
        if (bluetoothStatus == BluetoothStatusEnum.Connected)
        {
            if (BGBoxSmartBowHelper.IsBluetoothModuleInited()) btnConnectText.text = "<color=green>已连接</color>（点击断开）";
            else btnConnectText.text = "<color=green>已连接</color><color=blue>（正在初始化）</color>";
        }
        //更新显示电量
        int battery = BGBoxSmartBowHelper.GetBattery();
        _textBattery6Axis.text = battery > 0 ? $"电量值:{battery}" : "未获得电量值";
        //更新显示Mac地址
        string macAddress = BGBoxSmartBowHelper.GetMacAddress();
        _textMacAddress6Axis.text = macAddress != null ? $"Mac地址:{macAddress}" : "未获得Mac地址";
    }
    void OnBluetoothModuleInited6Axis()
    {
        BGBoxSmartBowHelper.StartRotationSensor();
    }
    public void Open6Axis()
    {
        BGBoxSmartBowHelper.StartRotationSensor();
    }
    public void Close6Axis() {
        BGBoxSmartBowHelper.StopRotationSensor();
    }

    public void Reset6Axis()
    {
        // if(sixAxisFusion!=null) sixAxisFusion.ResetToInitialRotation();
        BGBoxSmartBowHelper.ResetSixAxisIdentity();
    }

    void OnBluetoothError6Axis(BluetoothError error, string message)
    {
        Debug.Log("6Axis OnBluetoothError:" + error);
        if (error == BluetoothError.ScanNotFoundTargetDevice)
        {
            TipText.Show("连接失败，未发现目标设备！");
            return;
        }
        TipText.Show(message);
    }
    void OnClick_ConnectBGBox() {
        if (BGBoxSmartBowHelper.GetBluetoothStatus() == BluetoothStatusEnum.Connecting) return;
        if (BGBoxSmartBowHelper.GetBluetoothStatus() == BluetoothStatusEnum.None)
        {
            BGBoxSmartBowHelper.Connect("test2", 1);
            return;
        }
        if (BGBoxSmartBowHelper.GetBluetoothStatus() == BluetoothStatusEnum.Connected)
        {
            BGBoxSmartBowHelper.Disconnect();
            return;
        }
    }


    #endregion
}
public class ParamFloatValue
{
    private string _saveKey;
    private float _valueDefault;
    private bool _valueLoaded;
    private float _value;
    public ParamFloatValue(string saveKey, float valueDefault)
    {
        _saveKey = saveKey;
        _valueDefault = valueDefault;
    }
    public float Get()
    {
        if (!_valueLoaded) _value = PlayerPrefs.GetFloat(_saveKey, _valueDefault);
        return _value;
    }
    public void Set(float value)
    {
        _value = value;
        PlayerPrefs.SetFloat(_saveKey, _value);
        PlayerPrefs.Save();
    }
}