MRClass/Assets/cloudPoint/VyncGetData.cs

using System;
using System.Collections;
using System.Collections.Generic;
using System.Diagnostics;
using System.Runtime.InteropServices;
using System.Threading;
using System.Threading.Tasks;
using UnityEngine;
using Debug = UnityEngine.Debug;

public class VyncGetData
{
    // Start is called before the first frame update
    private const string dllPath = "InVisionKinectDll";
    //private const string dllPath = "InVisionKinectReceiver";
    
    //public string HOST = "172.16.15.121";
    public string HOST = "192.168.50.85";
    //private const string HOST = "192.168.1.100";
    private const int PORT = 6666;
    private const int IMAGE_WIDTH = 640;
    private const int IMAGE_HEIGHT = 576;

    private char _deviceId=(char)0;
    private char _dataType=(char)0;
    private uint _frameIndex=0;
    private uint _headDataNum=0;
    private float[] _headDataArray;    
    private uint _bodyDataNum=0;
    private float[] _bodyDataArray;
    private uint _TriNum=0;
    public short[] _TriArray=new short[]{};
    private bool _bDumpFrame=false;
    private bool _bCompressed=true;
    
    public float headPointSize=0.005f;
    public float bodyPointSize=0.01f;
    public float timeDelay=0f;
    private Stopwatch st = new Stopwatch(); 

    private const int BUFFER_STRIDE = 9;   
    

    [DllImport(dllPath)]
    private static extern int invision_kinect_init(string host, int port, int width, int height);

    [DllImport(dllPath)]
    //INVISION_KINECT_CLIENT_API int invision_kinect_fetchOneFrameData_unity(uint8_t& outDeviceId, uint8_t& outDataType, uint32_t& outFrameIndex,
   // uint32_t& outVertexNum, float* outVertexArray, uint32_t &outSecondaryVertexNum, float *outSecondaryVertexArray,
   //     uint32_t& outMeshTrianglesNum, uint16_t* outVertexIndexArray, bool bDumpFrame, bool bCompressed);
  //DeviceId 设备号 DataType发送类型 0点云  1mesh   FrameIndex传输的帧索引 headDataNum单帧的头部点数量 bodyDataNum单帧的身体点数量 headDataArray，bodyDataArray数据集6位一组 x y z r g b TriNum mesh顶点数量 TriArray mesh顶点数组 bDumpFrame是否本地保存单帧数据 bCompressed是否压缩
    private static extern int invision_kinect_fetchOneFrameData_unity( ref char DeviceId, ref char DataType, ref uint FrameIndex, ref uint headDataNum, float[] headDataArray, ref uint bodyDataNum, float[] bodyDataArray, ref uint TriNum,ref short[] TriArray, bool bDumpFrame, bool bCompressed);

    [DllImport(dllPath)]
    private static extern int invision_kinect_destroy();

    private Thread thread;

    private static VyncGetData instance;  
    
    public static VyncGetData GetInstance()    
    {  
        if (instance == null)    
        {  
            instance = new VyncGetData();    
        }    
        return instance;   
    }  
    
    public int limitPoints = 30000; //单shader的绘制长度
    public int topologyNum=3;//拓展成三个
    public int headPointGroups; //head随需要的组件数量
    public int bodyPointGroups; //body随需要的组件数量
    //public int totalPoint;

    //序列化数组
    public List<Vector3[]> headpostionList;
    public List<Color[]> headcolorList;
    public List<Vector3[]> bodypostionList;
    public List<Color[]> bodycolorList;
    //只取6W个点
    public Vector3[] curListImg;
    public Color[] curListCol;
    public int offsetZ=1;

    public void StartThread()
    {
       // totalPoint = limitPoints*topologyNum;
       // numPointGroups = Mathf.CeilToInt(totalPoint * 1.0f / limitPoints * 1.0f);  
       headpostionList=new List<Vector3[]>();
       headcolorList=new List<Color[]>();       
       bodypostionList=new List<Vector3[]>();
       bodycolorList=new List<Color[]>();
      
        //curListImg=new Vector3[totalPoint];
        //curListCol=new Color[totalPoint];
        invision_kinect_initParas();
    }

    private void invision_kinect_initParas()
    {               
        Debug.Log("invision_kinect_initParas");        
        invision_kinect_init(HOST, PORT, IMAGE_WIDTH, IMAGE_HEIGHT);
        _headDataArray = new float[IMAGE_WIDTH * IMAGE_HEIGHT];
        _bodyDataArray = new float[IMAGE_WIDTH * IMAGE_HEIGHT];
        //imageColor = new Int32[IMAGE_WIDTH * IMAGE_HEIGHT];
        //imageBufferSize = IMAGE_WIDTH * IMAGE_HEIGHT * BUFFER_STRIDE;
       // imageBuffer = new Byte[imageBufferSize];    //0~5 bytes:xyz, 6~8 bytes:rgb
        //receiveBufferSize = 0;
        
        ThreadStart childRef=new ThreadStart(readCloudPoint);
        thread=new Thread(childRef);
        thread.Start();
    }

    //判断是否更新
    public bool isUpdatae=false;
    public bool isLost=true;
    private void readCloudPoint()
    {
        while (true)
        {
            if (isUpdatae)
            {
                Thread.Sleep(5);
                continue;
            }
                //private static extern int invision_kinect_fetchOneFrameData(int Index,int BufferSize,Out float[] PosArray,Out Byte[] ColData,Out int TNum, Out short[] TDatas);
              st.Start(); //开始计时
           //  Debug.Log("通讯开始 ");
             int mDataType = invision_kinect_fetchOneFrameData_unity( ref _deviceId,ref _dataType, ref _frameIndex, ref _headDataNum, _headDataArray,ref _bodyDataNum, _bodyDataArray, ref _TriNum,ref _TriArray, _bDumpFrame, _bCompressed);
//             Debug.Log("开始数据处理：_dataType"+(int)(_dataType)+"  "+Convert.ToInt32(_dataType)+"   _deviceId:"+ (int)(_deviceId)+"  "+Convert.ToInt32(_deviceId)+"  __headDataNum:"+Convert.ToInt32(_headDataNum)+"  ___bodyDataNum:"+Convert.ToInt32(_bodyDataNum));
             if (mDataType == 0)
             {
                 if (Convert.ToInt32(_dataType) == 1)
                 {
                     //点云数据
                     if(_headDataNum<=0)
                     {
                        // Debug.Log("丢失更新1");
                         isLost=true;
                         isUpdatae = false;
                         st.Stop();
                         timeDelay = st.ElapsedMilliseconds;
                         // Debug.Log("Running time " + st.ElapsedMilliseconds.ToString() + "ms");
                         st.Reset();
                    
                         Thread.Sleep(5);
                     }else
                     {
                         //Debug.Log("更新");
                         UpdateCloudData();
                         st.Stop();
                         timeDelay = st.ElapsedMilliseconds;
                         // Debug.Log("Running time " + st.ElapsedMilliseconds.ToString() + "ms");
                         st.Reset();
                         isUpdatae = true;
                         isLost=false;
                         Thread.Sleep(5);
                     }
                 }
                 else
                 {
                     //mesh数据
                   /*  Debug.Log("更新更新");
                     isLost=true;
                     st.Stop();
                     timeDelay = st.ElapsedMilliseconds;
                     // Debug.Log("Running time " + st.ElapsedMilliseconds.ToString() + "ms");
                     st.Reset();
                     isUpdatae = false;
                     Thread.Sleep(5);*/
                 }
              
             }
             else 
             {
                 //mesh数据
                 //Debug.Log("丢失更新2");
                 //CloudDataclear();
                 isLost=true;
                 isUpdatae = false;
                 Thread.Sleep(5);
             }
             
        }
    }
    
    private void UpdateCloudData()
    {
        headPointGroups = Mathf.CeilToInt((int)_headDataNum * topologyNum *1.0f/ limitPoints*1.0f );  
        bodyPointGroups = Mathf.CeilToInt((int)_bodyDataNum * topologyNum *1.0f/ limitPoints*1.0f);

        if (headpostionList.Count < headPointGroups)
        {
            int len=headpostionList.Count;
            for (int i = 0; i < headPointGroups-len; i++)
            {
                headpostionList.Add(new Vector3[limitPoints]);
                headcolorList.Add(new Color[limitPoints]);
            }
        }
        if (bodypostionList.Count < bodyPointGroups)
        {
            int len=bodypostionList.Count;
            for (int i = 0; i < bodyPointGroups-len; i++)
            {
                bodypostionList.Add(new Vector3[limitPoints]);
                bodycolorList.Add(new Color[limitPoints]);
            }
        }
        
        int pointIndex = 0;
        long lostCount = 0;
        int arylndex=0;
        int indexTemp=0;
        float x, y, z, r, g, b;
         for (int i = 0; i < (int)_headDataNum; i++)
         {

             x = -_headDataArray[i * 6];
             y = -_headDataArray[i * 6+1]-0.4f;
             z = _headDataArray[i * 6+2];
             r = _headDataArray[i * 6+5];
             g = _headDataArray[i * 6+4];
             b = _headDataArray[i * 6+3];
             
           
             arylndex = pointIndex / (limitPoints);
             indexTemp = pointIndex - arylndex * limitPoints;
             headcolorList[arylndex][indexTemp] = new Color(r, g, b, 1.0f);
             headpostionList[arylndex][indexTemp] = new Vector3(x - headPointSize, y, z+offsetZ);
             headcolorList[arylndex][indexTemp+1] = new Color(r, g, b, 1.0f);
             headpostionList[arylndex][indexTemp+1] = new Vector3(x , y + headPointSize, z+offsetZ);
             headcolorList[arylndex][indexTemp+2] = new Color(r, g, b, 1.0f);
             headpostionList[arylndex][indexTemp+2] = new Vector3(x + headPointSize, y, z+offsetZ);
            //curListImg[i]=new Vector3(-x , -y, z);
           // curListCol[i]=new Color(r, g, b, 1.0f);
             pointIndex+=3;
         }
         //Debug.Log(headpostionList[0][0].z+"~~~~~~~~~~~~~");

         if (pointIndex < headPointGroups * limitPoints)
         {
             arylndex = pointIndex/(limitPoints);
             
             Array.Clear( headcolorList[arylndex], pointIndex-limitPoints*arylndex, limitPoints*(arylndex+1)-pointIndex);
             Array.Clear( headpostionList[arylndex], pointIndex-limitPoints*arylndex, limitPoints*(arylndex+1)-pointIndex);
             //Array.Clear(curListCol, pointIndex, totalPoint-pointIndex );
            // Array.Clear(curListImg, pointIndex, totalPoint -pointIndex);
         }

         int bodyPointIndex = 0;
         for (int i = 0; i < (int)_bodyDataNum; i++)
         {

             x =-_bodyDataArray[i * 6];
             y = -_bodyDataArray[i * 6+1]-0.4f;
             z = _bodyDataArray[i * 6+2];
             r = _bodyDataArray[i * 6+5];
             g = _bodyDataArray[i * 6+4];
             b = _bodyDataArray[i * 6+3];
             
           
             arylndex = bodyPointIndex / (limitPoints);
             indexTemp = bodyPointIndex - arylndex * limitPoints;
             bodycolorList[arylndex][indexTemp] = new Color(r, g, b, 1.0f);
             bodypostionList[arylndex][indexTemp] = new Vector3(x - bodyPointSize, y, z+offsetZ);
             bodycolorList[arylndex][indexTemp+1] = new Color(r, g, b, 1.0f);
             bodypostionList[arylndex][indexTemp+1] = new Vector3(x , y + bodyPointSize, z+offsetZ);
             bodycolorList[arylndex][indexTemp+2] = new Color(r, g, b, 1.0f);
             bodypostionList[arylndex][indexTemp+2] = new Vector3(x + bodyPointSize, y, z+offsetZ);
             //curListImg[i]=new Vector3(-x , -y, z);
             // curListCol[i]=new Color(r, g, b, 1.0f);
             bodyPointIndex+=3;
            
         }
         
         if (bodyPointIndex < bodyPointGroups * limitPoints)
         {
             arylndex = bodyPointIndex/(limitPoints);
             
             Array.Clear( bodycolorList[arylndex], bodyPointIndex-limitPoints*arylndex, limitPoints*(arylndex+1)-bodyPointIndex);
             Array.Clear( bodypostionList[arylndex], bodyPointIndex-limitPoints*arylndex, limitPoints*(arylndex+1)-bodyPointIndex);
             
             //Array.Clear(curListCol, pointIndex, totalPoint-pointIndex );
             // Array.Clear(curListImg, pointIndex, totalPoint -pointIndex);
         }
         
         //Debug.Log("Cached headpoint: " + pointIndex + ", Cached bodypoint:  " + bodyPointIndex);
    }
    
    
    public void destory()
    {
        Debug.Log("OnDestroy invision_kinect_destroy Data");
        thread.Abort();
        invision_kinect_destroy();
    }

   
}