MotionCapture/Assets/ModelBehaviour.cs

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

public class ModelBehaviour : MonoBehaviour
{
    private Animator _animator;

    private readonly UdpListener _listener = new UdpListener();

    private static readonly List<PosTransformator> _posLandmarks = new List<PosTransformator>();

    private static readonly List<PoseTransform> PoseTransforms = new List<PoseTransform>();

    private void Start()
    {
        _animator = GetComponent<Animator>(); // 获取动画控件

        TransformatorInit(); //匹配初始化
        InitPoseTransformList();

        _listener.AddHandler(LogLandmarks);
        _listener.AddHandler(RigPoint);
        _listener.Connect(5000);
    }

    // Update is called once per frame
    private void Update()
    {

        
        foreach (var landmark in _posLandmarks)
        {
            //animator.GetBoneTransform(landmark.unityName).rotation = landmark.currentQ;
            
        }


    }

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

    private static void LogLandmarks(List<PoseLandmark> landmarks)
    {
        foreach (var landmark in landmarks)
        {
            Debug.Log(landmark.ToString());
        }
    }


    //获取传上来的数据点坐标转换成四元数的回调函数
    private static void RigPoint(List<PoseLandmark> landmarks)
    {
        
        foreach (var landmark in landmarks)
        {
            PoseTransforms[(int)landmark.Type].CalculateCurrentQuaternion(landmark);
            
            
            // var temp = _posLandmarks.Find(t=>t.mediaPipeName == landmark.Type.ToString());
            // var sum = new Vector3(0,0,0);
            //var mediatemp = new Vector3(landmark.X, landmark.Y, landmark.Z);
            //temp.mediaPipePos = new Vector3(landmark.X, landmark.Y, landmark.Z);
            //temp.mediaPipePos.x = landmark.X;
            //temp.mediaPipePos.y = landmark.Y;
            //temp.mediaPipePos.z = landmark.Z;
            //foreach (var parents in temp.parent)
            //{
            //    if (parents != null)
            //   {
            //       sum += parents.mediaPipePos;
            //   }


            //}

            //var currentPos = sum / temp.parent.Count();

            //temp.currentQ = Quaternion.Euler(temp.mediaPipePos - currentPos);

            //Debug.Log("Here!!");
        }

    }

    /// <summary>
    /// 初始化骨骼变化结构体列表
    /// </summary>
    private void InitPoseTransformList()
    {
        for (var type = 0; type <= (int)PoseLandmarkType.RightFootIndex; type++)
        {
            var item = new PoseTransform((PoseLandmarkType)type);

            if (item.UnityName != HumanBodyBones.LastBone)
            {
                item.PreviousQuaternion = _animator.GetBoneTransform(item.UnityName).rotation;
                item.CurrentQuaternion = item.PreviousQuaternion;
            }
            
            PoseTransforms.Add(item);
        }
        
        // 在这里添加父节点
        AddParentTransform(PoseLandmarkType.LeftShoulder, PoseLandmarkType.LeftHip);
        AddParentTransform(PoseLandmarkType.LeftShoulder, PoseLandmarkType.RightHip);

        AddParentTransform(PoseLandmarkType.RightShoulder, PoseLandmarkType.LeftHip);
        AddParentTransform(PoseLandmarkType.RightShoulder, PoseLandmarkType.RightHip);
        
        AddParentTransform(PoseLandmarkType.Nose, PoseLandmarkType.LeftShoulder);
        AddParentTransform(PoseLandmarkType.Nose, PoseLandmarkType.RightShoulder);
        
        AddParentTransform(PoseLandmarkType.LeftElbow, PoseLandmarkType.LeftShoulder);
        AddParentTransform(PoseLandmarkType.RightElbow, PoseLandmarkType.RightShoulder);
        
        AddParentTransform(PoseLandmarkType.LeftWrist, PoseLandmarkType.LeftElbow);
        AddParentTransform(PoseLandmarkType.RightWrist, PoseLandmarkType.RightElbow);
        
        AddParentTransform(PoseLandmarkType.LeftKnee, PoseLandmarkType.LeftHip);
        AddParentTransform(PoseLandmarkType.RightKnee, PoseLandmarkType.RightHip);
        
        AddParentTransform(PoseLandmarkType.LeftAnkle, PoseLandmarkType.LeftKnee);
        AddParentTransform(PoseLandmarkType.RightAnkle, PoseLandmarkType.RightKnee);
        
        AddParentTransform(PoseLandmarkType.LeftFootIndex, PoseLandmarkType.LeftAnkle);
        AddParentTransform(PoseLandmarkType.RightFootIndex, PoseLandmarkType.RightAnkle);
    }

    /// <summary>
    /// 给指定的骨骼变化结构体添加父节点
    /// </summary>
    /// <param name="target">需要添加的节点</param>
    /// <param name="parent">添加的父节点</param>
    private void AddParentTransform(PoseLandmarkType target, PoseLandmarkType parent)
    {
        var parents = PoseTransforms[(int)target].Parent;
        parents.Add(PoseTransforms[(int)parent]);
    }

    private void TransformatorInit()
    {

        /*下面的语句的顺序最好别调换，不然可能会发生难以预料的事情*/
        
        PosTransformator[] transformator = new PosTransformator[15];

        {
            var temp = new PosTransformator();
            temp.mediaPipeName = "LeftHip";
            temp.unityName = HumanBodyBones.LeftUpperLeg;
            temp.parent.Add(null);
            transformator[0] = temp;
        }

        {
            var temp = new PosTransformator();
            temp.mediaPipeName = "RightHip";
            temp.unityName = HumanBodyBones.RightUpperLeg;
            temp.parent.Add(null);
            transformator[1] = temp;
        }

        {
            var temp = new PosTransformator();
            temp.mediaPipeName = "LeftShoulder";
            temp.unityName = HumanBodyBones.LeftUpperArm;
            temp.parent.Add(transformator[0]);
            temp.parent.Add(transformator[1]);
            transformator[2] = temp;
        }

        {
            var temp = new PosTransformator();
            temp.mediaPipeName = "RightShoulder";
            temp.unityName = HumanBodyBones.RightUpperArm;
            temp.parent.Add(transformator[0]);
            temp.parent.Add(transformator[1]);
            transformator[3] = temp;
        }

        {
            var temp = new PosTransformator();
            temp.mediaPipeName = "Nose";
            temp.unityName = HumanBodyBones.Head;
            temp.parent.Add(transformator[2]);
            temp.parent.Add(transformator[3]);
            transformator[4] = temp;
        }

        {
            var temp = new PosTransformator();
            temp.mediaPipeName = "LeftElbow";
            temp.unityName = HumanBodyBones.LeftLowerArm;
            temp.parent.Add(transformator[2]);
            transformator[5] = temp;
        }

        {
            var temp = new PosTransformator();
            temp.mediaPipeName = "RightElbow";
            temp.unityName = HumanBodyBones.RightLowerArm;
            temp.parent.Add(transformator[3]);
            transformator[6] = temp;
        }

        {
            var temp = new PosTransformator();
            temp.mediaPipeName = "LeftWrist";
            temp.unityName = HumanBodyBones.LeftHand;
            temp.parent.Add(transformator[5]);
            transformator[7] = temp;
        }

        {
            var temp = new PosTransformator();
            temp.mediaPipeName = "RightWrist";
            temp.unityName = HumanBodyBones.RightHand;
            temp.parent.Add(transformator[6]);
            transformator[8] = temp;
        }

        {
            var temp = new PosTransformator();
            temp.mediaPipeName = "LeftKnee";
            temp.unityName = HumanBodyBones.LeftLowerLeg;
            temp.parent.Add(transformator[0]);
            transformator[9] = temp;
        }

        {
            var temp = new PosTransformator();
            temp.mediaPipeName = "RightKnee";
            temp.unityName = HumanBodyBones.RightLowerLeg;
            temp.parent.Add(transformator[1]);
            transformator[10] = temp;
        }

        {
            var temp = new PosTransformator();
            temp.mediaPipeName = "LeftAnkle";
            temp.unityName = HumanBodyBones.LeftFoot;
            temp.parent.Add(transformator[9]);
            transformator[11] = temp;
        }

        {
            var temp = new PosTransformator();
            temp.mediaPipeName = "RightAnkle";
            temp.unityName = HumanBodyBones.RightFoot;
            temp.parent.Add(transformator[10]);
            transformator[12] = temp;
        }

        {
            var temp = new PosTransformator();
            temp.mediaPipeName = "LeftFootIndex";
            temp.unityName = HumanBodyBones.LeftToes;
            temp.parent.Add(transformator[11]);
            transformator[13] = temp;
        }

        {
            var temp = new PosTransformator();
            temp.mediaPipeName = "RightFootIndex";
            temp.unityName = HumanBodyBones.RightToes;
            temp.parent.Add(transformator[12]);
            transformator[14] = temp;
        }

        //Landmarks_Mapping.Add("Nose",HumanBodyBones.Head);                    //0 10

        //Landmarks_Mapping.Add("LeftShoulder",HumanBodyBones.LeftUpperArm);    //11

        //Landmarks_Mapping.Add("RightShoulder",HumanBodyBones.RightUpperArm);  //12

        //Landmarks_Mapping.Add("LeftElbow",HumanBodyBones.LeftLowerArm);       //13

        //Landmarks_Mapping.Add("RightElbow",HumanBodyBones.RightLowerArm);     //14

        //Landmarks_Mapping.Add("LeftWrist",HumanBodyBones.LeftHand);           //15

        //Landmarks_Mapping.Add("RightWrist",HumanBodyBones.RightHand);         //16

        //Landmarks_Mapping.Add("LeftHip",HumanBodyBones.LeftUpperLeg);         //23

        //Landmarks_Mapping.Add("RightHip",HumanBodyBones.RightUpperLeg);       //24

        //Landmarks_Mapping.Add("LeftKnee",HumanBodyBones.LeftLowerLeg);        //25

        //Landmarks_Mapping.Add("RightKnee",HumanBodyBones.RightLowerLeg);      //26

        //Landmarks_Mapping.Add("LeftAnkle",HumanBodyBones.LeftFoot);           //27

        //Landmarks_Mapping.Add("RightAnkle",HumanBodyBones.RightFoot);         //28

        //Landmarks_Mapping.Add("LeftFootIndex",HumanBodyBones.LeftToes);        //31

        //Landmarks_Mapping.Add("RightFootIndex",HumanBodyBones.RightToes);      //32

        foreach (var temp in transformator)
        {
            temp.prevQ = _animator.GetBoneTransform(temp.unityName).rotation; //获得初始角度

            temp.currentQ = temp.prevQ;

            _posLandmarks.Add(temp);
        }

    }
}