MRClass/Assets/cloudPoint/VyncCelect.cs

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

public class VyncCelect
{
    // Start is called before the first frame update
    private const string dllPath = "KinectDataTransfer";
    
    public string HOST = "192.168.1.11";
    private const int PORT = 66666;

    private int _deviceId=0;
    private byte[] __KinectSNArray = new byte[512];
    private byte[] _outDeviceSN=new byte[12];
    private byte[] _curDeviceSN=new byte[12];
    private byte _dataType=0;
    private uint _frameIndex=0;
    public uint _headDataNum=0;
    public float[] _headDataArray;    
    private uint _bodyDataNum=0;
    private uint _TriNum=0;
    public ushort[] _TriArray;
    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;
    public   string _curDeviceSN_NUM;
    public  int _curDeviceSN_Array_NUM;
    public  byte _curDeviceNUM=0;
    public  string _AllDeviceSN_NUM;

    [DllImport(dllPath)]
    private static extern int kinect_init_render(string host, int port);

    [DllImport(dllPath)]
    private  static extern int kinect_fetch_all_devices_allocated(ref  byte DataNum,bool dumpFrame);

    [DllImport(dllPath)]
    private static extern int kinect_list_available_devices(ref byte DataNum,byte[] outAvailableDevicesSN);

    [DllImport(dllPath)]
    private static extern int kinect_bind_device(byte[] deviceSN);

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

    [DllImport(dllPath)]
    private static extern int kinect_fetch_one_device_allocated( byte[] outDeviceSN, ref byte DataType, ref uint FrameIndex, ref uint headDataNum, float[] headDataArray, ref uint bodyDataNum, ref uint TriNum, ushort[] TriArray, int deviceId);

  //DeviceId 设备号 DataType发送类型 0点云  1mesh   FrameIndex传输的帧索引 headDataNum单帧的头部点数量 bodyDataNum单帧的身体点数量 headDataArray，bodyDataArray数据集6位一组 x y z r g b TriNum mesh顶点数量 TriArray mesh顶点数组 bDumpFrame是否本地保存单帧数据 bCompressed是否压缩

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

    private Thread thread;

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

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

    public string fileStr;
    public float[] _headDataArray1;    

    public void StartThread()
    {
       headpostionList=new List<Vector3[]>();
       headcolorList=new List<Color[]>();       
       invision_kinect_initParas();
    }

    private void invision_kinect_initParas()
    {               
        //0 000472192412 216 1 17720 8308
        //_headDataNum = 17720;
       // _bodyDataNum = 8308;
        Debug.Log("invision_kinect_initParas");        
        _headDataArray = new float[300000];
        _TriArray=new ushort[1];
        int res= kinect_init_render(HOST, PORT);
        
        if (0 == res)
        {
            kinect_list_available_devices(ref _curDeviceNUM,__KinectSNArray);
            _curDeviceSN_Array_NUM = (int)_curDeviceNUM;
            Debug.Log("xjytest获取sn数量：" + _curDeviceSN_Array_NUM);
            byte[] currSN = new byte[12];
            for (int i = 0; i < _curDeviceSN_Array_NUM; i++)
            {
                Array.Copy(__KinectSNArray, i * 12, currSN, 0, 12);
                string strSN = Encoding.ASCII.GetString(currSN);
                _AllDeviceSN_NUM += strSN + ",";
                Debug.Log("xjytest获取sn No." + i + " available sn=" + strSN);
            }
            if (_curDeviceSN_Array_NUM > 0)
            {
                byte[] firstSN = new byte[12];
                Array.Copy(__KinectSNArray, firstSN, 12);
                _curDeviceSN = firstSN;
                string strSN = Encoding.ASCII.GetString(firstSN);
                Debug.Log("xjytest绑定的 sn=" + strSN);
                int resSn = kinect_bind_device(firstSN);
                Debug.Log("xjytest绑定sn的结果 res=" + resSn);
                _curDeviceSN_NUM = strSN;
            }
        }
        
        ThreadStart childRef=new ThreadStart(readCloudPoint);
        thread=new Thread(childRef);
        thread.Start();
    }

    //判断是否更新
    public bool isUpdatae=false;
    public bool isLost=true;
    private int devices;
    private bool isLock=false;
    private void readCloudPoint()
    {
        while (true)
        {
            if (isUpdatae)
            {
                //Debug.Log("xjytest还没有绘制，等待");

                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(); //开始计时
              /*
              //测试
               UpdateCloudData(true);
              st.Stop();
              timeDelay = st.ElapsedMilliseconds;
              // Debug.Log("Running time " + st.ElapsedMilliseconds.ToString() + "ms");
              st.Reset();
              isUpdatae = true;
              isLost=false;
              Thread.Sleep(5);
              continue;*/
             //Debug.Log("通讯开始");
             
             isLost=true;
             isUpdatae = false;
             kinect_fetch_all_devices_allocated(ref _curDeviceNUM,false);
             devices = (int)_curDeviceNUM;
       
             for (int i = 0; i < devices; i++)
             {
                int mDataType = kinect_fetch_one_device_allocated(  _outDeviceSN,ref _dataType, ref _frameIndex,
                    ref _headDataNum, _headDataArray,ref _bodyDataNum,
                    ref _TriNum, _TriArray, i);
                /*if (i == 1)
                {
                    _headDataArray = _headDataArray1;
                    _headDataNum = 17720;
                     _bodyDataNum = 8308;
                }*/

                
//                 Debug.Log("开始数据处理："+"devices:"+devices+"   _outDeviceSN"+Encoding.ASCII.GetString(_outDeviceSN)+"   _dataType:"+ (int)(_dataType)+"   _deviceId:"+ i+"  __headDataNum:"+Convert.ToInt32(_headDataNum)+"  _headDataArray:"+_headDataArray.Length+"  ___bodyDataNum:"+Convert.ToInt32(_bodyDataNum));
                 if (mDataType == 0)
                 {
                    // Debug.Log("xjytest当前绑定的sn设备:"+ Encoding.ASCII.GetString(_outDeviceSN));
                     if (Convert.ToInt32(_dataType) == 1||Convert.ToInt32(_dataType) == 0)
                     {
                         if (devices-1==i)
                         {
                             UpdateCloudData(true,i+1);
                             isUpdatae = true;
                             isLost=false;
                             st.Stop();
                             timeDelay = st.ElapsedMilliseconds;
                             // Debug.Log("Running time " + st.ElapsedMilliseconds.ToString() + "ms");
                             st.Reset();
                             Thread.Sleep(5);
                         }
                         else
                         {
                             UpdateCloudData(false,i+1);
                         }
                     }
                 }
             }
        }
    }

    //临时数据
    private int TempHeadNum=0;
    private int pointIndex = 0;
    private int arylndex=0;
    private int indexTemp=0;
    private int startIndex ;
    private int lostIndex = 0;

    private void UpdateCloudData(bool isOver,int index)
    {
        TempHeadNum += (int) _headDataNum+(int) _bodyDataNum;
       
        headPointGroups = Mathf.CeilToInt(TempHeadNum * topologyNum *1.0f/ limitPoints*1.0f );  

        //Debug.Log("xjytest:headPointGroups: "+headPointGroups+"   headpostionList.Count"+headpostionList.Count+"   TempHeadNum:"+TempHeadNum+"   index:"+index);
        if (headpostionList.Count < headPointGroups)
        {
            int len=headpostionList.Count;
            for (int l = 0; l < headPointGroups-len; l++)
            {
                headpostionList.Add(new Vector3[limitPoints]);
                headcolorList.Add(new Color[limitPoints]);
            }
        }else if (isOver&&headpostionList.Count > headPointGroups)
        {
            //Debug.Log("xjytest:删除: " +headpostionList.Count+" 第" +(headPointGroups)+"开始删除"+(headpostionList.Count-headPointGroups));

            headpostionList.RemoveRange(headPointGroups,headpostionList.Count-headPointGroups);
            headcolorList.RemoveRange(headPointGroups,headcolorList.Count-headPointGroups);
        }

        float x, y, z, r, g, b;
        //startIndex = pointIndex/3;
         for (int i=0; i < (int) _headDataNum+(int) _bodyDataNum; i++)
         {
             

             x = -_headDataArray[i * 6]+((int)(index/2))*(index%2==0?1:-1);
             y = -_headDataArray[i * 6+1];
             z = _headDataArray[i * 6+2]+ 1;
             r = _headDataArray[i * 6+5];
             g = _headDataArray[i * 6+4];
             b = _headDataArray[i * 6+3];
             //fileStr += (-x).ToString() + "," + (-y).ToString() + "," + (z).ToString() + "," + b.ToString() + "," +g.ToString() + "," + r.ToString() + ",";
            arylndex = pointIndex / (limitPoints);
            indexTemp = pointIndex - arylndex * limitPoints;
             if (i < (int) _headDataNum)
             {
                 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);
             }
             else
             {
                 headcolorList[arylndex][indexTemp] = new Color(r, g, b, 1.0f);
                 headpostionList[arylndex][indexTemp] = new Vector3(x - bodyPointSize, y, z+offsetZ);
                 headcolorList[arylndex][indexTemp+1] = new Color(r, g, b, 1.0f);
                 headpostionList[arylndex][indexTemp+1] = new Vector3(x , y + bodyPointSize, z+offsetZ);
                 headcolorList[arylndex][indexTemp+2] = new Color(r, g, b, 1.0f);
                 headpostionList[arylndex][indexTemp+2] = new Vector3(x + bodyPointSize, y, z+offsetZ);
             }
             //Debug.Log("xjytest:lndex:"+index+"   aryindex"+arylndex+"    indexTemp:"+indexTemp+"   "+(-x).ToString() + "," + (-y).ToString() + "," + (z).ToString() + "," + b.ToString() + "," +g.ToString() + "," + r.ToString() );
             pointIndex+=topologyNum;
             if(r==0&g==0&b==0)
             lostIndex++;
         }
         
         //Debug.Log("xjytest:pointIndex:"+pointIndex+"    lostIndex:"+lostIndex+"   floatZ:"+((int)(index/2))*(index%2==0?1:-1));

         if (isOver)
         {
             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);
                // Debug.Log("xjytest:headPointGroups clear:"+ (pointIndex-limitPoints*arylndex)+"    "+(limitPoints*(arylndex+1)-pointIndex));
                 restValue();
             }
            
         }
        
         
         //Debug.Log("Cached headpoint: " + pointIndex + ", Cached bodypoint:  " + bodyPointIndex);
    }


    public void restValue()
    {
        TempHeadNum = 0;
        pointIndex = 0;
        lostIndex = 0;
        fileStr = "";
    }
    
    public void destory()
    {
        Debug.Log("OnDestroy xjytest invision_kinect_destroy Data");
        int res = kinect_unbind_device();
        Debug.Log("xjytest after invision_kinect_unbind_device res=" + res);

        thread.Abort();
        kinect_disconnect();
    }

   
}