NaiNiu/Assets/HandDriver/Scripts/HandDriver.cs

using System.Collections.Generic;
using TouchSocket.Core;
using UnityEngine;

public class HandDriver : MonoBehaviour
{
    public Network Network;

    public enum HandType
    {
        Left,
        Right
    }

    public int HandDeviceIndex = 1;
    public HandType Hand;
    private Dictionary<string, Quaternion> _originQuaternionDic;

    public Transform Thumb1;
    public Transform Thumb2;
    public Transform Thumb3;

    public Transform Index1;
    public Transform Index2;
    public Transform Index3;

    public Transform Middle1;
    public Transform Middle2;
    public Transform Middle3;

    public Transform Ring1;
    public Transform Ring2;
    public Transform Ring3;

    public Transform Pinky1;
    public Transform Pinky2;
    public Transform Pinky3;

    public Transform Wrist;

    [Header("坐标系偏差")] public int Pitch = 0;
    public int Roll = 0;

    public int Yaw = 0;

    [Header("IMU开关")] public bool HasIMU = false;

    [Header("拇指根节点系数")] [Range(0, 1)] public float coefficient = 0.6f;

    [Header("拇指根节点偏差")] public Vector3 Thumb1Offset;

    // Start is called before the first frame update
    void Start()
    {
        if (Network == null)
        {
            if (GameObject.Find("Network") == null)
            {
                GameObject network = new GameObject("Network");
                network.AddComponent<Network>();
            }

            Network = GameObject.Find("Network").GetComponent<Network>();
        }

        _originQuaternionDic = new Dictionary<string, Quaternion>();
        InitJoints();
    }

    private void InitJoints()
    {
        _originQuaternionDic.AddOrUpdate(Thumb1.name, Thumb1.localRotation);
        _originQuaternionDic.AddOrUpdate(Thumb2.name, Thumb2.localRotation);
        _originQuaternionDic.AddOrUpdate(Thumb3.name, Thumb3.localRotation);

        _originQuaternionDic.AddOrUpdate(Index1.name, Index1.localRotation);
        _originQuaternionDic.AddOrUpdate(Index2.name, Index2.localRotation);
        _originQuaternionDic.AddOrUpdate(Index3.name, Index3.localRotation);

        _originQuaternionDic.AddOrUpdate(Middle1.name, Middle1.localRotation);
        _originQuaternionDic.AddOrUpdate(Middle2.name, Middle2.localRotation);
        _originQuaternionDic.AddOrUpdate(Middle3.name, Middle3.localRotation);

        _originQuaternionDic.AddOrUpdate(Ring1.name, Ring1.localRotation);
        _originQuaternionDic.AddOrUpdate(Ring2.name, Ring2.localRotation);
        _originQuaternionDic.AddOrUpdate(Ring3.name, Ring3.localRotation);

        _originQuaternionDic.AddOrUpdate(Pinky1.name, Pinky1.localRotation);
        _originQuaternionDic.AddOrUpdate(Pinky2.name, Pinky2.localRotation);
        _originQuaternionDic.AddOrUpdate(Pinky3.name, Pinky3.localRotation);

        _originQuaternionDic.AddOrUpdate(Wrist.name, Wrist.localRotation);
    }

    private void Rotate(Transform tran, float angle, int angleType)
    {
        float angleX = 0;
        float angleY = 0;
        float angleZ = 0;

        switch (angleType)
        {
            case -3:
                angleX = -angle;
                break;
            case -2:
                angleY = -angle;
                break;
            case -1:
                angleZ = -angle;
                break;
            case 0:
                angleX = angle;
                break;
            case 1:
                angleY = angle;
                break;
            case 2:
                angleZ = angle;
                break;
        }

        tran.Rotate(angleX, angleY, angleZ);
    }

    private void ResetRotation(Transform trans)
    {
        if (_originQuaternionDic.TryGetValue(trans.name, out Quaternion rot))
        {
            trans.localRotation = rot;
        }
    }

    // Update is called once per frame
    void Update()
    {
        UpdateThumb();
        UpdateIndex();
        UpdateMiddle();
        UpdateRing();
        UpdatePinky();

        UpdateWrist();
    }

    private void UpdateWrist()
    {
        if (HasIMU)
        {
            // if (Hand == HandType.Left)
            // {
            //     Quaternion quat_r = new Quaternion(
            //         Network.Convert2Angle(HandDeviceIndex, "l26") + Network.ClbrationQuaternionOffset.x,
            //         Network.Convert2Angle(HandDeviceIndex, "l25") + Network.ClbrationQuaternionOffset.y,
            //         Network.Convert2Angle(HandDeviceIndex, "l27") + Network.ClbrationQuaternionOffset.z,
            //         Network.Convert2Angle(HandDeviceIndex, "l24") + Network.ClbrationQuaternionOffset.w);
            //     ResetRotation(Wrist);
            //     Wrist.Rotate(-quat_r.eulerAngles.z, -quat_r.eulerAngles.x, -quat_r.eulerAngles.y);
            // }
            // else
            // {
            //     Quaternion quat_r = new Quaternion(Network.Convert2Angle(HandDeviceIndex, "r26"),
            //         Network.Convert2Angle(HandDeviceIndex, "r25"),
            //         Network.Convert2Angle(HandDeviceIndex, "r27"), Network.Convert2Angle(HandDeviceIndex, "r24"));
            //     ResetRotation(Wrist);
            //     Wrist.Rotate(-quat_r.eulerAngles.z, -quat_r.eulerAngles.x, -quat_r.eulerAngles.y);
            // }
        }
    }

    private void UpdateThumb()
    {
        ResetRotation(Thumb1);
        ResetRotation(Thumb2);
        ResetRotation(Thumb3);
        if (Hand == HandType.Left)
        {
            Rotate(Thumb3, Network.Convert2Angle(HandDeviceIndex, "l0"), Pitch);
            Rotate(Thumb2, Network.Convert2Angle(HandDeviceIndex, "l1"), Pitch);
            Rotate(Thumb1, Network.Convert2Angle(HandDeviceIndex, "l2") * coefficient + Thumb1Offset.y, Pitch);
            Rotate(Thumb1, Network.Convert2Angle(HandDeviceIndex, "l3") + Thumb1Offset.z, Yaw);
            Rotate(Thumb1, Network.Convert2Angle(HandDeviceIndex, "l20") + Thumb1Offset.x, Roll);
        }
        else
        {
            Rotate(Thumb3, Network.Convert2Angle(HandDeviceIndex, "r0"), Pitch);
            Rotate(Thumb2, Network.Convert2Angle(HandDeviceIndex, "r1"), Pitch);
            Rotate(Thumb1, Network.Convert2Angle(HandDeviceIndex, "r2") * coefficient + Thumb1Offset.y, Pitch);
            Rotate(Thumb1, Network.Convert2Angle(HandDeviceIndex, "r3") + Thumb1Offset.z, Yaw);
            Rotate(Thumb1, Network.Convert2Angle(HandDeviceIndex, "r20") + Thumb1Offset.x, Roll);
        }
    }

    private void UpdateIndex()
    {
        ResetRotation(Index1);
        ResetRotation(Index2);
        ResetRotation(Index3);
        if (Hand == HandType.Left)
        {
            Rotate(Index3, Network.Convert2Angle(HandDeviceIndex, "l4"), Pitch);
            Rotate(Index2, Network.Convert2Angle(HandDeviceIndex, "l5"), Pitch);
            Rotate(Index1, Network.Convert2Angle(HandDeviceIndex, "l6"), Pitch);
            Rotate(Index1, Network.Convert2Angle(HandDeviceIndex, "l7"), Yaw);
            Rotate(Index1, Network.Convert2Angle(HandDeviceIndex, "l21"), Roll);
        }
        else
        {
            Rotate(Index3, Network.Convert2Angle(HandDeviceIndex, "r4"), Pitch);
            Rotate(Index2, Network.Convert2Angle(HandDeviceIndex, "r5"), Pitch);
            Rotate(Index1, Network.Convert2Angle(HandDeviceIndex, "r6"), Pitch);
            Rotate(Index1, Network.Convert2Angle(HandDeviceIndex, "r7"), Yaw);
            Rotate(Index1, Network.Convert2Angle(HandDeviceIndex, "r21"), Roll);
        }
    }

    private void UpdateMiddle()
    {
        ResetRotation(Middle1);
        ResetRotation(Middle2);
        ResetRotation(Middle3);
        if (Hand == HandType.Left)
        {
            Rotate(Middle3, Network.Convert2Angle(HandDeviceIndex, "l8"), Pitch);
            Rotate(Middle2, Network.Convert2Angle(HandDeviceIndex, "l9"), Pitch);
            Rotate(Middle1, Network.Convert2Angle(HandDeviceIndex, "l10"), Pitch);
            Rotate(Middle1, Network.Convert2Angle(HandDeviceIndex, "l11"), Yaw);
        }
        else
        {
            Rotate(Middle3, Network.Convert2Angle(HandDeviceIndex, "r8"), Pitch);
            Rotate(Middle2, Network.Convert2Angle(HandDeviceIndex, "r9"), Pitch);
            Rotate(Middle1, Network.Convert2Angle(HandDeviceIndex, "r10"), Pitch);
            Rotate(Middle1, Network.Convert2Angle(HandDeviceIndex, "r11"), Yaw);
        }
    }

    private void UpdateRing()
    {
        ResetRotation(Ring1);
        ResetRotation(Ring2);
        ResetRotation(Ring3);
        if (Hand == HandType.Left)
        {
            Rotate(Ring3, Network.Convert2Angle(HandDeviceIndex, "l12"), Pitch);
            Rotate(Ring2, Network.Convert2Angle(HandDeviceIndex, "l13"), Pitch);
            Rotate(Ring1, Network.Convert2Angle(HandDeviceIndex, "l14"), Pitch);
            Rotate(Ring1, -Network.Convert2Angle(HandDeviceIndex, "l15"), Yaw);
        }
        else
        {
            Rotate(Ring3, Network.Convert2Angle(HandDeviceIndex, "r12"), Pitch);
            Rotate(Ring2, Network.Convert2Angle(HandDeviceIndex, "r13"), Pitch);
            Rotate(Ring1, Network.Convert2Angle(HandDeviceIndex, "r14"), Pitch);
            Rotate(Ring1, -Network.Convert2Angle(HandDeviceIndex, "r15"), Yaw);
        }
    }

    private void UpdatePinky()
    {
        ResetRotation(Pinky1);
        ResetRotation(Pinky2);
        ResetRotation(Pinky3);
        if (Hand == HandType.Left)
        {
            Rotate(Pinky3, Network.Convert2Angle(HandDeviceIndex, "l16"), Pitch);
            Rotate(Pinky2, Network.Convert2Angle(HandDeviceIndex, "l17"), Pitch);
            Rotate(Pinky1, Network.Convert2Angle(HandDeviceIndex, "l18"), Pitch);
            Rotate(Pinky1, -Network.Convert2Angle(HandDeviceIndex, "l19"), Yaw);
            Rotate(Pinky1, -Network.Convert2Angle(HandDeviceIndex, "l22"), Roll);
        }
        else
        {
            Rotate(Pinky3, Network.Convert2Angle(HandDeviceIndex, "r16"), Pitch);
            Rotate(Pinky2, Network.Convert2Angle(HandDeviceIndex, "r17"), Pitch);
            Rotate(Pinky1, Network.Convert2Angle(HandDeviceIndex, "r18"), Pitch);
            Rotate(Pinky1, -Network.Convert2Angle(HandDeviceIndex, "r19"), Yaw);
            Rotate(Pinky1, -Network.Convert2Angle(HandDeviceIndex, "r22"), Roll);
        }
    }
}