GHZMRNavigator/Assets/Scripts/Vuforial/VuforialTargetMove.cs

using System.Collections;
using System.Collections.Generic;
using System.Linq;
using UnityEngine;
using QFramework.MREditor;
using QFramework;


/// <summary>
/// 根据ImageTarget 和对应锚点 计算物体的 pos 和 rot以及物体的显示关闭
/// </summary>
public class VuforialTargetMove : MonoBehaviour
{
    /// <summary>
    /// 需要显示的物体  只负责显示
    /// </summary>
    //  public Transform showObj;

    public List<Transform> showObjs = new List<Transform>();
    /// <summary>
    /// 每个ImageTarget 对应的锚点
    /// </summary>
    public List<TargetShowObj> listObjs = new List<TargetShowObj>();

    [HideInInspector]
    /// <summary>
    /// 识别成功时，图片位置信息
    /// </summary>
    public Transform targetObj;

    public bool is6Dof = true;
    public float interval = 2;

    public float closeInterval = 3600f;
    ///// <summary>
    ///// 每张图对应显示物体的 位置差
    ///// </summary>
    //public Vector3 posOffset;
    ///// <summary>
    ///// 每张图对应显示物体的 旋转差
    ///// </summary>
    //public Vector3 rotOffset;

    public bool isClose = false;

    private bool state = false;
    /// <summary>
    ///  识别成功 三秒后打开
    /// </summary>
    private float openT;

    /// <summary>
    ///  10秒后关闭
    /// </summary>
    public float closeT;

    /// <summary>
    ///  每次打开只执行一次
    /// </summary>
    private bool OnlySet;

    private float onleyT;

    /// <summary>
    /// 当前识别TargetObj 对应的点
    /// </summary>
    private int num;

    private List<float> listRotY;

    private int lBNumber = 0;

    private List<float> listPosX;
    private List<float> listPosZ;

    private List<float> listPosX2;
    private List<float> listPosZ2;


    private KalmanFilter kf;
    void Start()
    {
        //rotOffset = Vector3.zero;
        //posOffset = Vector3.zero;
        if (listPosZ == null)
        {
            listRotY = new List<float>();
            listPosX = new List<float>();
            listPosZ = new List<float>();
            listPosX2 = new List<float>();
            listPosZ2 = new List<float>();
        }
        kf = new KalmanFilter();
        kf.SetQ(0.01f);
        kf.SetQ(0.1f);

    }

    // Update is called once per frame
    void Update()
    {
        //GameManager.Instance.text3.text = isClose + "   " + GameManager.Instance.isEditor;
        if (isClose)
            return;
        TargetMove();

        #region 3Dof Move
        //if (state)
        //{
        //    // 记时三秒 打开

        //    openT += Time.deltaTime;

        //    if(openT > interval)
        //    {
        //        SetShowObjePos();
        //        closeT = 0;
        //       // SetShowObjRot();
        //        if (OnlySet)
        //        {

        //            onleyT += Time.deltaTime;

        //            if (showObjs[0].gameObject.activeSelf == false || onleyT > interval) // 第一次打开 或者 替换了其它识别点
        //            {
        //                OnlySet = false;

        //                for (int i = 0; i < showObjs.Count; i++)
        //                {
        //                    showObjs[i].gameObject.SetActive(true);
        //                }
        //                GameManager.Instance.text2.text = "Show";
        //                SetShowObjRot();

        //                GameManager.Instance.text2.text = "OnlySet";


        //                GameManager.Instance.editorPanel.transform.position = targetObj.position + new Vector3(-0.2f, -0.3f, 0.1f);
        //                GameManager.Instance.editorPanel.transform.eulerAngles = new Vector3(GameManager.Instance.editorPanel.transform.eulerAngles.x,targetObj.eulerAngles.y, GameManager.Instance.editorPanel.transform.eulerAngles.z);
        //                GameManager.Instance.SetIsEditor(false);

        //                UIKit.GetPanel<EditorPanel>().SelectSpoitObj(showObjs[0].name);

        //            }
        //        }
        //    }
        //}
        //else
        //{
        //    //  ⏲十秒 关闭
        //    closeT += Time.deltaTime;
        //    GameManager.Instance.text2.text = closeT.ToString();
        //    if (closeT> closeInterval)
        //    {
        //        DirectClose();
        //       // GameManager.Instance.SetIsEditor(true);
        //    }
        //} 
        #endregion
    }

    private void TargetMove()
    {
        if (state)
        {
            // 记时三秒 打开

            openT += Time.deltaTime;

            if (openT > interval && openT < interval * 2.1f)
            {
                SetShowObjePos();
                closeT = 0;
                // SetShowObjRot();
                if (OnlySet)
                {

                    onleyT += Time.deltaTime;

                    if (showObjs[0].gameObject.activeSelf == false || onleyT > interval) // 第一次打开 或者 替换了其它识别点
                    {
                        OnlySet = false;

                        for (int i = 0; i < showObjs.Count; i++)
                        {
                            showObjs[i].gameObject.SetActive(true);
                            GameManager.Instance.text.text = showObjs[i].name;
                        }
                        GameManager.Instance.text2.text = "Show";
                        SetShowObjRot();

                        GameManager.Instance.text2.text = "OnlySet";

                        //   GameManager.Instance.uiMove.OpenEditor(transform);
                        UIKit.GetPanel<EditorPanel>().SelectSpoitObj(showObjs[0].name);

                    }
                }
            }
        }
        else
        {
            //  ⏲十秒 关闭
            closeT += Time.deltaTime;
         //   GameManager.Instance.text2.text = closeT.ToString();
            if (closeT > closeInterval)
            {
                DirectClose();
                // GameManager.Instance.SetIsEditor(true);
            }
        }

    }
    /// <summary>
    ///  显示
    /// </summary>
    /// <param name="targetObj"> 识别成功后的target物体  </param>
    /// <param name="num"> 显示物体下对应的点 </param>
    public void Show(Transform targetObj, int num)
    {
        openT = 0;
        onleyT = 0;
        ReplaceShow(targetObj, num);
    }
    public void ReplaceShow(Transform targetObj, int num)
    {
        VuforialHint.Instance.Close();
        Debug.Log("@@@@" + transform.name);
        isClose = false;
        this.targetObj = targetObj;
        this.num = num;
        state = true;
        closeT = 0;
        OnlySet = true;
       

        if (listPosZ == null)
        {
            listPosZ = new List<float>();
            listPosX = new List<float>();
            listPosX2 = new List<float>();
            listPosZ2 = new List<float>();
        }
        else
        {
            listPosX.Clear();
            listPosZ.Clear();
            listPosX2.Clear();
            listPosZ2.Clear();
        }

        //showObj.parent = transform;
        //showObj.localEulerAngles = new Vector3(0, -90, 0);

        //showObj.parent = listObjs[num].transform;
        //showObj.localPosition = listObjs[num].pos;
        //showObj.eulerAngles = new Vector3(0, listObjs[num].rotY, 0);
        gameObject.SetActive(true);

    }
    public void Close()
    {
        state = false;
        // isClose = true;
        // GameManager.Instance.SetIsEditor(true);
        //  closeT = 0;

        //if(listRotY.Count>10)
        //transform.eulerAngles = new Vector3(transform.eulerAngles.x, listRotY.Average(), transform.eulerAngles.z);
        //listRotY.Clear();
        //listRotY = new List<float>();
        //  showObj.parent = null;
    }

    /// <summary>
    ///  强制关闭 
    /// </summary>
    public void DirectClose()
    {
        //showObj.parent = transform;
        //showObj.localEulerAngles = new Vector3(0, -90, 0);
        VuforialHint.Instance.Show();
        for (int i = 0; i < showObjs.Count; i++)
        {
            showObjs[i].gameObject.SetActive(false);
        }
        closeT = 0;
        state = false;
        isClose = true;
        VuforialControl.Instance.Close();
        //   GameManager.Instance.uiMove.CloseEditor();
        // GameManager.Instance.SetIsEditor(true);
        //  gameObject.SetActive(false);
    }


    private void SetShowObjePos()
    {
        if (listObjs.Count == 0)
        {
            Debug.Log(" 未设置锚点");
            return;
        }
        Vector3 pos = targetObj.position - listObjs[num].transform.position;
        #region 双卡尔曼滤波 物体晃动幅度小，移动平滑  （仅适合用于3Dof）
    //    Debug.Log(num + "@@@");

        if (Mathf.Abs(pos.x) < 0.25f && Mathf.Abs(pos.z) < 0.2f)
        {
            lBNumber++;

            listPosX.Add(pos.x);
            listPosZ.Add(pos.z);
            if (listPosX.Count > 10)
            {
                listPosX.RemoveAt(0);
                listPosZ.RemoveAt(0);
            }
            if (lBNumber > 10)
            {
                List<float> x = new List<float>(kf.Filter(listPosX.ToArray()));
                pos.x = kf.Filter(pos.x);
                List<float> z = new List<float>(kf.Filter(listPosZ.ToArray()));
                pos.z = kf.Filter(pos.z);
                //  transform.position += new Vector3(x.Average(), 0, z.Average());
                transform.position += new Vector3(pos.x, 0, pos.z);
                //VuforialControl.Instance.text1.text = transform.position.ToString();
                //VuforialControl.Instance.text3.text = "Kalman LB " + pos.x.ToString() + "   " + pos.z.ToString();
                return;
            }
        }
        else
        {
            lBNumber = 0;
            listPosX2.Add(pos.x);
            listPosZ2.Add(pos.z);
            if (listPosX2.Count > 10)
            {
                listPosX2.RemoveAt(0);
                listPosZ2.RemoveAt(0);

                List<float> x = new List<float>(kf.Filter(listPosX2.ToArray()));
                pos.x = kf.Filter(pos.x);

                List<float> z = new List<float>(kf.Filter(listPosZ2.ToArray()));
                pos.z = kf.Filter(pos.z);
                //   transform.position += new Vector3(pos.x, 0, pos.z);
                transform.position += new Vector3(pos.x, 0, pos.z);
                //VuforialControl.Instance.text3.text = "Kalman " + pos.x.ToString() + "   " + pos.z.ToString();
                // VuforialControl.Instance.text1.text = transform.position.ToString();
            }

        }
        #endregion

    }

    /// <summary>
    ///  设置旋转属性
    /// </summary>
    private void SetShowObjRot()
    {
        //  transform.eulerAngles = obj.eulerAngles + rotOffset;
        if (listObjs.Count == 0)
        {
            Debug.Log(" 未设置锚点");
            return;
        }

        if (targetObj.eulerAngles.x > 60 || targetObj.eulerAngles.x < 60)
        {
            float y = targetObj.eulerAngles.y - listObjs[num].transform.eulerAngles.y;
            //listObjs[num].transform.eulerAngles = new Vector3(listObjs[num].transform.eulerAngles.x, targetObj.eulerAngles.z, listObjs[num].transform.eulerAngles.z);
            // listObjs[num].transform.eulerAngles = targetObj.eulerAngles;

            transform.RotateAround(listObjs[num].transform.position, Vector3.down, -y);
            Vector3 pos = targetObj.position - listObjs[num].transform.position;
            transform.position += new Vector3(0, pos.y, 0);
        }
        else
        {
            float y = targetObj.eulerAngles.y - listObjs[num].transform.eulerAngles.y;
            //listObjs[num].transform.eulerAngles = new Vector3(listObjs[num].transform.eulerAngles.x, targetObj.eulerAngles.z, listObjs[num].transform.eulerAngles.z);
            // listObjs[num].transform.eulerAngles = targetObj.eulerAngles;

            transform.RotateAround(listObjs[num].transform.position, Vector3.up, y);
            Vector3 pos = targetObj.position - listObjs[num].transform.position;
            transform.position += new Vector3(0, pos.y, 0);
        }
    }
}