﻿using UnityEngine;
using System;
using System.Text;
using System.Collections.Generic;
using UnityEngine.UI;
using UnityEngine.EventSystems;
using FOHEART.GlovePlugin;

/*
RT: right tracker
LT: left tracker

RH: right hand
LH: left hand
 */

namespace FOHEART_Mocap
{
    using KHHS32 = FoheartSkeletonName.kinemHumanHandsSkeleton32Index;

    public partial class FoheartGloveH1Pose : MonoBehaviour
    {
        public class RealFrameCounter
        {
            public double lastRxTMs = 0;
            public double currentRxTMs = 0;
            public int lastRxCounter = 0;
            public int currentRxCounter = 0;
        };

        [System.Serializable]
        public class RawEulerLocInspector
        {
          

            public Vector3 RH_Loc;
            public Quaternion RH_Quat;
            public Vector3 RH_Euler;
            public Vector3 LH_Loc;
            public Quaternion LH_Quat;
            public Vector3 LH_Euler;
            /*是否绘制关节小球与小球之间的连线*/
            public bool isDrawHandJoint = false;
            /*开关变量 指示当前是否已经绘制了关节小球与小球之间的连线*/
            private bool isJointShow = false;

            [HideInInspector]
            public GameObject[] figureJointObjects = new GameObject[(int)KHHS32.NumOfBones];

            [HideInInspector]
            public Vector3[] handJointVector = new Vector3[(int)KHHS32.NumOfBones];

            public void Init()
            {
                if (isDrawHandJoint)
                {
                    for (int i = 0; i < (int)KHHS32.NumOfBones; i++)
                    {
                        figureJointObjects[i] = GameObject.CreatePrimitive(PrimitiveType.Sphere);
                        figureJointObjects[i].transform.localScale = new Vector3(0.01f, 0.01f, 0.01f);
                        figureJointObjects[i].SetActive(false);
                    }
                }
            }

            public void GenerateJointVector(int index, float x, float y, float z)
            {
                handJointVector[index].Set(x, y, z);
            }

            public void setJointShow(bool b)
            {
                if (isJointShow == b)
                    return;
                if (b)
                {
                    isJointShow = true;
                    for (int i = 0; i < (int)KHHS32.NumOfBones; i++)
                    {
                        figureJointObjects[i].SetActive(true);
                    }
                }
                else
                {
                    for (int i = 0; i < (int)KHHS32.NumOfBones; i++)
                    {
                        figureJointObjects[i].SetActive(false);
                    }
                    isJointShow = false;
                }
            }


            public void setJointPosition(int index, Quaternion handQ, float root_x, float root_y, float root_z)
            {
                int figureIndex = (int)KHHS32.RightHand + index;
                Vector3 figureLocInUnity = new Vector3();
                figureLocInUnity.Set(
                    handJointVector[index][0],
                    handJointVector[index][1],
                    handJointVector[index][2]
                    );


                /*根据角度旋转点的坐标*/
                Vector3 endLoc = new Vector3(0, 0, 0);

                {
                    /*
                     * 为什么要加这个tempQ还不明白 但是不加的话 最终小球是与手套的模型垂直的 
                       这里左右手的还不一样
                     */
                    Quaternion tempQ = Quaternion.identity;
                    Quaternion currentQ = Quaternion.identity;

                    //先转会到原点  再根据新的角度转到目的地
                    if (
                        (index >= (int)KHHS32.RightHand) && (index <= (int)KHHS32.RightHandPinky3)
                        )
                    {
                        tempQ.Set(0.0f, 0.0f, 0.0f, 1.0f);
                        tempQ.eulerAngles = new Vector3(-90, 0, -90);
                      
                        currentQ = handQ * tempQ * Quaternion.Inverse(RH_Quat);
                    }

                    if (
                        (index >= (int)KHHS32.LeftHand) && (index <= (int)KHHS32.LeftHandPinky3)
                        )
                    {
                        tempQ.Set(0.0f, 0.0f, 0.0f, 1.0f);
                        tempQ.eulerAngles = new Vector3(-90, 0, 90);
                        currentQ = handQ * tempQ * Quaternion.Inverse(LH_Quat);
                    }


                    float[] currentQArray = { currentQ.w, currentQ.x, currentQ.y, currentQ.z };
                    float[] pos = { 0, 0, 0 };
                    float[] initialLocation = { figureLocInUnity.x, figureLocInUnity.y, figureLocInUnity.z };
                    float[] resultLoc = PointRotWithQuat(currentQArray, pos, initialLocation);
                    resultLoc[0] += root_x;
                    resultLoc[1] += root_y;
                    resultLoc[2] += root_z;

                    endLoc.Set(resultLoc[0], resultLoc[1], resultLoc[2]);
                }

                figureJointObjects[figureIndex].transform.position = endLoc;
            }

            public void drawFigureLine()
            {
                setJointShow(true);
                for (int i = 0; i < (int)KHHS32.NumOfBones; i++)
                {
                    int fatherIndex = FoheartSkeletonName.JointEulerFather30[i];
                    if (fatherIndex == -1)
                    {
                        continue;
                    }
                    int childIndex = i;
                    Debug.DrawLine(
                                  figureJointObjects[fatherIndex].transform.position,
                                  figureJointObjects[childIndex].transform.position,
                                  Color.red
                                  );
                }
            }
        };

        [System.Serializable]
        public class TrackerBinding
        {
            [Tooltip("右手Tracker的序列号，以\"LHR-\"四个字符开头，比如LHR-A0AE37F3")]
            public string RH_TrackerSN;
         
            [ReadOnly]
            public int RH_TrackerIdx = -1;
            [Tooltip("右手位置使用Tracker的位置，默认选中")]
            public bool RH_LocTracker = true;
            [Tooltip("右手旋转使用Tracker的位置，默认选中")]
            public bool RH_RotTracker = true;

            [Tooltip("左手Tracker的序列号，以\"LHR-\"四个字符开头，比如LHR-B6F6DFDC")]
            public string LH_TrackerSN;
            [ReadOnly]
            public int LH_TrackerIdx = -1;
            [Tooltip("左手位置使用Tracker的位置，默认选中")]
            public bool LH_LocTracker = true;
            [Tooltip("左手旋转使用Tracker的位置，默认选中")]
            public bool LH_RotTracker = true;

            [HideInInspector]
            public Transform RH_Trans;
            [HideInInspector]
            public Quaternion RH_Q;
            [HideInInspector]
            public Vector3 RH_Loc;
            [HideInInspector]
            public Transform LH_Trans;
            [HideInInspector]
            public Quaternion LH_Q;
            [HideInInspector]
            public Vector3 LH_Loc;
        };

        [System.Serializable]
        public class MotionInspector
        {
            [HideInInspector]
            /*模型的中心点在底面与表面中间*/
            //public Vector3 trackerCenterBias = new Vector3(0, 0, -0.01f);
            public Vector3 trackerCenterBias = new Vector3(0, 0, 0);

            [HideInInspector]
            /*这里处理的是模型自身的问题 将模型在vr中的旋转与现实中统一起来 不需要换轴 只是初始的旋转有问题*/
            public Vector3 modelInVRConvert = new Vector3(180.0f, 0, 180.0f);

            /*头盔的角度 位置*/
            [ReadOnly]
            public Vector3 hmdEuler;
            [ReadOnly]
            public Vector3 hmdLoc;
            [Tooltip("调试使用的物体，包括指示当前头盔向前方向的线，tracker中心点，贯穿手腕的圆柱体")]
            public bool debugObjShow=false;
            [ReadOnly]
            public GameObject hmdYawObj;

            /*tracker模型*/
            [Tooltip("右手的Tracker模型，把设置为右手的Tracker拖拽过来")]
            public GameObject RT_Obj;
            [ReadOnly]
            [Tooltip("右手的Tracker中心点，自动创建，仅供调试使用")]
            public GameObject RT_CenterObj;
            [ReadOnly]
            [Tooltip("右手的贯穿手套中心点的圆柱体，自动创建，仅供调试使用")]
            public GameObject RH_cylinderObj;//圆柱
            [ReadOnly]
            [Tooltip("右手Tracker的欧拉角")]
            public Vector3 RT_Euler;

            [Tooltip("左手的Tracker模型，把设置为左手的Tracker拖拽过来")]
            public GameObject LT_Obj;
            [ReadOnly]
            [Tooltip("左手的Tracker中心点，自动创建，仅供调试使用")]
            public GameObject LT_CenterObj;
            [ReadOnly]
            [Tooltip("左手的贯穿手套中心点的圆柱体，自动创建，仅供调试使用")]
            public GameObject LH_cylinderObj;
            [ReadOnly]
            [Tooltip("左手Tracker的欧拉角")]
            public Vector3 LT_Euler;

            /*
             * 左右手腕最终的显示位置
             */
            [ReadOnly]
            public Vector3 LH_Root_Display_Loc;
            [HideInInspector]
            public Quaternion LH_Root_Display_LocalQ;
            [HideInInspector]
            public Quaternion LH_Root_Display_GlobalQ;

            [ReadOnly]
            public Vector3 RH_Root_Display_Loc;
            [HideInInspector]
            public Quaternion RH_Root_Display_LocalQ;
            [HideInInspector]
            public Quaternion RH_Root_Display_GlobalQ;

            public void Init()
            {
                Vector3 scale = new Vector3(0.01f, 0.01f, 0.01f);
                Vector3 cylinderScale = new Vector3(0.01f, 0.05f, 0.01f);

                if (debugObjShow)
                {
                    LT_CenterObj = GameObject.CreatePrimitive(PrimitiveType.Cube);
                    LT_CenterObj.transform.localScale = scale;
                }
                if (debugObjShow)
                {
                    LH_cylinderObj = GameObject.CreatePrimitive(PrimitiveType.Cylinder);
                    LH_cylinderObj.transform.localScale = cylinderScale;
                }
                if (debugObjShow)
                {
                    RT_CenterObj = GameObject.CreatePrimitive(PrimitiveType.Cube);
                    RT_CenterObj.transform.localScale = scale;
                }
                if (debugObjShow)
                {
                    RH_cylinderObj = GameObject.CreatePrimitive(PrimitiveType.Cylinder);
                    RH_cylinderObj.transform.localScale = cylinderScale;
                }

                Vector3 scale2 = new Vector3(0.01f, 0.01f, 10f);
                if(debugObjShow)
                {
                hmdYawObj = GameObject.CreatePrimitive(PrimitiveType.Cube);
                hmdYawObj.transform.localScale = scale2;
                }

            }


        };

        [System.Serializable]
        public class GloveRelativeTrackerCtrl
        {
            [Range(0, 360), ReadOnly]
            public int L_x = 0;
            [Range(0, 360), ReadOnly]
            public int L_y = 0;
            [Range(0, 360), ReadOnly]
            public int L_z = 0;

            [Range(0, 360), ReadOnly]
            public int R_x = 0;
            [Range(0, 360), ReadOnly]
            public int R_y = 0;
            [Range(0, 360), ReadOnly]
            public int R_z = 0;

           
            [Tooltip("手腕的直径 单位（米）")]
            public float wristDiameter_m = 0.06f;
            [Tooltip("tracker 沿手臂方向到手腕的距离 单位（米）")]
            public float wristToTracker_m=0.1f;

            /*
             将上面两者作为两个向量首尾相连 将手腕顺着向量移动到末端即可
             上面的两个值不可能绝对准确，如果想要求取绝对准确的值，需要进行特殊动作校准求取
             */

        };

        [System.Serializable]
        public class FrameProperties
        {
            [Tooltip("帧序号，来自MotionVenus")]
            [ReadOnly]
            public uint frameNumber;
            [ReadOnly]
            public bool dataPending ;
            [Tooltip("正在运行MotionVenus的PC机的IP地址")]
            [ReadOnly]
            public string serverIP;
            [Tooltip("正在运行MotionVenus的PC机的发送端口")]
            [ReadOnly]
            public uint serverPort;
            [ReadOnly]
            public string motionVenusSDKVersion;
            [Tooltip("数据协议的版本号")]
            [ReadOnly]
            public int prototolVersion;
            [ReadOnly]
            public string skeletonTemplate;
            [Tooltip("勾选：将在Console窗口输出程序的调试信息\n不勾选：隐藏程序的调试信息")]
            public bool printDebugLog = false;
        }
        [Tooltip("true：当前自动探测数据流，ActorName无效 false：不自动探测数据流，只接收ActorName指定名称的数据流。")]
        public bool autoDetectActorName = false;
        [Tooltip("当前数据流的名称，需要从MotionVenus端复制过来。\n 设置方法：在MotionVenus左侧设备列表栏中，右键复制需要转发的套装列表表头名称，粘贴到此处。")]
        public string ActorName;
        [Tooltip("数据帧的相关属性")]
        public FrameProperties frameProperties;
        public int LH_Action;
        public int RH_Action;
        [HideInInspector]
        public RawEulerLocInspector rawEulerLocInspector;
        public TrackerBinding trackerBinding;
        public MotionInspector motionInspector;


        [HideInInspector]
        public ActorFrameData actorFrameData = new ActorFrameData();

        [Tooltip("设置两只手的整体方向")]
        [HideInInspector]
        public RotationAdjust rotationAdjust;
        public GloveRelativeTrackerCtrl gloveRelativeTrackerCtrl;

        public ColorIdentifier colorIdentifier;

        // world new add
        Quaternion modelRootRot;
        GameObject modelRootObject;
        List<Quaternion> orignalRots = new List<Quaternion>();
        List<Quaternion> orignalParentRots = new List<Quaternion>();
        /*start skeleton & trans*/
        [HideInInspector]
        public Transform[] skeletonTransforms = new Transform[(int)KHHS32.NumOfBones];
        [HideInInspector]
        public GameObject[] skeletonObjects = new GameObject[(int)KHHS32.NumOfBones];
        [HideInInspector]
        public Transform[] endPointTransforms = new Transform[(int)FoheartSkeletonName.kinemHumanHandsEndPoint12Index.NumOfBones];
        [HideInInspector]
        public GameObject[] endPointObjects = new GameObject[(int)FoheartSkeletonName.kinemHumanHandsEndPoint12Index.NumOfBones];
        /*end*/
        RealFrameCounter realFrameCounter = new RealFrameCounter();
        double applicationStartMs = 0;
        [HideInInspector]
        /*计算骨骼的初始偏移*/
        private bool isOriginSkeletonAngleCalculated = false;

        public bool isValid { get; private set; }

        public FoheartGloveH1Pose()
        {
            ActorName = "Actor1";
        }

        void Awake()
        {
            Application.targetFrameRate = 60;
            motionInspector.Init();

            rawEulerLocInspector.Init();

            LoadAllSkeletons();
            LoadAllEndPoint();

            if (-1 == CheckCondition())
            {
                return;
            }

            FoheartNetFrameManager fnf = FoheartNetFrameManager.GetInstance();
            fnf.addModel(this);

            Transform LeftHandTrans = FindBone((int)KHHS32.LeftHand);
            if (LeftHandTrans == null)
            {
                Debug.LogWarning("Please bind lefthand in inspector panel.");
                //return;
            }

            Transform RightHandTrans = FindBone((int)KHHS32.RightHand);
            if (RightHandTrans == null)
            {
                Debug.LogWarning("Please bind righthand in inspector panel.");
                //return;
            }

            {
                int rightHandIdx = (int)FoheartSkeletonName._kinemHumanSkeleton32Index.KHS32_RightHand;
                if (skeletonObjects[rightHandIdx] != null)
                {
                    GameObject obj = skeletonObjects[rightHandIdx];
                    Quaternion tempQ = new Quaternion(0.0f, 0.0f, 0.0f, 1.0f);
                    // tempQ.eulerAngles = new Vector3(-90, 0, 0);//vr glove
                    tempQ.eulerAngles = new Vector3(0,-90,  0);//透明手

                    //obj.transform.localRotation = tempQ * obj.transform.localRotation;//局部旋转
                    //obj.transform.rotation = tempQ * obj.transform.rotation;//全局旋转
                }

            }
            {
                int leftHandIdx = (int)FoheartSkeletonName._kinemHumanSkeleton32Index.KHS32_LeftHand;
                if (skeletonObjects[leftHandIdx] != null)
                {
                    GameObject obj = skeletonObjects[leftHandIdx];
                    Quaternion tempQ = new Quaternion(0.0f, 0.0f, 0.0f, 1.0f);
                    // tempQ.eulerAngles = new Vector3(-90, 0, 0);
                    tempQ.eulerAngles = new Vector3(0, -90, 0);
                    //obj.transform.localRotation = tempQ * obj.transform.localRotation;
                   // obj.transform.rotation = tempQ * obj.transform.rotation;//全局旋转

                }

            }

            /*找到模型整个的旋转*/

            if (frameProperties.printDebugLog)
            {
                Debug.Log("CaluateOrignalRot from Awake.");
            }
            CaluateOrignalRot();

            TimeSpan ts = new TimeSpan(DateTime.Now.Ticks);
            applicationStartMs = ts.TotalMilliseconds;
            InvokeRepeating("CalculateRealFrameRate", 1, 1);
        }

        //检查是否已经指定骨骼绑定文件
        int CheckCondition()
        {
            int ret = -1;
            if (ActorName.Length == 0)
            {
                //该模型接受的动作角色名称
                Debug.LogError("ActorName not specified!");
                return ret;
            }
            ret = 0;
            return ret;
        }

        SteamVR_TrackedObjectPlus trackedObjs = null;

        // 手势与动作枚举映射
        private Dictionary<byte, Gesture> actionToEnumMap = new Dictionary<byte, Gesture>();

        //准备开始
        void Start()
        {
            actionToEnumMap.Add(23, Gesture.MARK_POINT);
            actionToEnumMap.Add(26, Gesture.GO_POINT);
            actionToEnumMap.Add(13, Gesture.BULLET);
            actionToEnumMap.Add(14, Gesture.FIRE);
            actionToEnumMap.Add(16, Gesture.GRAB_ITEM);
            actionToEnumMap.Add(27, Gesture.DEPART);
            actionToEnumMap.Add(24, Gesture.PRESS);
            actionToEnumMap.Add(255, Gesture.NONE);

            trackedObjs = gameObject.AddComponent<SteamVR_TrackedObjectPlus>();
            InitNameAndBoneWithoutAdd0();
            CaluateOrignalRot();
        }

        public void InitNameAndBoneWithoutAdd(ref ActorFrameData data)
        {
            if (frameProperties.printDebugLog)
            {
                Debug.Log("initNameAndBoneWithoutAdd");
            }
            string boneOriEuler = "Rx skeleton53RotBiasEuler:\r\n";
            for (int i = 0; i < (int)KHHS32.NumOfBones; i++)
            {
                boneOriEuler += FoheartSkeletonName.kinemHumanSkeleton32[i].ToString();
                boneOriEuler += ",";
                boneOriEuler += data.khhs32PosAtt.skeletons[i].euler_biasDegree[0].ToString();
                boneOriEuler += ",";
                boneOriEuler += data.khhs32PosAtt.skeletons[i].euler_biasDegree[1].ToString();
                boneOriEuler += ",";
                boneOriEuler += data.khhs32PosAtt.skeletons[i].euler_biasDegree[2].ToString();
                boneOriEuler += "\r\n";

                if (i == 0)
                {
                    continue;
                }

                if (skeletonTransforms[(int)i] != null)
                {
                    Vector3 addEuler = new Vector3(
                       data.khhs32PosAtt.skeletons[i].euler_biasDegree[0],
                         data.khhs32PosAtt.skeletons[i].euler_biasDegree[1],
                         data.khhs32PosAtt.skeletons[i].euler_biasDegree[2]
                        );

                    Quaternion tempQ = new Quaternion(0.0f, 0.0f, 0.0f, 1.0f);
                    tempQ.eulerAngles = new Vector3(addEuler.x, addEuler.y, addEuler.z);

                    Transform BoneT = FindBone((uint)i);
                    BoneT.localRotation = tempQ * BoneT.localRotation;
                }
            }
            if (frameProperties.printDebugLog)
            {
                Debug.Log(boneOriEuler);
            }

            /*找到所有控制的骨骼转换控制体*/
            for (int i = 0; i < (int)KHHS32.NumOfBones; i++)
            {
                if (skeletonTransforms[i] == null)
                {
                    continue;
                }

                /*transforms*/
                Vector3 boneEulerRadius = skeletonTransforms[i].localRotation.eulerAngles;
                Vector3 boneEulerDegree = new Vector3(
                   (float)(boneEulerRadius.x),
                   (float)(boneEulerRadius.y),
                   (float)(boneEulerRadius.z)
                    );

                if (frameProperties.printDebugLog)
                {
                    /*输出所有骨骼的初始角度，全局坐标，相对于u3d世界坐标系*/
                    // Debug.Log(kinemHumanSkeleton53[i] + " transforms EulerAngle:" + boneEulerDegree.x + "," + boneEulerDegree.y + "," + boneEulerDegree.z);
                }
            }
        }
        public void InitNameAndBoneWithoutAdd0()
        {
            if (frameProperties.printDebugLog)
            {
                Debug.Log("initNameAndBoneWithoutAdd");
            }
            string boneOriEuler = "Rx skeleton53RotBiasEuler:\r\n";
            for (int i = 0; i < (int)KHHS32.NumOfBones; i++)
            {
              

                if (i == 0)
                {
                    continue;
                }

                if (skeletonTransforms[(int)i] != null)
                {
                    Vector3 addEuler = new Vector3(
                      0,0,0
                        );

                    Quaternion tempQ = new Quaternion(0.0f, 0.0f, 0.0f, 1.0f);
                    tempQ.eulerAngles = new Vector3(addEuler.x, addEuler.y, addEuler.z);

                    Transform BoneT = FindBone((uint)i);
                    BoneT.localRotation = tempQ * BoneT.localRotation;
                }
            }
            if (frameProperties.printDebugLog)
            {
                Debug.Log(boneOriEuler);
            }

            /*找到所有控制的骨骼转换控制体*/
            for (int i = 0; i < (int)KHHS32.NumOfBones; i++)
            {
                if (skeletonTransforms[i] == null)
                {
                    continue;
                }

                /*transforms*/
                Vector3 boneEulerRadius = skeletonTransforms[i].localRotation.eulerAngles;
                Vector3 boneEulerDegree = new Vector3(
                   (float)(boneEulerRadius.x),
                   (float)(boneEulerRadius.y),
                   (float)(boneEulerRadius.z)
                    );

                if (frameProperties.printDebugLog)
                {
                    /*输出所有骨骼的初始角度，全局坐标，相对于u3d世界坐标系*/
                    // Debug.Log(kinemHumanSkeleton53[i] + " transforms EulerAngle:" + boneEulerDegree.x + "," + boneEulerDegree.y + "," + boneEulerDegree.z);
                }
            }
        }
        void OnEnable()
        {

        }

        void OnDisable()
        {
            isValid = false;
            FoheartNetFrameManager m1 = FoheartNetFrameManager.GetInstance();
            m1.OnDisable();
        }
        /*更新左右手tracker的位置与旋转 */
        void UpdateTrackerRotLoc()
        {
            float trackerHeight = 0.02f;//Tracker厚度2cm
            float trackerHalfHeight = trackerHeight / 2;
            /*
             hmd的角度
             */
            motionInspector.hmdEuler = getHandTrackerRotByIndex(0).eulerAngles;
            motionInspector.hmdLoc = getHandTrackerPosByIndex(0);
            /*获取头盔朝向的方向，只取朝向不要俯仰和翻滚，因为用户校准的时候可能会低头，取Y的欧拉角，再用欧拉角生成一个方向四元数*/
            float hmdY = motionInspector.hmdEuler.y;
            Quaternion hmdYQ = new Quaternion(0.0f, 0.0f, 0.0f, 1.0f);
            hmdYQ.eulerAngles = new Vector3(0, hmdY, 0);
            if (motionInspector.hmdYawObj)
            {
                motionInspector.hmdYawObj.transform.rotation = hmdYQ;
                motionInspector.hmdYawObj.transform.localPosition = new Vector3(motionInspector.hmdLoc.x, 0, motionInspector.hmdLoc.z);
            }

            if (motionInspector.LT_Obj)
            {
                motionInspector.LT_Obj.transform.localPosition = getHandTrackerPosBySN(trackerBinding.LH_TrackerSN, out trackerBinding.LH_TrackerIdx);
                motionInspector.LT_Obj.transform.eulerAngles = getHandTrackerRotBySN(trackerBinding.LH_TrackerSN, out trackerBinding.LH_TrackerIdx).eulerAngles;
                /*这里处理的是模型自身的问题 将模型在vr中的旋转与现实中统一起来*/
                Quaternion tempQ = new Quaternion(0.0f, 0.0f, 0.0f, 1.0f);
                tempQ.eulerAngles = new Vector3(motionInspector.modelInVRConvert.x, motionInspector.modelInVRConvert.y, motionInspector.modelInVRConvert.z);
                motionInspector.LT_Obj.transform.rotation = motionInspector.LT_Obj.transform.rotation * tempQ;
                /*矫正tracker的中心点*/
                Vector3 endLoc = new Vector3(0, 0, 0);
                {
                    /*start 减掉厚度的影响*/
                    Quaternion currentQ = motionInspector.LT_Obj.transform.rotation;
                    float[] currentQArray = { currentQ.w, currentQ.x, currentQ.y, currentQ.z };
                    float[] pos = { 0, 0, 0 };
                    float[] initialLocation = { motionInspector.trackerCenterBias.x, motionInspector.trackerCenterBias.y, motionInspector.trackerCenterBias.z };
                    float[] resultLoc = PointRotWithQuat(currentQArray, pos, initialLocation);
                    resultLoc[0] += motionInspector.LT_Obj.transform.localPosition.x;
                    resultLoc[1] += motionInspector.LT_Obj.transform.localPosition.y;
                    resultLoc[2] += motionInspector.LT_Obj.transform.localPosition.z;

                    endLoc.Set(resultLoc[0], resultLoc[1], resultLoc[2]);
                }

                if (motionInspector.LT_CenterObj)
                {
                    motionInspector.LT_CenterObj.transform.localPosition = endLoc;
                }
                motionInspector.LT_Obj.transform.localPosition = endLoc;
                /*end*/

                /*旋转和位置复制到binding中*/
                trackerBinding.LH_Q = motionInspector.LT_Obj.transform.rotation;

                {
                    Quaternion currentQ = motionInspector.LT_Obj.transform.rotation;
                    float[] currentQArray = { currentQ.w, currentQ.x, currentQ.y, currentQ.z };
                    float[] pos = { 0, 0, 0 };
                   
                    float[] initialLocation = { 0, gloveRelativeTrackerCtrl.wristToTracker_m, trackerHalfHeight+ gloveRelativeTrackerCtrl.wristDiameter_m / 2 };
                    float[] resultLoc = PointRotWithQuat(currentQArray, pos, initialLocation);
                    resultLoc[0] += motionInspector.LT_Obj.transform.localPosition.x;
                    resultLoc[1] += motionInspector.LT_Obj.transform.localPosition.y;
                    resultLoc[2] += motionInspector.LT_Obj.transform.localPosition.z;
                    endLoc.Set(resultLoc[0], resultLoc[1], resultLoc[2]);
                }
                if(motionInspector.LH_cylinderObj)
                {
                    motionInspector.LH_cylinderObj.transform.localPosition = endLoc;
                    motionInspector.LH_cylinderObj.transform.rotation = motionInspector.LT_Obj.transform.rotation;

                }

                trackerBinding.LH_Loc = endLoc;
            }
            if (motionInspector.RT_Obj)
            {
                motionInspector.RT_Obj.transform.localPosition = getHandTrackerPosBySN(trackerBinding.RH_TrackerSN, out trackerBinding.RH_TrackerIdx);
                motionInspector.RT_Obj.transform.eulerAngles = getHandTrackerRotBySN(trackerBinding.RH_TrackerSN, out trackerBinding.RH_TrackerIdx).eulerAngles;
                /*这里处理的是模型自身的问题 将模型在vr中的旋转与现实中统一起来*/
                Quaternion tempQ = new Quaternion(0.0f, 0.0f, 0.0f, 1.0f);
                tempQ.eulerAngles = new Vector3(motionInspector.modelInVRConvert.x, motionInspector.modelInVRConvert.y, motionInspector.modelInVRConvert.z);
                motionInspector.RT_Obj.transform.rotation = motionInspector.RT_Obj.transform.rotation * tempQ;
                /*start 减掉厚度的影响*/
                Vector3 endLoc = new Vector3(0, 0, 0);
                {
                    Quaternion currentQ = motionInspector.RT_Obj.transform.rotation;
                    float[] currentQArray = { currentQ.w, currentQ.x, currentQ.y, currentQ.z };
                    float[] pos = { 0, 0, 0 };
                    float[] initialLocation = { motionInspector.trackerCenterBias.x, motionInspector.trackerCenterBias.y, motionInspector.trackerCenterBias.z };
                    float[] resultLoc = PointRotWithQuat(currentQArray, pos, initialLocation);
                    resultLoc[0] += motionInspector.RT_Obj.transform.localPosition.x;
                    resultLoc[1] += motionInspector.RT_Obj.transform.localPosition.y;
                    resultLoc[2] += motionInspector.RT_Obj.transform.localPosition.z;
                    endLoc.Set(resultLoc[0], resultLoc[1], resultLoc[2]);
                }

                if (motionInspector.RT_CenterObj)
                {
                    motionInspector.RT_CenterObj.transform.localPosition = endLoc;
                }
              
                    motionInspector.RT_Obj.transform.localPosition = endLoc;
                

                /*end*/
                /*旋转和位置复制到binding中*/
                trackerBinding.RH_Q = motionInspector.RT_Obj.transform.rotation;

                {
                    Quaternion currentQ = motionInspector.RT_Obj.transform.rotation;
                    float[] currentQArray = { currentQ.w, currentQ.x, currentQ.y, currentQ.z };
                    float[] pos = { 0, 0, 0 };
                    //float[] initialLocation = { 0.04f, 0, 0.03f };
                    float[] initialLocation = { 0, -gloveRelativeTrackerCtrl.wristToTracker_m, trackerHalfHeight + gloveRelativeTrackerCtrl.wristDiameter_m / 2 };
                    float[] resultLoc = PointRotWithQuat(currentQArray, pos, initialLocation);
                    resultLoc[0] += motionInspector.RT_Obj.transform.localPosition.x;
                    resultLoc[1] += motionInspector.RT_Obj.transform.localPosition.y;
                    resultLoc[2] += motionInspector.RT_Obj.transform.localPosition.z;
                    endLoc.Set(resultLoc[0], resultLoc[1], resultLoc[2]);
                }
                if(motionInspector.RH_cylinderObj)
                {
                    motionInspector.RH_cylinderObj.transform.localPosition = endLoc;
                    motionInspector.RH_cylinderObj.transform.rotation = motionInspector.RT_Obj.transform.rotation;

                }

                trackerBinding.RH_Loc = endLoc;
            }
        }


        /// <summary>
        /// 左手触碰到的物体
        /// </summary>
        private GameObject leftHandAdjacentItem = null;
        /// <summary>
        /// 右手触碰到的物体
        /// </summary>
        private GameObject rightHandAdjacentItem = null;

        [Tooltip("左手碰撞器")]
        public GameObject leftHandTouchObject;
        [Tooltip("右手碰撞器")]
        public GameObject rightHandTouchObject;
        [Tooltip("手势动作事件响应处理器")]
        public List<FOGestureActionAdatper> gestureActionHandlers;


        void FixedUpdate()
        {
            // 处理手势动作
            handleGestureAction();

            UpdateTrackerRotLoc();

            if (!frameProperties.dataPending)
            {
                return;
            }

            realFrameCounter.currentRxCounter++;
            ApplyBoneTransRot(actorFrameData);
            frameProperties.dataPending = false;
        }

        public void setAdjacentItem(GameObject adjacentItem, bool isLeftHand)
        {
            if (isLeftHand)
            {
                leftHandAdjacentItem = adjacentItem;
            }
            else
            {
                rightHandAdjacentItem = adjacentItem;
            }
        }

        /// <summary>
        /// 手势动作处理逻辑
        /// </summary>
        private void handleGestureAction()
        {
            // 当前左手的手势动作index
            byte leftHandGestureIndex = actorFrameData.khhs32PosAtt.hdr.leftHandAction;
            // 当前右手的手势动作index
            byte rightHandGestureIndex = actorFrameData.khhs32PosAtt.hdr.rightHandAction;

            // 判断是否有需要调用的处理器
            if (gestureActionHandlers == null || gestureActionHandlers.Count == 0)
            {
                return;
            }

            // 当前左手手势枚举
            Gesture leftHandGesture = Gesture.NONE; 
            if (actionToEnumMap.ContainsKey(leftHandGestureIndex))
            {
                leftHandGesture = actionToEnumMap[leftHandGestureIndex];
            }

            // 当前右手手势枚举
            Gesture rightHandGesture = Gesture.NONE; 
            if (actionToEnumMap.ContainsKey(rightHandGestureIndex))
            {
                rightHandGesture = actionToEnumMap[rightHandGestureIndex];
            }

            // 手势动作处理器
            foreach (IFOGestureAction act in gestureActionHandlers)
            {
                act.onHandGesture(leftHandGesture, leftHandTouchObject, leftHandAdjacentItem, rightHandGesture, rightHandTouchObject, rightHandAdjacentItem);
                //act.onHandGesture(testGesture, leftHandTouchObject, leftHandAdjacentItem, rightHandGesture, rightHandTouchObject, rightHandAdjacentItem);
            }

            leftHandAdjacentItem = null;
            rightHandAdjacentItem = null;
        }

        public Gesture testGesture = Gesture.BULLET;


        /*给骨骼设置实际的角度*/
        public void ApplyBoneTransRot(ActorFrameData data)
        {
            ASCIIEncoding encoding = new ASCIIEncoding();
            int actualNameLen = 0;
            for (int i = 0; i < data.khhs32PosAtt.hdr.AvatarNameLength; i++)
            {
                if (data.khhs32PosAtt.hdr.AvatarName[i] != 0x00)
                {
                    actualNameLen++;
                }
            }
            string strActorName = encoding.GetString(data.khhs32PosAtt.hdr.AvatarName, 0, actualNameLen);
            if (strActorName != ActorName)
            {
                Debug.Log(gameObject + " Drop:" + data.khhs32PosAtt.hdr.frameNumber);
                return;
            }


            // Debug.Log("ApplyBoneTransRot:"+data.local_KHS53PosAttitude_managed.hdr.frameNumber);
            // return;
            try
            {
                System.Net.IPAddress remoteAddr = new System.Net.IPAddress(data.khhs32PosAtt.remoteIP);

                frameProperties.serverIP = remoteAddr.ToString();
                frameProperties.serverPort = data.khhs32PosAtt.remotePort;

                int unity3D_SkeletonTemplateIndex = data.khhs32PosAtt.hdr.unity3D_SkeletonTemplate;

                if (unity3D_SkeletonTemplateIndex < FoheartSkeletonName.u3dSkeletonTemplate.Length)
                {
                    frameProperties.skeletonTemplate = FoheartSkeletonName.u3dSkeletonTemplate[unity3D_SkeletonTemplateIndex];
                }

                frameProperties.prototolVersion = data.khhs32PosAtt.hdr.protocolVersion;
                frameProperties.frameNumber = data.khhs32PosAtt.hdr.frameNumber;

                uint motionVenusSDKVersion = data.khhs32PosAtt.sdkVersion;
                byte mainVersion = (byte)((motionVenusSDKVersion & 0xFF000000) >> 24);
                byte subVersion = (byte)((motionVenusSDKVersion & 0x00FF0000) >> 16);
                byte modifyVersion = (byte)((motionVenusSDKVersion & 0x0000FF00) >> 8);

                frameProperties.motionVenusSDKVersion = mainVersion.ToString() + "." + subVersion.ToString() + "." + modifyVersion.ToString();

                LH_Action = data.khhs32PosAtt.hdr.leftHandAction;
                RH_Action = data.khhs32PosAtt.hdr.rightHandAction;

            }
            catch (InvalidCastException e)
            {
                Debug.Log("Exception:" + e.ToString());
            }

            {
                /*计算手指关节的向量*/
                for (int i = (int)KHHS32.RightHand; i <= (int)KHHS32.RightHandPinky3; i++)
                {
                    int fatherIndex = (int)KHHS32.RightHand;

                    int childIndex = i;

                    Vector3 fatherLoc = ConvertLocFromMotionVenusToUnity3D(
                        data.khhs32PosAtt.skeletons[fatherIndex].position_meter[0],
                          data.khhs32PosAtt.skeletons[fatherIndex].position_meter[1],
                          data.khhs32PosAtt.skeletons[fatherIndex].position_meter[2]
                        );

                    Vector3 childLoc = ConvertLocFromMotionVenusToUnity3D(
                      data.khhs32PosAtt.skeletons[childIndex].position_meter[0],
                        data.khhs32PosAtt.skeletons[childIndex].position_meter[1],
                        data.khhs32PosAtt.skeletons[childIndex].position_meter[2]
                      );

                    rawEulerLocInspector.GenerateJointVector(i,
                        childLoc.x - fatherLoc.x,
                        childLoc.y - fatherLoc.y,
                       childLoc.z - fatherLoc.z
                        );

                    rawEulerLocInspector.RH_Quat = ConvertQFromMotionVenusToUnity3D(
                        new Quaternion(
                                       data.khhs32PosAtt.skeletons[(int)KHHS32.RightHand].quat_xyzw[0],
                                       data.khhs32PosAtt.skeletons[(int)KHHS32.RightHand].quat_xyzw[1],
                                       data.khhs32PosAtt.skeletons[(int)KHHS32.RightHand].quat_xyzw[2],
                                       data.khhs32PosAtt.skeletons[(int)KHHS32.RightHand].quat_xyzw[3])
                                       );
                }
                for (int i = (int)KHHS32.LeftHand; i <= (int)KHHS32.LeftHandPinky3; i++)
                {
                    int fatherIndex = (int)KHHS32.LeftHand;

                    int childIndex = i;

                    Vector3 fatherLoc = ConvertLocFromMotionVenusToUnity3D(
                        data.khhs32PosAtt.skeletons[fatherIndex].position_meter[0],
                          data.khhs32PosAtt.skeletons[fatherIndex].position_meter[1],
                          data.khhs32PosAtt.skeletons[fatherIndex].position_meter[2]
                        );

                    Vector3 childLoc = ConvertLocFromMotionVenusToUnity3D(
                      data.khhs32PosAtt.skeletons[childIndex].position_meter[0],
                        data.khhs32PosAtt.skeletons[childIndex].position_meter[1],
                        data.khhs32PosAtt.skeletons[childIndex].position_meter[2]
                      );

                    rawEulerLocInspector.GenerateJointVector(i,
                        childLoc.x - fatherLoc.x,
                        childLoc.y - fatherLoc.y,
                       childLoc.z - fatherLoc.z
                        );

                    rawEulerLocInspector.LH_Quat = ConvertQFromMotionVenusToUnity3D(
                      new Quaternion(
                                     data.khhs32PosAtt.skeletons[(int)KHHS32.LeftHand].quat_xyzw[0],
                                     data.khhs32PosAtt.skeletons[(int)KHHS32.LeftHand].quat_xyzw[1],
                                     data.khhs32PosAtt.skeletons[(int)KHHS32.LeftHand].quat_xyzw[2],
                                     data.khhs32PosAtt.skeletons[(int)KHHS32.LeftHand].quat_xyzw[3])
                                     );
                }
            }

            /*
             * 检测到有偏移欧拉角的设定
             * 就重新初始化模型
             */

            if (isOriginSkeletonAngleCalculated)
            {

            }
            else
            {
                FoheartSkeletonName.U3D_GloveSkeleton_Template_TypeDef skeletonTemplate =
                    (FoheartSkeletonName.U3D_GloveSkeleton_Template_TypeDef)data.khhs32PosAtt.hdr.unity3D_SkeletonTemplate;
                switch (skeletonTemplate)
                {
                    case FoheartSkeletonName.U3D_GloveSkeleton_Template_TypeDef.U3D_GLOVE_SKELETON_TEMPLATE_STRAIGHT:
                    case FoheartSkeletonName.U3D_GloveSkeleton_Template_TypeDef.U3D_GLOVE_SKELETON_TEMPLATE_THUMBOPEN:
                    case FoheartSkeletonName.U3D_GloveSkeleton_Template_TypeDef.U3D_GLOVE_SKELETON_TEMPLATE_STEAMVRGLOVE:
                        {
                          //  InitNameAndBoneWithoutAdd(ref data);
                         //   CaluateOrignalRot();
                            rotationAdjust.thumbX = 0;
                            rotationAdjust.thumbY = 0;
                            rotationAdjust.thumbZ = 0;

                        };
                        break;
                    default:
                        {
                            break;
                        }
                }
                isOriginSkeletonAngleCalculated = true;
                /*将模型初始的偏移 归为到T-Pose*/
            }
            /*start 位置 仅设置手腕的位置 */
            Transform leftOrRightRootTrans = FindBone((int)KHHS32.LeftHand);
            Vector3 LeftWristLocInUnity = ConvertLocFromMotionVenusToUnity3D(
                       data.khhs32PosAtt.skeletons[(int)KHHS32.LeftHand].position_meter[0],
                       data.khhs32PosAtt.skeletons[(int)KHHS32.LeftHand].position_meter[1],
                       data.khhs32PosAtt.skeletons[(int)KHHS32.LeftHand].position_meter[2]
                       );

            rawEulerLocInspector.LH_Loc = LeftWristLocInUnity;
            if (trackerBinding.LH_LocTracker)
            {
                leftOrRightRootTrans.position = trackerBinding.LH_Loc;
            }
            else
            {
               // leftOrRightRootTrans.position = LeftWristLocInUnity;
            }
            motionInspector.LH_Root_Display_Loc = leftOrRightRootTrans.position;
            //-----start right
            //Debug.Log(leftOrRightRootTrans.position);
            leftOrRightRootTrans = FindBone((int)KHHS32.RightHand);
            Vector3 RightWristLocInUnity = ConvertLocFromMotionVenusToUnity3D(
                      data.khhs32PosAtt.skeletons[(int)KHHS32.RightHand].position_meter[0],
                      data.khhs32PosAtt.skeletons[(int)KHHS32.RightHand].position_meter[1],
                      data.khhs32PosAtt.skeletons[(int)KHHS32.RightHand].position_meter[2]
                                             );
            rawEulerLocInspector.RH_Loc = RightWristLocInUnity;
            if (trackerBinding.RH_LocTracker)
            {
                leftOrRightRootTrans.position = trackerBinding.RH_Loc;
                //leftOrRightRootTrans.position = motionInspector.RH_testObj.transform.localPosition;

            }
            else
            {
                //leftOrRightRootTrans.position = RightWristLocInUnity;
            }
            motionInspector.RH_Root_Display_Loc = leftOrRightRootTrans.position;

            /*end*/
            /*start */
            int currentSkeletonIndex = 0;
            for (uint i = 0; i < (int)KHHS32.NumOfBones; i++)
            {
                Transform BoneT = FindBone(i);

                if (BoneT)
                {
                    /*左手的 角度*/
                    if (
                        (i == (int)KHHS32.LeftHand) //&& trackerBinding.LH_RotTracker
                        )
                    {
                        /*start 给左手添加一个角度*/
                        Quaternion leftHandFinalQ;
                        Quaternion tempQ = new Quaternion(0.0f, 0.0f, 0.0f, 1.0f);
                        tempQ.eulerAngles = new Vector3(gloveRelativeTrackerCtrl.L_x, gloveRelativeTrackerCtrl.L_y, gloveRelativeTrackerCtrl.L_z);
                        // leftHandFinalQ = trackerBinding.LH_Q * tempQ;
                        //leftHandFinalQ = BoneT.transform.rotation* tempQ;
                        leftHandFinalQ = tempQ;
                        /*end*/
                        BoneT.localRotation = leftHandFinalQ;
                        motionInspector.LH_Root_Display_GlobalQ = leftHandFinalQ;
                        motionInspector.LT_Euler = BoneT.localRotation.eulerAngles;
                        continue;
                    }
                    /*右手的 角度*/
                    if (
                        (i == (int)KHHS32.RightHand) //&& trackerBinding.RH_RotTracker
                       )
                    {
                        /*start 给右手添加一个角度*/
                        Quaternion rightHandFinalQ;
                        Quaternion tempQ = new Quaternion(0.0f, 0.0f, 0.0f, 1.0f);
                        tempQ.eulerAngles = new Vector3(gloveRelativeTrackerCtrl.R_x, gloveRelativeTrackerCtrl.R_y, gloveRelativeTrackerCtrl.R_z);
                        //rightHandFinalQ = trackerBinding.RH_Q * tempQ;
                        rightHandFinalQ =  tempQ;
                        /*end*/

                       BoneT.localRotation = rightHandFinalQ;
                        motionInspector.RH_Root_Display_GlobalQ = rightHandFinalQ;

                        motionInspector.RT_Euler = BoneT.localRotation.eulerAngles;
                        continue;
                    }
#if false
                    float figureDistance = pointBias(endPointTransforms[(int)FoheartSkeletonName.kinemHumanHandsEndPoint12Index.LeftHandThumbEndPoint],
                        endPointTransforms[(int)FoheartSkeletonName.kinemHumanHandsEndPoint12Index.LeftHandIndexEndPoint]
                        );
                    Debug.Log("figureDistance:" + figureDistance);
#endif

                    // world new add remove
                    Quaternion srcQ = ConvertQFromMotionVenusToUnity3D(
                        new Quaternion(
                                       data.khhs32PosAtt.skeletons[i].quat_xyzw[0],
                                       data.khhs32PosAtt.skeletons[i].quat_xyzw[1],
                                       data.khhs32PosAtt.skeletons[i].quat_xyzw[2],
                                       data.khhs32PosAtt.skeletons[i].quat_xyzw[3])
                                       );
                    Quaternion orignalBoneRot = orignalRots[(int)i];
                    Quaternion orignalParentBoneRot = orignalParentRots[(int)i];
                    Quaternion usedQ = Quaternion.Inverse(orignalParentBoneRot) * srcQ * orignalParentBoneRot;
                    Quaternion finalBoneQ = usedQ * orignalBoneRot;
                   

                    if (i == (int)KHHS32.LeftHand)
                    {
                        motionInspector.LH_Root_Display_LocalQ = finalBoneQ;

                        rawEulerLocInspector.LH_Euler = BoneT.localRotation.eulerAngles;

                    }
                    else if (i == (int)KHHS32.RightHand)
                    {
                        motionInspector.RH_Root_Display_LocalQ = finalBoneQ;

                        rawEulerLocInspector.RH_Euler = BoneT.localRotation.eulerAngles;
                    }
                    else {
                        /*局部旋转*/
                        BoneT.localRotation = finalBoneQ;
                    }
                    //BoneT.rotation = finalBoneQ;
                    {
                        /*打印感兴趣的骨骼名称*/
                        int inspectKey = (int)KHHS32.LeftHandThumb1;
                        /*把finalBoneQ转换成欧拉角进行观察*/
                        Vector3 boneEulerRadius = finalBoneQ.eulerAngles;
                        Vector3 boneEulerDegree = new Vector3(
                            (float)(boneEulerRadius.x),
                            (float)(boneEulerRadius.y),
                            (float)(boneEulerRadius.z)
                            );

                        if (frameProperties.printDebugLog && (inspectKey == i))
                        {
                            /*输出所有骨骼的初始角度，全局坐标，相对于u3d世界坐标系*/
                            // Debug.Log(kinemHumanSkeleton53[BoneR.Key] + " finalBoneQ EulerAngle:" + boneEulerDegree.x + "," + boneEulerDegree.y + "," + boneEulerDegree.z);
                        }
                    }
                }
                currentSkeletonIndex++;
            }

#if true
            if (rawEulerLocInspector.isDrawHandJoint)
            {
                for (int i = (int)KHHS32.RightHand; i <= (int)KHHS32.RightHandPinky3; i++)
                {
                    Quaternion tempQ = new Quaternion(0.0f, 0.0f, 0.0f, 1.0f);
                    tempQ.eulerAngles = new Vector3(gloveRelativeTrackerCtrl.R_x, gloveRelativeTrackerCtrl.R_y, gloveRelativeTrackerCtrl.R_z);

                    rawEulerLocInspector.setJointPosition(i,
                                                            motionInspector.RH_Root_Display_GlobalQ,
                                                            // Quaternion.Inverse(tempQ),
                                                            motionInspector.RH_Root_Display_Loc[0],
                                                            motionInspector.RH_Root_Display_Loc[1],
                                                            motionInspector.RH_Root_Display_Loc[2]);
                }
                for (int i = (int)KHHS32.LeftHand; i <= (int)KHHS32.LeftHandPinky3; i++)
                {
                    Quaternion tempQ = new Quaternion(0.0f, 0.0f, 0.0f, 1.0f);
                    tempQ.eulerAngles = new Vector3(gloveRelativeTrackerCtrl.L_x, gloveRelativeTrackerCtrl.L_y, gloveRelativeTrackerCtrl.L_z);

                    rawEulerLocInspector.setJointPosition(i,
                        motionInspector.LH_Root_Display_GlobalQ,
                        //Quaternion.Inverse(  tempQ),
                        motionInspector.LH_Root_Display_Loc[0],
                               motionInspector.LH_Root_Display_Loc[1],
                                motionInspector.LH_Root_Display_Loc[2]);
                }
                rawEulerLocInspector.drawFigureLine();
            }
            else
            {
                rawEulerLocInspector.setJointShow(false);
            }

#endif
        }

        //快捷查找骨骼节点.如果没有找到则返回 null
        protected Transform FindBone(uint BoneIndex)
        {
            return skeletonTransforms[(int)BoneIndex];
        }

        float pointBias(Transform t1, Transform t2)
        {
            float ret = 0;
            if ((t1 == null) || (t2 == null))
            {
                return ret;
            }
            Vector3 v1 = t1.position;
            Vector3 v2 = t2.position;
            ret = (v2 - v1).magnitude;
            return ret;
        }

        void CalculateRealFrameRate()
        {
            int bias = realFrameCounter.currentRxCounter - realFrameCounter.lastRxCounter;
            realFrameCounter.lastRxCounter = realFrameCounter.currentRxCounter;
            //Debug.Log(gameObject + "pose bias:" + bias);
        }

        double getCurrentMs()
        {
            TimeSpan ts = new TimeSpan(DateTime.Now.Ticks);
            double ret = ts.TotalMilliseconds;
            ret -= applicationStartMs;
            return ret;
        }
    }
}
