From 87831fd869530495826d417a76dbbd97ac666c0e Mon Sep 17 00:00:00 2001
From: Ichirinko <1621543655@qq.com>
Date: Tue, 28 Feb 2023 22:59:38 +0800
Subject: [PATCH] =?UTF-8?q?fix:=E5=B0=86=E7=88=B6=E8=8A=82=E7=82=B9?=
 =?UTF-8?q?=E7=9A=84=E7=B1=BB=E5=9E=8B=E6=94=B9=E4=B8=BAPoseLandmarkType?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 Assets/ModelBehaviour.cs       | 27 ++++++++++++++++++++-------
 Assets/Models/PoseTransform.cs | 13 +++++++++++--
 2 files changed, 31 insertions(+), 9 deletions(-)

diff --git a/Assets/ModelBehaviour.cs b/Assets/ModelBehaviour.cs
index 3db17e3..56a81dd 100644
--- a/Assets/ModelBehaviour.cs
+++ b/Assets/ModelBehaviour.cs
@@ -7,14 +7,14 @@ public class ModelBehaviour : MonoBehaviour
     private Animator _animator;
 
     private readonly UdpListener _listener = new UdpListener();
-    private static readonly List<PoseTransform> PoseTransforms = new List<PoseTransform>();
+    private static readonly PoseTransform[] PoseTransforms = new PoseTransform[(int)PoseLandmarkType.MaxValue];
 
     private void Start()
     {
         _animator = GetComponent<Animator>(); // 获取动画控件
         InitPoseTransformList();
 
-        _listener.AddHandler(LogLandmarks);
+        //_listener.AddHandler(LogLandmarks);
         _listener.AddHandler(RigPoint);
         _listener.Connect(5000);
     }
@@ -28,9 +28,19 @@ private void Update()
         {
             if (landmark.UnityName != HumanBodyBones.LastBone)
             {
+                var transfrom = _animator.GetBoneTransform(landmark.UnityName);
+
+                var parentsum = new Vector3(0, 0, 0);
+
+                foreach (var parent in landmark.Parent)
+                {
+                    parentsum += PoseTransforms[(int)parent].AveragePos;
+                }
+
+                parentsum /= landmark.Parent.Count;
                 
-                 _animator.GetBoneTransform(landmark.UnityName).rotation = landmark.CurrentQuaternion;
-                 
+                transfrom.rotation = Quaternion.FromToRotation(transfrom.rotation.eulerAngles,landmark.AveragePos-parentsum);
+
             }
         }
 
@@ -44,7 +54,7 @@ private void OnDisable()
 
     private static void LogLandmarks(List<PoseLandmark> landmarks)
     {
-        Debug.Log("Here");
+        
         foreach (var landmark in landmarks)
         {
             Debug.Log(landmark.ToString());
@@ -59,6 +69,9 @@ private static void RigPoint(List<PoseLandmark> landmarks)
         foreach (var landmark in landmarks)
         {
             
+            PoseTransform.UpdatePosition(ref PoseTransforms[(int)landmark.Type], landmark);
+            
+            Debug.Log(PoseTransforms[(int)landmark.Type].UnityName+":"+PoseTransforms[(int)landmark.Type].AveragePos);
         }
 
     }
@@ -78,7 +91,7 @@ private void InitPoseTransformList()
                 item.CurrentQuaternion = item.PreviousQuaternion;
             }
             
-            PoseTransforms.Add(item);
+            PoseTransforms[type] = item;
         }
         
         // 在这里添加父节点
@@ -115,7 +128,7 @@ private void InitPoseTransformList()
     private void AddParentTransform(PoseLandmarkType target, PoseLandmarkType parent)
     {
         var parents = PoseTransforms[(int)target].Parent;
-        parents.Add(PoseTransforms[(int)parent]);
+        parents.Add(PoseTransforms[(int)parent].MediaPipeName);
     }
 
 }
diff --git a/Assets/Models/PoseTransform.cs b/Assets/Models/PoseTransform.cs
index b65351e..fbe50e1 100644
--- a/Assets/Models/PoseTransform.cs
+++ b/Assets/Models/PoseTransform.cs
@@ -1,6 +1,7 @@
 using System.Collections.Generic;
 using UnityEngine;
 using System;
+using System.Collections;
 
 namespace Models
 {
@@ -32,7 +33,7 @@ public struct PoseTransform
         /// <summary>
         /// 节点的父节点列表
         /// </summary>
-        public readonly List<PoseTransform> Parent;
+        public readonly List<PoseLandmarkType> Parent;
 
         /// <summary>
         /// 骨骼节点的初始角度
@@ -55,7 +56,7 @@ public PoseTransform(
             MediaPipeName = type;
             UnityName = GetRelatedBone(type);
             AveragePos = new Vector3();
-            Parent = new List<PoseTransform>();
+            Parent = new List<PoseLandmarkType>();
             PreviousQuaternion = new Quaternion();
             CurrentQuaternion = new Quaternion();
             AverageLength = averageLength;
@@ -78,12 +79,20 @@ public static void UpdatePosition(ref PoseTransform pose, PoseLandmark landmark)
 
                 var sum = new Vector3();
 
+                var parentsum = new Vector3(0f,0f,0f);
+
                 foreach (var poseLandmark in pose._landmarkQueue)
                 {
                     sum += new Vector3(poseLandmark.X, poseLandmark.Y, poseLandmark.Z);
                 }
 
                 pose.AveragePos = sum / AverageLength;
+                
+                
+
+                
+                //pose.CurrentQuaternion = Quaternion.LookRotation(pose.AveragePos - parentsum).normalized;
+               
             }
         }