GHZSDKXR/RokidSDK/com.rokid.xr.unity/Runtime/Scripts/Utils/HandUtils.cs

using Rokid.UXR.Interaction;
using UnityEngine;
using Rokid.UXR.Native;
namespace Rokid.UXR.Utility
{
    public class HandUtils
    {
        public static Quaternion[] AdjustSkeletonsRot(Vector3[] skeletons, Quaternion[] rotations, Vector3 forward, Quaternion[] skeletonsRot)
        {
            skeletonsRot[0] = Quaternion.FromToRotation(rotations[0] * forward, skeletons[9] - skeletons[0]) * rotations[0];

            skeletonsRot[1] = Quaternion.FromToRotation(rotations[1] * forward, skeletons[2] - skeletons[1]) * rotations[1];
            skeletonsRot[2] = Quaternion.FromToRotation(rotations[2] * forward, skeletons[3] - skeletons[2]) * rotations[2];
            skeletonsRot[3] = Quaternion.FromToRotation(rotations[3] * forward, skeletons[4] - skeletons[3]) * rotations[3];
            skeletonsRot[4] = Quaternion.FromToRotation(rotations[4] * forward, skeletons[4] - skeletons[3]) * rotations[4];

            skeletonsRot[5] = Quaternion.FromToRotation(rotations[5] * forward, skeletons[6] - skeletons[5]) * rotations[5];
            skeletonsRot[6] = Quaternion.FromToRotation(rotations[6] * forward, skeletons[7] - skeletons[6]) * rotations[6];
            skeletonsRot[7] = Quaternion.FromToRotation(rotations[7] * forward, skeletons[8] - skeletons[7]) * rotations[7];
            skeletonsRot[8] = Quaternion.FromToRotation(rotations[8] * forward, skeletons[8] - skeletons[7]) * rotations[8];

            skeletonsRot[9] = Quaternion.FromToRotation(rotations[9] * forward, skeletons[10] - skeletons[9]) * rotations[9];
            skeletonsRot[10] = Quaternion.FromToRotation(rotations[10] * forward, skeletons[11] - skeletons[10]) * rotations[10];
            skeletonsRot[11] = Quaternion.FromToRotation(rotations[11] * forward, skeletons[12] - skeletons[11]) * rotations[11];
            skeletonsRot[12] = Quaternion.FromToRotation(rotations[12] * forward, skeletons[12] - skeletons[11]) * rotations[12];

            skeletonsRot[13] = Quaternion.FromToRotation(rotations[13] * forward, skeletons[14] - skeletons[13]) * rotations[13];
            skeletonsRot[14] = Quaternion.FromToRotation(rotations[14] * forward, skeletons[15] - skeletons[14]) * rotations[14];
            skeletonsRot[15] = Quaternion.FromToRotation(rotations[15] * forward, skeletons[16] - skeletons[15]) * rotations[15];
            skeletonsRot[16] = Quaternion.FromToRotation(rotations[16] * forward, skeletons[16] - skeletons[15]) * rotations[16];

            skeletonsRot[17] = Quaternion.FromToRotation(rotations[17] * forward, skeletons[18] - skeletons[17]) * rotations[17];
            skeletonsRot[18] = Quaternion.FromToRotation(rotations[18] * forward, skeletons[19] - skeletons[18]) * rotations[18];
            skeletonsRot[19] = Quaternion.FromToRotation(rotations[19] * forward, skeletons[20] - skeletons[19]) * rotations[19];
            skeletonsRot[20] = Quaternion.FromToRotation(rotations[20] * forward, skeletons[20] - skeletons[19]) * rotations[20];

            return skeletonsRot;
        }

        #region  AdjustSkeletonPosition
        static float wrist_middle_cmp_dis = 0.099f;

        static float index_mcp_pip_dis = 0.0302203f;
        static float middle_mcp_pip_dis = 0.03317486f;
        static float ring_mcp_pip_dis = 0.02867948f;
        static float pinky_mcp_pip_dis = 0.02232722f;

        static float index_dip_pip_dis = 0.01864309f;
        static float middle_dip_pip_dis = 0.02395519f;
        static float ring_dip_pip_dis = 0.02499392f;
        static float pinky_dip_pip_dis = 0.02014625f;

        static float index_dip_tip_dis = 0.02287725f;
        static float middle_dip_tip_dis = 0.02539373f;
        static float ring_dip_tip_dis = 0.02466991f;
        static float pinky_dip_tip_dis = 0.01812316f;

        static float[] boneDis = new float[] {
            wrist_middle_cmp_dis,

            index_mcp_pip_dis,
            middle_mcp_pip_dis,
            ring_mcp_pip_dis,
            pinky_mcp_pip_dis,

            index_dip_pip_dis,
            middle_dip_pip_dis,
            ring_dip_pip_dis,
            pinky_dip_pip_dis,

            index_dip_tip_dis,
            middle_dip_tip_dis,
            ring_dip_tip_dis,
            pinky_dip_tip_dis
         };

        public static Vector3[] AdjustSkeletonsPos(Vector3[] skeletons, Vector3[] skeletonsPos)
        {
            int fingerMcpDisIndex = 1, fingerPipDisIndex = 5, fingerTipDisIndex = 9;
            for (int i = 0; i < 21; i++)
            {
                skeletonsPos[i] = skeletons[i];
                //调整MCP
                if (i == 5 || i == 9 || i == 13 || i == 17)
                {
                    Vector3 direct = Vector3.Normalize(skeletons[i + 1] - skeletons[i]);
                    skeletonsPos[i] = skeletons[i + 1] - direct * boneDis[fingerMcpDisIndex];
                    fingerMcpDisIndex++;
                }

                //调整Pip
                if (i == 7 || i == 11 || i == 15 || i == 19)
                {
                    Vector3 direct = Vector3.Normalize(skeletons[i] - skeletons[i - 1]);
                    skeletonsPos[i] = skeletons[i - 1] + direct * boneDis[fingerPipDisIndex];
                    fingerPipDisIndex++;
                }


                //调整Tip
                if (i == 8 || i == 12 || i == 16 || i == 20)
                {
                    Vector3 direct = Vector3.Normalize(skeletons[i] - skeletons[i - 1]);
                    skeletonsPos[i] = skeletons[i - 1] + direct * boneDis[fingerTipDisIndex];
                    fingerTipDisIndex++;
                }

                //调整Wrist
                if (i == 0)
                {
                    Vector3 direct = Vector3.Normalize(skeletons[9] - skeletons[0]);
                    skeletonsPos[0] = skeletons[9] - direct * boneDis[0];
                }
            }

            return skeletonsPos;
        }

        #endregion

        private static Quaternion rotation = Quaternion.identity;


        public static Quaternion GetQuaternion(float[] data, bool isRight, Pose cameraPose)
        {
            rotation[0] = data[0];
            rotation[1] = -data[1];
            rotation[2] = data[2];
            rotation[3] = -data[3];
            if (isRight == false)
            {
                rotation = cameraPose.rotation * rotation * Quaternion.Euler(new Vector3(-90, 0, -90));
            }
            else
            {
                rotation = cameraPose.rotation * rotation * Quaternion.Euler(new Vector3(-90, 90, 0));
            }
            return rotation;
        }

        public static Quaternion GetQuaternion(float[] data)
        {
            return new Quaternion(data[0], data[1], data[2], data[3]);
        }

        public static Vector3 GetVector3(float[] data)
        {
            return new Vector3(data[0], data[1], data[2]);
        }


        public static Vector3[] GetVertor3Arr(float[] data, Pose cameraPose)
        {
            Vector3[] vertices = new Vector3[data.Length / 3];
            Vector3 vert = Vector3.zero;
            for (int i = 0; i < vertices.Length; i++)
            {
                vert[0] = data[3 * i] / 1000.0f;
                vert[1] = -data[3 * i + 1] / 1000.0f;
                vert[2] = data[3 * i + 2] / 1000.0f;
                vertices[i] = cameraPose.rotation * vert;
                vertices[i] += cameraPose.position;
            }
            return vertices;
        }

        public static Vector3[] GetVertor3Arr(float[][] data, Pose cameraPose)
        {
            Vector3[] vertices = new Vector3[data.Length];
            Vector3 vert = Vector3.zero;
            for (int i = 0; i < data.Length; i++)
            {
                vert[0] = data[i][0] / 1000.0f;
                vert[1] = -data[i][1] / 1000.0f;
                vert[2] = data[i][2] / 1000.0f;
                vertices[i] = cameraPose.rotation * vert;
                vertices[i] += cameraPose.position;
            }
            return vertices;
        }

        private static Matrix4x4 rightHandMatrix = Matrix4x4.identity;
        private static Quaternion leftHandRotation = Quaternion.identity;
        public static Quaternion[] GetSkeletonsQuaternion(float[] data, bool isRight, Quaternion[] rotations, Pose cameraPose)
        {
            for (int i = 0; i < 21; i++)
            {
                for (int j = 0; j < 3; j++)
                {
                    rightHandMatrix.SetRow(j, new Vector3(data[9 * i + 3 * j], data[9 * i + 3 * j + 1], data[9 * i + 3 * j + 2]));
                }
                //右->左
                leftHandRotation[0] = rightHandMatrix.rotation[0];
                leftHandRotation[1] = -rightHandMatrix.rotation[1];
                leftHandRotation[2] = rightHandMatrix.rotation[2];
                leftHandRotation[3] = -rightHandMatrix.rotation[3];
                //左手坐标系中的空间和坐标轴转换
                if (isRight == false)
                {
                    //左手
                    leftHandRotation = cameraPose.rotation * leftHandRotation * Quaternion.Euler(new Vector3(-90, 0, -90));
                }
                else
                {
                    //右手
                    leftHandRotation = cameraPose.rotation * leftHandRotation * Quaternion.Euler(new Vector3(-90, 90, 0));
                }
                rotations[i] = leftHandRotation;
            }
            return rotations;
        }

        public static bool UnPinchForDistance(Vector3 indexTip, Vector3 thumbTip, float unPinchDistance)
        {
            return Vector3.Distance(indexTip, thumbTip) > unPinchDistance;
        }

        public static bool CanReleseHandDrag(HandType handType)
        {
            if (Utils.IsAndroidPlatfrom())
            {
                Vector3 indexPos = GesEventInput.Instance.GetSkeletonPose(SkeletonIndexFlag.INDEX_FINGER_TIP, handType).position;
                Vector3 thumbPos = GesEventInput.Instance.GetSkeletonPose(SkeletonIndexFlag.THUMB_TIP, handType).position;
                return HandUtils.UnPinchForDistance(indexPos, thumbPos, 0.04f) && GesEventInput.Instance.GetGestureType(handType) != GestureType.Grip;
            }
            else
            {
                return Input.GetMouseButtonUp(0);
            }
        }

        public static Vector3 InverseTransformPoint(Pose pose, Vector3 point)
        {
            return Quaternion.Inverse(pose.rotation) * (point - pose.position);
        }

        public static Vector3 TransformPoint(Pose pose, Vector3 point)
        {
            return pose.rotation * point + pose.position;
        }
    }
}