From 4cd3bad951d0c4ba6e037ec1884d50d8a47b58a7 Mon Sep 17 00:00:00 2001
From: Ichirinko <1621543655@qq.com>
Date: Sat, 25 Mar 2023 11:43:42 +0800
Subject: [PATCH] =?UTF-8?q?add:=E5=A2=9E=E5=8A=A0=E4=BA=86=E5=AD=90?=
 =?UTF-8?q?=E5=85=B3=E8=8A=82=E4=B8=8E=E7=88=B6=E5=85=B3=E8=8A=82=E7=9A=84?=
 =?UTF-8?q?=E8=AE=A1=E7=AE=97=E6=96=B9=E6=B3=95?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 Assets/Behaviours/BoneBehaviour.cs | 62 ++++++++++++++++++++++++------
 1 file changed, 50 insertions(+), 12 deletions(-)

diff --git a/Assets/Behaviours/BoneBehaviour.cs b/Assets/Behaviours/BoneBehaviour.cs
index 1a8437e..5d5dbcd 100644
--- a/Assets/Behaviours/BoneBehaviour.cs
+++ b/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(
-                _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);
+            HipUpdate();
+            
+            RShoulderUpdate();
         }
 
         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;
+        }
     }
+    
+    
+   
 }