GHZSDKXR/Runtime/HandTracking/Scripts/Kernel/NativeSwapManager.cs

#define GrayCamera

using UnityEngine;
using System.Runtime.InteropServices;
using System;
using EZXR.Glass.SixDof;
using EZXR.Glass.Core;

namespace EZXR.Glass.Inputs
{
    /// <summary>
    /// 确保此脚本执行顺序在ARHandSDK中为首位，因为整个ARHandSDK的数据刷新在此脚本的Update中
    /// </summary>
    [ScriptExecutionOrder(-53)]
    /// <summary>
    /// 与底层算法交互
    /// </summary>
    public class NativeSwapManager : MonoBehaviour
    {
        #region 单例
        private static NativeSwapManager instance;
        public static NativeSwapManager Instance
        {
            get
            {
                return instance;
            }
        }
        #endregion

        #region 与算法交互的struct定义
        [StructLayout(LayoutKind.Sequential)]
        public struct Point2
        {
            public float x;
            public float y;

            public override string ToString()
            {

                return "(" + x + "," + y + ")";
            }
        }

        [StructLayout(LayoutKind.Sequential)]
        public struct Point3
        {
            public float x;
            public float y;
            public float z;

            public Point3(float x, float y, float z)
            {
                this.x = x;
                this.y = y;
                this.z = z;
            }
            public Point3(Vector3 point)
            {
                x = point.x;
                y = point.y;
                z = point.z;
            }
            public override string ToString()
            {

                return "(" + x + "," + y + "," + z + ")";
            }
        }

        [StructLayout(LayoutKind.Sequential, Pack = 1)]
        public struct HandTrackingData
        {
            public int version;
            /// <summary>
            /// 检测范围内是否存在手
            /// </summary>
            [MarshalAs(UnmanagedType.I1)]
            public bool isTracked;
            public float confidence;
            /// <summary>
            /// 用来确定这个数据是左手的还是右手的
            /// </summary>
            public HandType handType;
            /// <summary>
            /// 手势类型
            /// </summary>
            public GestureType gestureType;
            /// <summary>
            /// 手部节点的数量
            /// </summary>
            public uint handJointsCount;
            /// <summary>
            /// 0是拇指根节点，3是大拇指指尖点，4是食指根节点，16是小拇指根节点向手腕延伸点（不到手腕），21是掌心点，22是手腕靠近大拇指，23是手腕靠近小拇指，24是手腕中心点
            /// </summary>
            [MarshalAs(UnmanagedType.ByValArray, SizeConst = 25)]
            public HandJointData[] handJointData;
            /// <summary>
            /// 模型手拉伸的比例参数
            /// </summary>
            public float model_scale;
            /// <summary>
            /// 数组id从0到4分别代表大拇指到小拇指
            /// 值：0代表伸直，1代表完全弯曲
            /// </summary>
            [MarshalAs(UnmanagedType.ByValArray, SizeConst = 5)]
            public float[] fingler_curl;
            [MarshalAs(UnmanagedType.ByValArray, SizeConst = 10)]
            public int[] reserved;
        }

        [StructLayout(LayoutKind.Sequential)]
        public struct HandJointData
        {
            public int version;
            public HandJointType handJointType;
            public Mat4f handJointPose;
            [MarshalAs(UnmanagedType.ByValArray, SizeConst = 16)]
            public int[] reserved;
        }

        [StructLayout(LayoutKind.Sequential)]
        public struct Mat4f
        {
            public Vector4f col0;
            public Vector4f col1;
            public Vector4f col2;
            public Vector4f col3;
        }

        public struct Vector4f
        {
            public float x, y, z, w;
        }
        #endregion

        private partial struct NativeAPI
        {
#if EZXRCS
            [DllImport(NativeConsts.NativeLibrary)]
            public static extern void getHandPoseData([Out, In] IntPtr handposedata, int size);

            [DllImport(NativeConsts.NativeLibrary)]
            public static extern void getHandPoseDataWithHeadPose([Out, In] IntPtr handposedata, int handposedataSize, float[] headpose, int headposeSize, double headposetimestamp);
#else
            [DllImport(NativeConsts.NativeLibrary)]
            public static extern void getHandPoseData([Out, In] IntPtr handposedata);

            [DllImport(NativeConsts.NativeLibrary)]
            public static extern void getHandPoseDataWithHeadPose([Out, In] IntPtr handposedata, float[] headpose, double headposetimestamp);
#endif
        }

        /// <summary>
        /// 
        /// </summary>
        /// <param name="mode">0是GPU，1是DSP</param>
        /// <returns></returns>
        [DllImport(NativeConsts.NativeLibrary)]
        public static extern bool setHandRunMode(int mode);

        public static void filterPoint(ref Point3 point, int id)
        {
            //if (!Application.isEditor)
            //{
            //    filter_point(ref point, id);
            //}
        }

        #region 变量定义
        /// <summary>
        /// HandTrackingData更新的话会通知到外部
        /// </summary>
        public static event Action<HandTrackingData[]> OnHandTrackingDataUpdated;

        /// <summary>
        /// 用于从算法c++获得检测结果，这个指针是在非托管区域开辟的内存的指针（内存大小是2个HandTrackingData长度）
        /// </summary>
        public static IntPtr ptr_HandTrackingData;
        public static IntPtr ptr_HandTrackingData0;
        /// <summary>
        /// 手部追踪数据
        /// </summary>
        HandTrackingData[] handTrackingData = new HandTrackingData[2];
        int handTrackingDataLength;
        #endregion


        private void Awake()
        {
            instance = this;
        }

        void Start()
        {
            //为了接收算法的结果，在非托管内存区域开辟长度为2个HandTrackingData的内存
            handTrackingDataLength = Marshal.SizeOf(typeof(HandTrackingData));
            ptr_HandTrackingData = Marshal.AllocHGlobal(handTrackingDataLength * 2);
            ptr_HandTrackingData0 = new IntPtr(ptr_HandTrackingData.ToInt64() + handTrackingDataLength);

            //用于将Marshal开辟的非托管区域的所有值清零以避免内存区域原数据造成的影响
            HandTrackingData handTrackingDataTemp = new HandTrackingData();
            Marshal.StructureToPtr(handTrackingDataTemp, ptr_HandTrackingData, false);
            Marshal.StructureToPtr(handTrackingDataTemp, ptr_HandTrackingData0, false);
            handTrackingData[0] = new HandTrackingData();
            handTrackingData[1] = new HandTrackingData();
        }

        void Update()
        {
            if (!Application.isEditor && (ARFrame.SessionStatus == EZVIOState.EZVIOCameraState_Tracking || ARFrame.SessionStatus == EZVIOState.EZVIOCameraState_Track_Limited))
            {
                Vector3 position = Vector3.zero;
                Quaternion rotation = Quaternion.identity;
                //Debug.Log("开始get_nreal_result_predict");
                //getHandPoseData(ptr_HandTrackingData);

                position = HMDPoseTracker.Instance.Head.position;
                rotation = HMDPoseTracker.Instance.Head.rotation;

                float[] headPosearr = new float[]{
                        position.x,position.y,position.z,
                        rotation.x,rotation.y,rotation.z,rotation.w };

#if EZXRCS
                NativeAPI.getHandPoseDataWithHeadPose(ptr_HandTrackingData, handTrackingDataLength * 2, headPosearr, headPosearr.Length, ARFrame.HeadPoseTimestamp);
#else
                    NativeAPI.getHandPoseDataWithHeadPose(ptr_HandTrackingData, headPosearr, ARFrame.HeadPoseTimestamp);
#endif
                //Debug.Log("得到数据get_nreal_result_predict");

                //Debug.Log("开始解析handTrackingData");
                handTrackingData[0] = Marshal.PtrToStructure<HandTrackingData>(ptr_HandTrackingData);
                handTrackingData[1] = Marshal.PtrToStructure<HandTrackingData>(ptr_HandTrackingData0);
                //Debug.Log("解析handTrackingData完毕");

                //#region 用于测试
                //NativeSwapManager.Mat4f mat4 = handTrackingData[0].handJointData[21].handJointPose;
                //Matrix4x4 m = new Matrix4x4();
                //m.SetColumn(0, new Vector4(mat4.col0.x, mat4.col0.y, mat4.col0.z, mat4.col0.w));
                //m.SetColumn(1, new Vector4(mat4.col1.x, mat4.col1.y, mat4.col1.z, mat4.col1.w));
                //m.SetColumn(2, new Vector4(mat4.col2.x, mat4.col2.y, mat4.col2.z, mat4.col2.w));
                //m.SetColumn(3, new Vector4(mat4.col3.x, mat4.col3.y, mat4.col3.z, mat4.col3.w));
                //if (!m.ValidTRS())
                //{
                //    return;
                //}
                //Vector3 pos = m.GetColumn(3);
                //Debug.Log("Launcher - Test => headPosition:" + position.ToString("0.00") + ", headRotation:" + rotation.ToString("0.00000") + ", HeadPoseTimestamp:" + ARFrame.HeadPoseTimestamp + ", handPosition:" + pos.ToString("0.00"));
                //#endregion

                if (OnHandTrackingDataUpdated != null)
                {
                    OnHandTrackingDataUpdated(handTrackingData);
                }
            }
        }
    }
}