feature: 模型绑定四肢和身体

Assets/Behaviours/BallStickBehaviour.cs

@@ -28,9 +28,12 @@ namespace Behaviours
         // Update is called once per frame
         private void Update()
         {
+            var mid = _poseTransforms[PoseLandmarkType.LeftHip] + _poseTransforms[PoseLandmarkType.RightHip];
+            mid = mid / 2;
+            
             for (var i = 0; i < PoseLandmarkType.MaxValue; i++)
             {
-                _nodes[i].transform.position = _poseTransforms[i].ResultPosition * -5;
+                _nodes[i].transform.position = (_poseTransforms[i].ResultPosition - mid) * -5f;
             }
 
             foreach (var bond in _bonds)
@@ -62,15 +65,15 @@ namespace Behaviours
 
                 if (i <= 10)
                 {
-                    ball.GetComponent<Renderer>().material.color = Color.red;//给头部添加颜色
+                    ball.GetComponent<Renderer>().material.color = Color.red; //给头部添加颜色
                 }
                 else if (i <= 22)
                 {
-                    ball.GetComponent<Renderer>().material.color = Color.blue;//给手部添加颜色
+                    ball.GetComponent<Renderer>().material.color = Color.blue; //给手部添加颜色
                 }
                 else if (i <= 32)
                 {
-                    ball.GetComponent<Renderer>().material.color = Color.green;//给脚部添加颜色
+                    ball.GetComponent<Renderer>().material.color = Color.green; //给脚部添加颜色
                 }
 
                 _nodes[i] = ball;

Assets/Behaviours/BoneBehaviour.cs

@@ -7,14 +7,21 @@ namespace Behaviours
     public class BoneBehaviour : MonoBehaviour
     {
         private readonly UdpListener _listener = new UdpListener();
+        private readonly GameObject[] _nodes = new GameObject[PoseLandmarkType.MaxValue];
+        private readonly List<Bond> _bonds = new List<Bond>();
         private Dictionary<HumanBodyBones, RotationNode> _rotationNodes;
+        private List<PoseLandmark> _landmarks;
 
         private Animator _animator;
+        private bool _isReceived;
+        private const float Scale = 0.2f;
 
         private void Start()
         {
             _animator = GetComponent<Animator>();
             CreateRotationNodes();
+            CreateNodes();
+            CreateBonds();
 
             _listener.AddHandler(OnReceive);
             _listener.Connect(5000);
@@ -22,9 +29,41 @@ namespace Behaviours
 
         private void Update()
         {
-            foreach (var node in _rotationNodes)
+            if (_isReceived)
             {
-                _animator.GetBoneTransform(node.Key).rotation = node.Value.Rotation;
+                _animator.GetBoneTransform(HumanBodyBones.Hips).rotation = _rotationNodes[HumanBodyBones.Hips].Rotation;
+
+                /*var rotation = _rotationNodes[HumanBodyBones.Hips].CalculateRotate
+                               * Quaternion.Inverse(_rotationNodes[HumanBodyBones.RightUpperArm].CalculateRotate);
+
+                _animator.GetBoneTransform(HumanBodyBones.RightUpperArm).Rotate(rotation.eulerAngles);*/
+                
+                var mid = (_landmarks[PoseLandmarkType.LeftHip].Position + 
+                           _landmarks[PoseLandmarkType.RightHip].Position) / 2;
+
+                for (var i = 0; i < PoseLandmarkType.MaxValue; i++)
+                {
+                    _nodes[i].transform.position = (_landmarks[i].Position - mid) * 5f;
+                }
+
+                foreach (var bond in _bonds)
+                {
+                    bond.UpdateBond();
+                }
+
+                _animator.GetBoneTransform(HumanBodyBones.LeftUpperArm).rotation = _bonds[9].Rotation;
+                _animator.GetBoneTransform(HumanBodyBones.LeftLowerArm).rotation = _bonds[10].Rotation;
+
+                _animator.GetBoneTransform(HumanBodyBones.RightUpperArm).rotation = _bonds[15].Rotation;
+                _animator.GetBoneTransform(HumanBodyBones.RightLowerArm).rotation = _bonds[16].Rotation;
+
+                _animator.GetBoneTransform(HumanBodyBones.LeftUpperLeg).rotation = _bonds[22].Rotation;
+                _animator.GetBoneTransform(HumanBodyBones.LeftLowerLeg).rotation = _bonds[23].Rotation;
+
+                _animator.GetBoneTransform(HumanBodyBones.RightUpperLeg).rotation = _bonds[28].Rotation;
+                _animator.GetBoneTransform(HumanBodyBones.RightLowerLeg).rotation = _bonds[29].Rotation;
+                
+                _isReceived = false;
             }
         }
 
@@ -39,12 +78,12 @@ namespace Behaviours
         {
             // 计算腰部
             var frontLeft = Vector3.Cross(
-                landmarks[PoseLandmarkType.RightHip].Vector3 - landmarks[PoseLandmarkType.LeftHip].Vector3,
+                landmarks[PoseLandmarkType.RightHip].Position - landmarks[PoseLandmarkType.LeftHip].Position,
-                landmarks[PoseLandmarkType.RightShoulder].Vector3 - landmarks[PoseLandmarkType.LeftHip].Vector3);
+                landmarks[PoseLandmarkType.RightShoulder].Position - landmarks[PoseLandmarkType.LeftHip].Position);
 
             var frontRight = Vector3.Cross(
-                landmarks[PoseLandmarkType.LeftShoulder].Vector3 - landmarks[PoseLandmarkType.RightHip].Vector3,
+                landmarks[PoseLandmarkType.LeftShoulder].Position - landmarks[PoseLandmarkType.RightHip].Position,
-                landmarks[PoseLandmarkType.LeftHip].Vector3 - landmarks[PoseLandmarkType.LeftHip].Vector3);
+                landmarks[PoseLandmarkType.LeftHip].Position - landmarks[PoseLandmarkType.LeftHip].Position);
 
             var front = frontLeft + frontRight;
             front.Normalize();
@@ -54,6 +93,14 @@ namespace Behaviours
                 oldRotation.UnityName,
                 oldRotation.Rotation,
                 Quaternion.LookRotation(front));
+
+            _rotationNodes[HumanBodyBones.RightUpperArm] = new RotationNode(
+                _rotationNodes[HumanBodyBones.RightUpperArm],
+                landmarks[PoseLandmarkType.RightShoulder],
+                landmarks[PoseLandmarkType.RightElbow]);
+
+            _landmarks = landmarks;
+            _isReceived = true;
         }
 
         private void CreateRotationNodes()
@@ -79,5 +126,132 @@ namespace Behaviours
                 _rotationNodes.Add(bone, new RotationNode(bone, rotation, rotation));
             }
         }
+        
+        private void CreateNodes()
+        {
+            for (var i = 0; i < PoseLandmarkType.MaxValue; 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[i] = ball;
+            }
+        }
+
+        private void CreateBonds()
+        {
+            var headBonds = new[]
+            {
+                PoseLandmarkType.RightEar,
+                PoseLandmarkType.RightEyeOuter,
+                PoseLandmarkType.RightEye,
+                PoseLandmarkType.RightEyeInner,
+                PoseLandmarkType.Nose,
+                PoseLandmarkType.LeftEyeInner,
+                PoseLandmarkType.LeftEye,
+                PoseLandmarkType.LeftEyeOuter,
+                PoseLandmarkType.LeftEar
+            };
+            _bonds.AddRange(GenerateBondsList(headBonds));
+
+            var monthBonds = new[]
+            {
+                PoseLandmarkType.MouthLeft,
+                PoseLandmarkType.MouthRight,
+            };
+            _bonds.AddRange(GenerateBondsList(monthBonds));
+
+            var leftArmBonds = new[]
+            {
+                PoseLandmarkType.LeftShoulder,
+                PoseLandmarkType.LeftElbow,
+                PoseLandmarkType.LeftWrist,
+                PoseLandmarkType.LeftPinky,
+                PoseLandmarkType.LeftIndex,
+                PoseLandmarkType.LeftWrist,
+                PoseLandmarkType.LeftThumb
+            };
+            _bonds.AddRange(GenerateBondsList(leftArmBonds));
+
+            var rightArmBonds = new[]
+            {
+                PoseLandmarkType.RightShoulder,
+                PoseLandmarkType.RightElbow,
+                PoseLandmarkType.RightWrist,
+                PoseLandmarkType.RightPinky,
+                PoseLandmarkType.RightIndex,
+                PoseLandmarkType.RightWrist,
+                PoseLandmarkType.RightThumb
+            };
+            _bonds.AddRange(GenerateBondsList(rightArmBonds));
+
+            var leftLegBonds = new[]
+            {
+                PoseLandmarkType.LeftShoulder,
+                PoseLandmarkType.LeftHip,
+                PoseLandmarkType.LeftKnee,
+                PoseLandmarkType.LeftAnkle,
+                PoseLandmarkType.LeftHeel,
+                PoseLandmarkType.LeftFootIndex,
+                PoseLandmarkType.LeftAnkle
+            };
+            _bonds.AddRange(GenerateBondsList(leftLegBonds));
+
+            var rightLegBonds = new[]
+            {
+                PoseLandmarkType.RightShoulder,
+                PoseLandmarkType.RightHip,
+                PoseLandmarkType.RightKnee,
+                PoseLandmarkType.RightAnkle,
+                PoseLandmarkType.RightHeel,
+                PoseLandmarkType.RightFootIndex,
+                PoseLandmarkType.RightAnkle
+            };
+            _bonds.AddRange(GenerateBondsList(rightLegBonds));
+
+            // 最后手动添加身体上的两条横线
+            _bonds.Add(new Bond(
+                _nodes[PoseLandmarkType.LeftShoulder],
+                _nodes[PoseLandmarkType.RightShoulder],
+                Scale));
+            _bonds.Add(new Bond(
+                _nodes[PoseLandmarkType.LeftHip],
+                _nodes[PoseLandmarkType.RightHip],
+                Scale));
+        }
+        
+        /// <summary>
+        /// 创建棍子列表
+        /// </summary>
+        /// <param name="nodes">需要连接起来的关键点 需要按顺序设置</param>
+        /// <returns></returns>
+        private List<Bond> GenerateBondsList(int[] nodes)
+        {
+            var bonds = new List<Bond>();
+
+            for (var i = 0; i < nodes.Length - 1; i++)
+            {
+                bonds.Add(new Bond(
+                    _nodes[nodes[i]],
+                    _nodes[nodes[i + 1]],
+                    Scale));
+            }
+
+            return bonds;
+        }
     }
 }

Assets/Models/Bond.cs

@@ -11,6 +11,8 @@ namespace Models
         protected readonly GameObject End;
         private readonly GameObject _bond;
 
+        public Quaternion Rotation => _bond.transform.rotation;
+        
         public Bond(GameObject start,GameObject end,float scale)
         {
             Start = start;

Assets/Models/PoseLandmark.cs

@@ -40,7 +40,7 @@ namespace Models
         /// </summary>
         public float Z { get; }
 
-        public Vector3 Vector3 => new Vector3(X, Y, Z);
+        public Vector3 Position => new Vector3(-X, -Y, -0.2f * Z);
 
         /// <summary>
         /// 该坐标点估计的可见性

Assets/Models/RotationNode.cs

@@ -4,11 +4,11 @@ namespace Models
 {
     public struct RotationNode
     {
-        public HumanBodyBones UnityName;
+        public readonly HumanBodyBones UnityName;
         public Quaternion PreviousRotation;
         public Quaternion Rotation;
 
-        public Quaternion CalculateRotate => PreviousRotation * Quaternion.Inverse(PreviousRotation);
+        public Quaternion CalculateRotate => PreviousRotation * Quaternion.Inverse(Rotation);
 
         public RotationNode(HumanBodyBones humanBodyBone, Quaternion previousRotation, Quaternion rotation)
         {
@@ -22,7 +22,7 @@ namespace Models
             UnityName = node.UnityName;
             PreviousRotation = node.Rotation;
 
-            var forward = new Vector3(end.X - begin.X, end.Y - begin.Y, end.Z - begin.Z);
+            var forward = end.Position - begin.Position;
             Rotation = Quaternion.LookRotation(forward);
         }
     }

Assets/Scenes/SampleScene.unity

@@ -136,11 +136,11 @@ PrefabInstance:
       objectReference: {fileID: 0}
     - target: {fileID: -8679921383154817045, guid: c5470c9a1aff50643b6a9ff0bab4297d, type: 3}
       propertyPath: m_LocalPosition.x
-      value: 0
+      value: -12.91
       objectReference: {fileID: 0}
     - target: {fileID: -8679921383154817045, guid: c5470c9a1aff50643b6a9ff0bab4297d, type: 3}
       propertyPath: m_LocalPosition.y
-      value: 0
+      value: -4.98
       objectReference: {fileID: 0}
     - target: {fileID: -8679921383154817045, guid: c5470c9a1aff50643b6a9ff0bab4297d, type: 3}
       propertyPath: m_LocalPosition.z
@@ -178,6 +178,10 @@ PrefabInstance:
       propertyPath: m_IsActive
       value: 0
       objectReference: {fileID: 0}
+    - target: {fileID: -464305122009946348, guid: c5470c9a1aff50643b6a9ff0bab4297d, type: 3}
+      propertyPath: m_LocalPosition.x
+      value: -4.32
+      objectReference: {fileID: 0}
     - target: {fileID: 919132149155446097, guid: c5470c9a1aff50643b6a9ff0bab4297d, type: 3}
       propertyPath: m_Name
       value: troop