MotionCapture/Assets/Models/PoseTransform.cs

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

namespace Models
{
    /// <summary>
    /// 捕捉骨骼的动作转换
    /// </summary>
    public struct PoseTransform
    {
        /// <summary>
        /// MediaPipe中动捕节点标识
        /// </summary>
        public readonly PoseLandmarkType MediaPipeName;

        /// <summary>
        /// Unity中绑定骨骼名称
        /// </summary>
        public readonly HumanBodyBones UnityName;

        /// <summary>
        /// MediaPipe捕捉空间坐标
        /// </summary>
        public  Vector3  MediaPipePos;

        /// <summary>
        /// 节点的父节点列表
        /// </summary>
        public readonly List<PoseTransform> Parent;

        /// <summary>
        /// 骨骼节点的初始角度
        /// </summary>
        public Quaternion PreviousQuaternion;

        /// <summary>
        /// 骨骼节点的当前角度
        /// </summary>
        public Quaternion CurrentQuaternion;

        public PoseTransform(
            PoseLandmarkType type
        )
        {
            MediaPipeName = type;
            UnityName = GetRelatedBone(type);
            MediaPipePos = new Vector3();
            Parent = new List<PoseTransform>();
            PreviousQuaternion = new Quaternion();
            CurrentQuaternion = new Quaternion();
        }

        /// <summary>
        /// 计算当前的旋转四元数
        /// </summary>
        /// <param name="landmark"></param>
        public void CalculateCurrentQuaternion(PoseLandmark landmark)
        {
            
            //var temp = _posLandmarks.Find(t=>t.mediaPipeName == landmark.Type.ToString());
             var sum = new Vector3(0,0,0);
             
             if (landmark.Visibility > 0.6)
             {
                 var temp = new Vector4(landmark.X, landmark.Y, landmark.Z, 1);
                 
                 
                 MediaPipePos = new Vector3(-temp.x, -temp.y, -temp.z);
                 
             }

             if (UnityName != HumanBodyBones.LastBone)
             {
                 Debug.Log(UnityName+".MediaPipePos:"+MediaPipePos);
             }
            
            foreach (var parents in Parent)
            {
               
                    sum += parents.MediaPipePos;
                    //Debug.Log("parents:"+parents.UnityName+".MediaPipePos:"+parents.MediaPipePos);
                    
            }
            //Debug.Log("sum:"+sum);
            var currentPos = sum / Parent.Count;
            
            CurrentQuaternion = Quaternion.LookRotation(MediaPipePos - currentPos);
            
        }

        /// <summary>
        /// 获得同相关捕捉点关联的骨骼
        /// </summary>
        /// <param name="type">捕捉点的种类</param>
        /// <returns>关联骨骼的种类</returns>
        private static HumanBodyBones GetRelatedBone(PoseLandmarkType type)
        {
            switch (type)
            {
                case PoseLandmarkType.LeftHip:
                    return HumanBodyBones.LeftUpperLeg;
                case PoseLandmarkType.RightHip:
                    return HumanBodyBones.RightUpperLeg;
                case PoseLandmarkType.LeftShoulder:
                    return HumanBodyBones.LeftUpperArm;
                case PoseLandmarkType.RightShoulder:
                    return HumanBodyBones.RightUpperArm;
                case PoseLandmarkType.Nose:
                    return HumanBodyBones.Head;
                case PoseLandmarkType.LeftElbow:
                    return HumanBodyBones.LeftLowerArm;
                case PoseLandmarkType.RightElbow:
                    return HumanBodyBones.RightLowerArm;
                case PoseLandmarkType.LeftWrist:
                    return HumanBodyBones.LeftHand;
                case PoseLandmarkType.RightWrist:
                    return HumanBodyBones.RightHand;
                case PoseLandmarkType.LeftKnee:
                    return HumanBodyBones.LeftLowerLeg;
                case PoseLandmarkType.RightKnee:
                    return HumanBodyBones.RightLowerLeg;
                case PoseLandmarkType.LeftAnkle:
                    return HumanBodyBones.LeftFoot;
                case PoseLandmarkType.RightAnkle:
                    return HumanBodyBones.RightFoot;
                case PoseLandmarkType.LeftFootIndex:
                    return HumanBodyBones.LeftToes;
                case PoseLandmarkType.RightFootIndex:
                    return HumanBodyBones.RightToes;
                default:
                    return HumanBodyBones.LastBone;
            }
        }
        
        
        //绕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;
        }
        
        
    }
}