MotionCapture/Assets/Behaviours/BoneBehaviour.cs

using System;
using System.Collections.Generic;
using Models;
using UnityEngine;
using Utils.PoseTransformHandlers;

namespace Behaviours
{
    public class BoneBehaviour : MonoBehaviour
    {
        private readonly UdpListener _listener = new UdpListener();
        private readonly PoseTransform[] _transforms = new PoseTransform[PoseLandmarkType.MaxValue];
    
        private Animator _animator;

        private void Start()
        {
            _animator = GetComponent<Animator>();

            for (var i = 0; i < PoseLandmarkType.MaxValue; i++)
            {
                _transforms[i] = new PoseTransform(i, new ZAxisHandler());
            }
        
            _listener.AddHandler(OnReceive);
            _listener.Connect(5000);
        }

        private void Update()
        {
            HipUpdate();
            
            RShoulderUpdate();
        }

        private void OnDisable()
        {
            _listener.DisConnect();
        }

        private void OnReceive(List<PoseLandmark> landmarks)
        {
            foreach (var landmark in landmarks)
            {
                PoseTransform.UpdatePosition(ref _transforms[landmark.Type.Value], landmark);
            }
        }


        private void HipUpdate()
        {
            var frontLeft = Vector3.Cross(
                _transforms[PoseLandmarkType.RightHip] - _transforms[PoseLandmarkType.LeftHip],
                _transforms[PoseLandmarkType.RightShoulder] - _transforms[PoseLandmarkType.LeftHip]);

            var frontRight = Vector3.Cross(
                _transforms[PoseLandmarkType.LeftShoulder] - _transforms[PoseLandmarkType.RightHip],
                _transforms[PoseLandmarkType.LeftHip] - _transforms[PoseLandmarkType.RightHip]);

            var front = frontLeft + frontRight;
            front.Normalize();

            _animator.GetBoneTransform(HumanBodyBones.Hips).rotation = Quaternion.LookRotation(front);
        }
        
        private void RShoulderUpdate()
        {

            var relativeRot = _transforms[PoseLandmarkType.RightElbow] - _transforms[PoseLandmarkType.RightShoulder];
            relativeRot.Normalize();

            var hipRot = _animator.GetBoneTransform(HumanBodyBones.Hips).rotation;
            hipRot.Normalize();

            var interal = Quaternion.FromToRotation(relativeRot, hipRot.eulerAngles);
            interal.Normalize();

            _animator.GetBoneTransform(HumanBodyBones.RightUpperArm).rotation =  interal; //Quaternion.LookRotation(relativeRot + hipRot);
        }
        
        
        //计算向量a到向量b的旋转角
        //参数 a：起始向量; b：目标向量; n：旋转方向 (0, 1, 0)顺时针  (0, -1, 0)逆时针
        private static float SignedAngleBetween(Vector3 a, Vector3 b, Vector3 n)
        {
            var angle = Vector3.Angle(a,b);
            var sign = Mathf.Sign(Vector3.Dot(n,Vector3.Cross(a,b)));
            var signedAngle = angle * sign;
            return (signedAngle <= 0) ? 360 + signedAngle : signedAngle;
        }
    }
    
    
   
}