MotionCapture/Assets/MainBehaviour.cs

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

public class MainBehaviour : MonoBehaviour
{
    private readonly List<GameObject> _nodes = new List<GameObject>();
    private readonly List<Bonds> _bonds = new List<Bonds>();
    private readonly List<PoseTransform> _poseTransforms = new List<PoseTransform>();
    private readonly UdpListener _listener = new UdpListener();
    private const float _scale = 0.2f;
    

    // Start is called before the first frame update
    private void Start()
    {
        CreateNodes();
        CreateBonds();
        _listener.AddHandler(OnReceive);
        _listener.Connect(5000);
    }

    // Update is called once per frame
    private void Update()
    {
        foreach (var poseTransform in _poseTransforms)
        {
            var index = (int)poseTransform.MediaPipeName;
            
            var temp = new Vector4(_poseTransforms[index].MediaPipePos.x,
                                   _poseTransforms[index].MediaPipePos.y,
                                    _poseTransforms[index].MediaPipePos.z,1);
            //temp = RotateAxisX(-45) * temp;
            //warning:乘法的顺序不能改!
            
            _nodes[index].transform.position = new Vector3(-temp.x, -temp.y, -temp.z) * 5;
            //_nodes[index].transform.position = _poseTransforms[index].MediaPipePos * 5;
        }

        foreach (var bond in _bonds)
        {
            bond.UpdateBond();
        }
    }

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

    private void CreateNodes()
    {
        for (var i = 0; i < 33; i++)
        {
            var ball = GameObject.CreatePrimitive(PrimitiveType.Sphere);

            ball.transform.localScale = new Vector3(_scale, _scale, _scale);

            if (i <= 10)
            {
                ball.GetComponent<Renderer>().material.color = Color.red;//给头部添加颜色
            }
            else if (i <= 22)
            {
                ball.GetComponent<Renderer>().material.color = Color.blue;//给手部添加颜色
            }
            else if (i <= 32)
            {
                ball.GetComponent<Renderer>().material.color = Color.green;//给脚部添加颜色
            }
            
            _nodes.Add(ball);

            var poseTransform = new PoseTransform((PoseLandmarkType)i);
            
            _poseTransforms.Add(poseTransform);
        }
    }

    private void CreateBonds()
    {
        //shit code,it's my fault
        Bonds temp;
        temp = new Bonds(_nodes[0], _nodes[4], _scale);
        _bonds.Add(temp);
        
        temp = new Bonds(_nodes[0], _nodes[1], _scale);
        _bonds.Add(temp);
        
        temp = new Bonds(_nodes[1], _nodes[2], _scale);
        _bonds.Add(temp);
        
        temp = new Bonds(_nodes[2], _nodes[3], _scale);
        _bonds.Add(temp);
        
        temp = new Bonds(_nodes[3], _nodes[7], _scale);
        _bonds.Add(temp);
        
        temp = new Bonds(_nodes[4], _nodes[5], _scale);
        _bonds.Add(temp);
        
        temp = new Bonds(_nodes[5], _nodes[6], _scale);
        _bonds.Add(temp);
        
        temp = new Bonds(_nodes[6], _nodes[8], _scale);
        _bonds.Add(temp);
        
        temp = new Bonds(_nodes[9], _nodes[10], _scale);
        _bonds.Add(temp);
        
        temp = new Bonds(_nodes[2], _nodes[3], _scale);
        _bonds.Add(temp);
        
        temp = new Bonds(_nodes[11], _nodes[12], _scale);
        _bonds.Add(temp);
        
        temp = new Bonds(_nodes[12], _nodes[14], _scale);
        _bonds.Add(temp);
        
        temp = new Bonds(_nodes[14], _nodes[16], _scale);
        _bonds.Add(temp);
        
        temp = new Bonds(_nodes[16], _nodes[22], _scale);
        _bonds.Add(temp);
        
        temp = new Bonds(_nodes[16], _nodes[18], _scale);
        _bonds.Add(temp);
        
        temp = new Bonds(_nodes[18], _nodes[20], _scale);
        _bonds.Add(temp);
        
        temp = new Bonds(_nodes[16], _nodes[20], _scale);
        _bonds.Add(temp);
        
        temp = new Bonds(_nodes[11], _nodes[13], _scale);
        _bonds.Add(temp);
        
        temp = new Bonds(_nodes[13], _nodes[15], _scale);
        _bonds.Add(temp);
        
        temp = new Bonds(_nodes[15], _nodes[17], _scale);
        _bonds.Add(temp);
        
        temp = new Bonds(_nodes[15], _nodes[21], _scale);
        _bonds.Add(temp);
        
        temp = new Bonds(_nodes[15], _nodes[19], _scale);
        _bonds.Add(temp);
        
        temp = new Bonds(_nodes[17], _nodes[19], _scale);
        _bonds.Add(temp);
        
        temp = new Bonds(_nodes[12], _nodes[24], _scale);
        _bonds.Add(temp);
        
        temp = new Bonds(_nodes[23], _nodes[24], _scale);
        _bonds.Add(temp);
        
        temp = new Bonds(_nodes[11], _nodes[23], _scale);
        _bonds.Add(temp);
        
        temp = new Bonds(_nodes[24], _nodes[26], _scale);
        _bonds.Add(temp);
        
        temp = new Bonds(_nodes[26], _nodes[28], _scale);
        _bonds.Add(temp);
        
        temp = new Bonds(_nodes[28], _nodes[32], _scale);
        _bonds.Add(temp);
        
        temp = new Bonds(_nodes[28], _nodes[30], _scale);
        _bonds.Add(temp);
        
        temp = new Bonds(_nodes[23], _nodes[25], _scale);
        _bonds.Add(temp);
        
        temp = new Bonds(_nodes[25], _nodes[27], _scale);
        _bonds.Add(temp);
        
        temp = new Bonds(_nodes[27], _nodes[29], _scale);
        _bonds.Add(temp);
        
        temp = new Bonds(_nodes[27], _nodes[31], _scale);
        _bonds.Add(temp);
        
        temp = new Bonds(_nodes[29], _nodes[31], _scale);
        _bonds.Add(temp);
    }
    
    
    
    private void OnReceive(List<PoseLandmark> landmarks)
    {
        foreach (var landmark in landmarks)
        {
            var poseTransform = new PoseTransform(landmark.Type)
            {
                MediaPipePos = new Vector3(landmark.X, landmark.Y, landmark.Z)
            };

            _poseTransforms[(int)landmark.Type] = poseTransform;
        }
    }

    //绕X轴旋转
    private Matrix4x4 RotateAxisX(float rot)
    {
        var matrix = new Matrix4x4();
        float cosrot = MathF.Cos(rot);
        float sinrot = MathF.Sin(rot);
            
        matrix.SetRow(0, new Vector4(1,0,0,0));
        matrix.SetRow(1, new Vector4(0,cosrot,-sinrot,0));
        matrix.SetRow(2, new Vector4(0,sinrot,cosrot,0));
        matrix.SetRow(3, new Vector4(0,0,0,1));

        return matrix;
    }
    
    //绕Y轴旋转
    private Matrix4x4 RotateAxisY(float rot)
    {
        var matrix = new Matrix4x4();
        float cosrot = MathF.Cos(rot);
        float sinrot = MathF.Sin(rot);
            
        matrix.SetRow(0, new Vector4(cosrot,0,sinrot,0));
        matrix.SetRow(1, new Vector4(0,1,0,0));
        matrix.SetRow(2, new Vector4(-sinrot,0,cosrot,0));
        matrix.SetRow(3, new Vector4(0,0,0,1));

        return matrix;
    }
    
    //绕Z轴旋转
    private Matrix4x4 RotateAxisZ(float rot)
    {
        var matrix = new Matrix4x4();
        float cosrot = MathF.Cos(rot);
        float sinrot = MathF.Sin(rot);
            
        matrix.SetRow(0, new Vector4(cosrot,-sinrot,0,0));
        matrix.SetRow(1, new Vector4(sinrot,cosrot,0,0));
        matrix.SetRow(2, new Vector4(0,0,1,0));
        matrix.SetRow(3, new Vector4(0,0,0,1));

        return matrix;
    }
}