GHZSDKXR/Runtime/SpatialMesh/Scripts/SpatialMeshManager.cs

using System;
using UnityEngine;
using UnityEngine.UI;
using System.Collections;
using System.Collections.Generic;
using EZXR.Glass.SixDof;
using UnityEngine.SceneManagement;
using System.Runtime.InteropServices;
using System.Threading;
using EZXR.Glass.Core;

namespace EZXR.Glass.SpatialMesh
{
    public class SpatialMeshManager : MonoBehaviour
    {
        #region singleton
        private static SpatialMeshManager instance;
        public static SpatialMeshManager Instance
        {
            get
            {
                return instance;
            }
        }
        #endregion

        public bool IsMeshDetecting
        {
            get
            {
                return isMeshDetecting;
            }
        }
        private bool isMeshDetecting = false;

        public int DetectedMeshChunksCount
        {
            get
            {
                return m_MeshGameObjects == null ? 0 : m_MeshGameObjects.Count;
            }
        }

        public bool incrementalMeshVisible
        {
            set
            {
                if (m_MeshRoot != null)
                {
                    m_MeshRoot.SetActive(value);
                }
            }
        }
        public bool smoothedMeshVisible
        {
            set
            {
                if (m_SmoothedMeshes != null)
                {
                    m_SmoothedMeshes.SetActive(value);
                }
            }
        }
        public uint meshExtractFrame = 30;

        public uint chunkUpdateLimitation = 5;

        //vertex array
        private Vector3[] smoothedMeshVertexs = null;
        //normal
        private Vector3[] smoothedMeshNormals = null;
        //faces array
        private int[] smoothedMeshFaces = null;

        private GameObject m_MeshRoot;
        private GameObject m_SmoothedMeshes;
        private Dictionary<Vector3Int, GameObject> m_MeshGameObjects;

        private ulong m_FrameCount = 0;
        private EZVIOBackendIncrementalMesh m_IncrementalMesh = new EZVIOBackendIncrementalMesh();
        private bool isNewSmoothMesh = false;

        private Thread meshHandleThread;
        private const int MESH_HANDLE_EVENT_UPDATE_INCREMENTAL = 0x0001;
        private const int MESH_HANDLE_EVENT_UPDATE_SMOOTHED = 0x0002;
        private const int MESH_HANDLE_EVENT_SAVE_SMOOTHED = 0x0003;
        private List<int> meshHandleEventList = new List<int>();

        private void Awake()
        {
            instance = this;

            ARConfig.DefaultConfig.MeshFindingMode = MeshFindingMode.Enable;

            Application.targetFrameRate = 60;
        }

        private void OnEnable()
        {
            // 注册回调
            ARFrame.trackableManager.recenterIncrementalMeshesListener += RecenterMeshes;
            resumeMeshDetecting();
        }

        private void OnDisable()
        {
            pauseMeshDetecting();
            // 注销回调
            ARFrame.trackableManager.recenterIncrementalMeshesListener -= RecenterMeshes;
        }
        private void OnDestroy()
        {
            if (meshHandleThread != null)
                meshHandleThread.Abort();
        }

        // Use this for initialization
        void Start()
        {
            if (chunkUpdateLimitation == 0)
                chunkUpdateLimitation = 5;

            if (meshExtractFrame == 0)
            {
                meshExtractFrame = 30;
            }
            m_MeshRoot = new GameObject();
            m_MeshRoot.name = "backendmesh";
            m_MeshRoot.transform.position = new Vector3(0, 0, 0);
            m_MeshRoot.transform.rotation = new Quaternion(0, 0, 0, 1);

            m_MeshGameObjects = new Dictionary<Vector3Int, GameObject>();

            meshHandleThread = new Thread(this.MeshHandleRun);
            meshHandleThread.Start();
            meshHandleEventList.Add(MESH_HANDLE_EVENT_UPDATE_INCREMENTAL);
        }
        public void resumeMeshDetecting()
        {
            Debug.Log("SpatialMeshManager resumeMeshDetecting");
            ARConfig.DefaultConfig.MeshFindingMode = MeshFindingMode.Enable;
            SessionManager.Instance.ResumeSession();
            isMeshDetecting = true;
            InvokeRepeating("postUpdateIncrementalMeshEvent", 1.0f, 0.05f);
        }

        public void pauseMeshDetecting()
        {
            Debug.Log("SpatialMeshManager pauseMeshDetecting");
            CancelInvoke("postUpdateIncrementalMeshEvent");
            ARConfig.DefaultConfig.MeshFindingMode = MeshFindingMode.Disable;
            SessionManager.Instance.ResumeSession();
            isMeshDetecting = false;
        }

        // Update is called once per frame
        void Update()
        {
        }
        private void postUpdateIncrementalMeshEvent()
        {
            meshHandleEventList.Add(MESH_HANDLE_EVENT_UPDATE_INCREMENTAL);
        }
        private void MeshHandleRun()
        {
            while (true)
            {
                if (meshHandleEventList.Count < 1)
                {
                    Thread.Sleep(50);
                    continue;
                }
                int eventID = meshHandleEventList[0];
                meshHandleEventList.RemoveAt(0);
                switch (eventID)
                {
                    case MESH_HANDLE_EVENT_UPDATE_INCREMENTAL:
                        if (isMeshDetecting && ARFrame.SessionStatus == EZVIOState.EZVIOCameraState_Tracking)
                        {
                            bool getMeshResult = SpatialMeshDetectorWrapper.getBackendIncrementalMeshData(ref m_IncrementalMesh);
                            if (getMeshResult)
                                ARFrame.trackableManager.UpdateIncrementalMeshes(m_IncrementalMesh);
                        }
                        break;
                    case MESH_HANDLE_EVENT_UPDATE_SMOOTHED:
                        updateSmoothedMeshesData();
                        break;
                    case MESH_HANDLE_EVENT_SAVE_SMOOTHED:
                        updateSmoothedMeshesData();
                        //Save Smoothed to SDCard/Download/backendSmoothedMesh_xxx.ply
                        SpatialMeshDetectorWrapper.SaveBackendSmoothedMeshData();
                        break;
                    default:
                        break;
                }
            }
        }
        public void SaveBackendSmoothedMesh()
        {
            Debug.Log("SpatialMeshManager -- SaveBackendSmoothedMesh add event");
            meshHandleEventList.Add(MESH_HANDLE_EVENT_UPDATE_SMOOTHED);
            meshHandleEventList.Add(MESH_HANDLE_EVENT_SAVE_SMOOTHED);
        }

        public void UpdateSmoothedMeshes()
        {
            Debug.Log("SpatialMeshManager -- UpdateSmoothedMeshes add event");
            meshHandleEventList.Add(MESH_HANDLE_EVENT_UPDATE_SMOOTHED);
        }

        public void ShowSmoothedMeshes()
        {
            if (!isNewSmoothMesh) {
                return;
            }
            if (m_SmoothedMeshes == null) {
                m_SmoothedMeshes = new GameObject();
                m_SmoothedMeshes.name = "smoothedmesh";
                m_SmoothedMeshes.transform.position = Vector3.zero;
                m_SmoothedMeshes.transform.rotation = Quaternion.identity;

                MeshFilter mf = m_SmoothedMeshes.AddComponent<MeshFilter>();
                MeshRenderer mr = m_SmoothedMeshes.AddComponent<MeshRenderer>();
                MeshCollider mc = m_SmoothedMeshes.AddComponent<MeshCollider>();
                Material material = new Material(Shader.Find("SuperSystems/SpatialMapping"));

                mf.mesh = new UnityEngine.Mesh();
                mr.material = material;
                mr.allowOcclusionWhenDynamic = true;

                m_SmoothedMeshes.layer = LayerMask.NameToLayer("Mesh");

                m_SmoothedMeshes.transform.parent = m_SmoothedMeshes.transform;
                PhysicMaterial pm = new PhysicMaterial();
                pm.staticFriction = 0.9f;//设置Mesh通用物理材质
                mc.material = pm;
            }
            m_SmoothedMeshes.SetActive(true);
           
            if (smoothedMeshFaces != null && smoothedMeshFaces.Length > 0)
            {
                m_SmoothedMeshes.GetComponent<MeshFilter>().sharedMesh.Clear();
                m_SmoothedMeshes.GetComponent<MeshFilter>().sharedMesh.indexFormat = UnityEngine.Rendering.IndexFormat.UInt32;
                m_SmoothedMeshes.GetComponent<MeshFilter>().sharedMesh.vertices = smoothedMeshVertexs;
                m_SmoothedMeshes.GetComponent<MeshFilter>().sharedMesh.triangles = smoothedMeshFaces;

                m_SmoothedMeshes.GetComponent<MeshCollider>().sharedMesh = m_SmoothedMeshes.GetComponent<MeshFilter>().sharedMesh;

                smoothedMeshFaces = null;
            }
            isNewSmoothMesh = false;
        }
        public void ShowIncrementalMeshes()
        {
            m_MeshRoot.SetActive(true);

            if (ARFrame.trackableManager.chunksToUpdate.Count > 0)
            {
                List<Vector3Int> chunksToUpdate = ARFrame.trackableManager.chunksToUpdate;
                List<Vector3[]> cachedVSToUpdate = ARFrame.trackableManager.cachedVSToUpdate;
                List<int[]> cachedTSToUpdate = ARFrame.trackableManager.cachedTSTupUpdate;

                int count = Math.Min(chunksToUpdate.Count, (int)chunkUpdateLimitation);

                Debug.Log("SpatialMeshManager ShowIncrementalMeshes " + chunksToUpdate.Count + " tobeupdate, really use " + count);

                Debug.Log("SpatialMeshManager ShowIncrementalMeshes BEFORE chunkstoupdate totally " + m_MeshGameObjects.Count + " gameobjects");

                for (int i = 0; i < count; i++)
                {
                    Vector3Int chunkIdx = chunksToUpdate[i];
                    if (!m_MeshGameObjects.ContainsKey(chunkIdx))
                    {
                        GameObject newMesh = new GameObject();
                        newMesh.name = "backendmesh" + chunkIdx;
                        //newMesh.tag = "NoInteraction";
                        newMesh.transform.position = new Vector3(0, 0, 0);
                        newMesh.transform.rotation = new Quaternion(0, 0, 0, 1);

                        MeshFilter mf = newMesh.AddComponent<MeshFilter>();
                        MeshRenderer mr = newMesh.AddComponent<MeshRenderer>();
                        MeshCollider mc = newMesh.AddComponent<MeshCollider>();
                        Material material = new Material(Shader.Find("SuperSystems/SpatialMapping"));

                        mf.mesh = new UnityEngine.Mesh();
                        mr.material = material;

                        mr.allowOcclusionWhenDynamic = true;

                        newMesh.layer = LayerMask.NameToLayer("Mesh");

                        newMesh.transform.parent = m_MeshRoot.transform;

                        m_MeshGameObjects.Add(chunkIdx, newMesh);
                    }

                    m_MeshGameObjects[chunkIdx].GetComponent<MeshFilter>().sharedMesh.Clear();
                    m_MeshGameObjects[chunkIdx].GetComponent<MeshFilter>().sharedMesh.indexFormat = UnityEngine.Rendering.IndexFormat.UInt32;
                    m_MeshGameObjects[chunkIdx].GetComponent<MeshFilter>().sharedMesh.vertices = cachedVSToUpdate[i];
                    m_MeshGameObjects[chunkIdx].GetComponent<MeshFilter>().sharedMesh.triangles = cachedTSToUpdate[i];

                    m_MeshGameObjects[chunkIdx].GetComponent<MeshCollider>().sharedMesh = m_MeshGameObjects[chunkIdx].GetComponent<MeshFilter>().sharedMesh;

                    PhysicMaterial pm = new PhysicMaterial();
                    pm.staticFriction = 0.9f;//设置Mesh通用物理材质
                    m_MeshGameObjects[chunkIdx].GetComponent<MeshCollider>().material = pm;
                }

                ARFrame.trackableManager.CommitCachedChunksUse(count);

                Debug.Log("SpatialMeshManager ShowIncrementalMeshes AFTER chunkstoupdate totally " + m_MeshGameObjects.Count + " gameobjects");
            }
        }

        private void RecenterMeshes(Matrix4x4 recenterOffset)
        {
            if (m_MeshGameObjects != null)
            {
                foreach (var item in m_MeshGameObjects)
                {
                    for (int i = 0; i < item.Value.GetComponent<MeshFilter>().sharedMesh.vertices.Length; i++)
                    {
                        Vector3 tmp = item.Value.GetComponent<MeshFilter>().sharedMesh.vertices[i];
                        item.Value.GetComponent<MeshFilter>().sharedMesh.vertices[i] = recenterOffset.MultiplyPoint3x4(tmp);
                    }
                }
            }

            if (m_SmoothedMeshes != null)
            {
                MeshFilter[] allOldMeshes = m_SmoothedMeshes.GetComponentsInChildren<MeshFilter>();
                if (allOldMeshes != null)
                {
                    foreach (MeshFilter item in allOldMeshes)
                    {
                        for (int i = 0; i < item.sharedMesh.vertices.Length; i++)
                        {
                            Vector3 tmp = item.sharedMesh.vertices[i];
                            item.sharedMesh.vertices[i] = recenterOffset.MultiplyPoint3x4(tmp);
                        }
                    }
                }
            }
        }
        private void updateSmoothedMeshesData()
        {
            EZVIOBackendMesh smoothedMesh = new EZVIOBackendMesh();
            isNewSmoothMesh = SpatialMeshDetectorWrapper.getBackendSmoothedMeshData(ref smoothedMesh);
            if (!isNewSmoothMesh)
            {
                Debug.Log("SpatialMeshManager -- updateSmoothedMeshesData failed");
                UpdateSmoothedMeshes();
                return;
            }
            Debug.Log("SpatialMeshManager -- updateSmoothedMeshesData vertex Count " + smoothedMesh.vertexCount);

            //vertex
            float[] smoothedMeshVertexArrays = new float[smoothedMesh.vertexCount * 3];
            Marshal.Copy(smoothedMesh.vertex, smoothedMeshVertexArrays, 0, smoothedMesh.vertexCount * 3);
            smoothedMeshVertexs = new Vector3[smoothedMesh.vertexCount];
            for (int i = 0; i < smoothedMesh.vertexCount; i++)
            {
                Vector3 tmp = new Vector3(
                    smoothedMeshVertexArrays[i * 3 + 0], smoothedMeshVertexArrays[i * 3 + 2],
                    smoothedMeshVertexArrays[i * 3 + 1]); // yz互换
                smoothedMeshVertexs[i] = ARFrame.accumulatedRecenterOffset4x4.MultiplyPoint3x4(tmp);
            }

            //normal
            float[] smoothedMeshNormalArrays = new float[smoothedMesh.normalCount * 3];
            Marshal.Copy(smoothedMesh.normal, smoothedMeshNormalArrays, 0, smoothedMesh.normalCount * 3);
            smoothedMeshNormals = new Vector3[smoothedMesh.normalCount];
            for (int i = 0; i < smoothedMesh.normalCount; i++)
            {
                Vector3 tmp = new Vector3(
                    smoothedMeshNormalArrays[i * 3 + 0], smoothedMeshNormalArrays[i * 3 + 2],
                    smoothedMeshNormalArrays[i * 3 + 1]); // yz互换
                smoothedMeshNormals[i] = ARFrame.accumulatedRecenterOffset4x4.MultiplyPoint3x4(tmp);
            }
            //faces
            int[] smoothedMeshFaceArrays = new int[smoothedMesh.facesCount * 3];
            Marshal.Copy(smoothedMesh.faces, smoothedMeshFaceArrays, 0, smoothedMesh.facesCount * 3);
            smoothedMeshFaces = new int[smoothedMesh.facesCount * 3];
            for (int i = 0; i < smoothedMesh.facesCount; i++)
            {
                smoothedMeshFaces[i * 3 + 0] = smoothedMeshFaceArrays[i * 3 + 0];
                smoothedMeshFaces[i * 3 + 1] = smoothedMeshFaceArrays[i * 3 + 2];
                smoothedMeshFaces[i * 3 + 2] = smoothedMeshFaceArrays[i * 3 + 1];
            }
        }
    }
}