add:增加了子关节与父关节的计算方法

Assets/Behaviours/BoneBehaviour.cs

@@ -1,3 +1,4 @@
+using System;
 using System.Collections.Generic;
 using Models;
 using UnityEngine;
@@ -27,18 +28,9 @@ private void Start()
 
         private void Update()
         {
-            var frontLeft = Vector3.Cross(
+            HipUpdate();
-                _transforms[PoseLandmarkType.RightHip] - _transforms[PoseLandmarkType.LeftHip],
-                _transforms[PoseLandmarkType.RightShoulder] - _transforms[PoseLandmarkType.LeftHip]);
             
-            var frontRight = Vector3.Cross(
+            RShoulderUpdate();
-                _transforms[PoseLandmarkType.LeftShoulder] - _transforms[PoseLandmarkType.RightHip],
-                _transforms[PoseLandmarkType.LeftHip] - _transforms[PoseLandmarkType.RightHip]);
-
-            var front = frontLeft + frontRight;
-            front.Normalize();
-
-            _animator.GetBoneTransform(HumanBodyBones.Hips).rotation = Quaternion.LookRotation(front);
         }
 
         private void OnDisable()
@@ -53,5 +45,51 @@ private void OnReceive(List<PoseLandmark> landmarks)
                 PoseTransform.UpdatePosition(ref _transforms[landmark.Type.Value], landmark);
             }
         }
+
+
+        private void HipUpdate()
+        {
+            var frontLeft = Vector3.Cross(
+                _transforms[PoseLandmarkType.RightHip] - _transforms[PoseLandmarkType.LeftHip],
+                _transforms[PoseLandmarkType.RightShoulder] - _transforms[PoseLandmarkType.LeftHip]);
+
+            var frontRight = Vector3.Cross(
+                _transforms[PoseLandmarkType.LeftShoulder] - _transforms[PoseLandmarkType.RightHip],
+                _transforms[PoseLandmarkType.LeftHip] - _transforms[PoseLandmarkType.RightHip]);
+
+            var front = frontLeft + frontRight;
+            front.Normalize();
+
+            _animator.GetBoneTransform(HumanBodyBones.Hips).rotation = Quaternion.LookRotation(front);
         }
+        
+        private void RShoulderUpdate()
+        {
+
+            var relativeRot = _transforms[PoseLandmarkType.RightElbow] - _transforms[PoseLandmarkType.RightShoulder];
+            relativeRot.Normalize();
+
+            var hipRot = _animator.GetBoneTransform(HumanBodyBones.Hips).rotation;
+            hipRot.Normalize();
+
+            var interal = Quaternion.FromToRotation(relativeRot, hipRot.eulerAngles);
+            interal.Normalize();
+
+            _animator.GetBoneTransform(HumanBodyBones.RightUpperArm).rotation =  interal; //Quaternion.LookRotation(relativeRot + hipRot);
+        }
+        
+        
+        //计算向量a到向量b的旋转角
+        //参数 a：起始向量; b：目标向量; n：旋转方向 (0, 1, 0)顺时针  (0, -1, 0)逆时针
+        private static float SignedAngleBetween(Vector3 a, Vector3 b, Vector3 n)
+        {
+            var angle = Vector3.Angle(a,b);
+            var sign = Mathf.Sign(Vector3.Dot(n,Vector3.Cross(a,b)));
+            var signedAngle = angle * sign;
+            return (signedAngle <= 0) ? 360 + signedAngle : signedAngle;
+        }
+    }
+    
+    
+   
 }