添加了旋转方法

Assets/ModelBehaviour.cs

@@ -8,7 +8,7 @@ public class ModelBehaviour : MonoBehaviour
 
     private readonly UdpListener _listener = new UdpListener();
 
-    private static readonly List<PosTransformator> _posLandmarks = new List<PosTransformator>();
+    //private static readonly List<PosTransformator> _posLandmarks = new List<PosTransformator>();
 
     private static readonly List<PoseTransform> PoseTransforms = new List<PoseTransform>();
 
@@ -16,10 +16,10 @@ private void Start()
     {
         _animator = GetComponent<Animator>(); // 获取动画控件
 
-        TransformatorInit(); //匹配初始化
+        
         InitPoseTransformList();
 
-        _listener.AddHandler(LogLandmarks);
+        //_listener.AddHandler(LogLandmarks);
         _listener.AddHandler(RigPoint);
         _listener.Connect(5000);
     }
@@ -28,11 +28,17 @@ private void Start()
     private void Update()
     {
 
-        
+
-        foreach (var landmark in _posLandmarks)
+        foreach (var landmark in PoseTransforms)
         {
-            //animator.GetBoneTransform(landmark.unityName).rotation = landmark.currentQ;
+            if (landmark.UnityName != HumanBodyBones.LastBone)
-            
+            {
+                
+                 _animator.GetBoneTransform(landmark.UnityName).rotation = landmark.CurrentQuaternion;
+                 
+            }
+           
+
         }
 
 
@@ -55,12 +61,15 @@ private static void LogLandmarks(List<PoseLandmark> landmarks)
     //获取传上来的数据点坐标转换成四元数的回调函数
     private static void RigPoint(List<PoseLandmark> landmarks)
     {
-        
+
         foreach (var landmark in landmarks)
         {
-            PoseTransforms[(int)landmark.Type].CalculateCurrentQuaternion(landmark);
+            var temp = PoseTransforms[(int)landmark.Type];
-            
+            temp.CalculateCurrentQuaternion(landmark);
-            
+            PoseTransforms[(int)landmark.Type] = temp;
+            //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);
@@ -101,32 +110,32 @@ private void InitPoseTransformList()
                 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);
     }
@@ -142,174 +151,4 @@ private void AddParentTransform(PoseLandmarkType target, PoseLandmarkType 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);
-        }
-
-    }
-}

Assets/Models/PosTransformator.cs

@@ -1,4 +1,5 @@
-using System.Collections.Generic;
+/*
+using System.Collections.Generic;
 using UnityEngine;
 
 namespace Models
@@ -23,4 +24,5 @@ public class PosTransformator
         public Quaternion currentQ;
         //该骨骼节点的当前角度
     }
-} 
+} 
+*/

Assets/Models/PoseTransform.cs

@@ -1,5 +1,6 @@
 using System.Collections.Generic;
 using UnityEngine;
+using System;
 
 namespace Models
 {
@@ -21,7 +22,7 @@ public struct PoseTransform
         /// <summary>
         /// MediaPipe捕捉空间坐标
         /// </summary>
-        public Vector3 MediaPipePos;
+        public  Vector3  MediaPipePos;
 
         /// <summary>
         /// 节点的父节点列表
@@ -57,6 +58,35 @@ PoseLandmarkType type
         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>
@@ -104,5 +134,51 @@ private static HumanBodyBones GetRelatedBone(PoseLandmarkType type)
         }
         
         
+        //绕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;
+        }
+        
+        
     }
 }

Assets/Scenes/SampleScene.unity

@@ -38,7 +38,7 @@ RenderSettings:
   m_ReflectionIntensity: 1
   m_CustomReflection: {fileID: 0}
   m_Sun: {fileID: 705507994}
-  m_IndirectSpecularColor: {r: 0.18029127, g: 0.22572401, b: 0.3069303, a: 1}
+  m_IndirectSpecularColor: {r: 0.18028378, g: 0.22571412, b: 0.30692285, a: 1}
   m_UseRadianceAmbientProbe: 0
 --- !u!157 &3
 LightmapSettings:
@@ -174,6 +174,10 @@ PrefabInstance:
       propertyPath: m_LocalEulerAnglesHint.z
       value: 0
       objectReference: {fileID: 0}
+    - target: {fileID: -2845779275431937956, guid: c5470c9a1aff50643b6a9ff0bab4297d, type: 3}
+      propertyPath: m_IsActive
+      value: 0
+      objectReference: {fileID: 0}
     - target: {fileID: 919132149155446097, guid: c5470c9a1aff50643b6a9ff0bab4297d, type: 3}
       propertyPath: m_Name
       value: troop
@@ -197,6 +201,11 @@ MonoBehaviour:
   m_Script: {fileID: 11500000, guid: a2b30b6aa6e08c4468a02e1fcabcd485, type: 3}
   m_Name: 
   m_EditorClassIdentifier: 
+--- !u!4 &404755309 stripped
+Transform:
+  m_CorrespondingSourceObject: {fileID: -8679921383154817045, guid: c5470c9a1aff50643b6a9ff0bab4297d, type: 3}
+  m_PrefabInstance: {fileID: 404755305}
+  m_PrefabAsset: {fileID: 0}
 --- !u!1 &705507993
 GameObject:
   m_ObjectHideFlags: 0
@@ -388,7 +397,7 @@ MonoBehaviour:
   m_Script: {fileID: 11500000, guid: 9189b565e8ba9bfc3bdaa38149e3e13f, type: 3}
   m_Name: 
   m_EditorClassIdentifier: 
-  target: {fileID: 0}
+  target: {fileID: 404755309}
 --- !u!1 &1880773849
 GameObject:
   m_ObjectHideFlags: 0