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 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; + } } + + + }