From 77bf5230019787409fd372f57653f6eea125fe1a Mon Sep 17 00:00:00 2001 From: jackfiled Date: Wed, 29 Mar 2023 22:58:07 +0800 Subject: [PATCH] =?UTF-8?q?feature:=20=E8=88=B9=E6=96=B0=E7=9A=84=E5=9F=BA?= =?UTF-8?q?=E4=BA=8E=E8=BF=87=E7=A8=8B=E7=9A=84=E5=8A=A8=E4=BD=9C=E7=BB=91?= =?UTF-8?q?=E5=AE=9A?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Assets/Behaviours/BallStickBehaviour.cs | 295 +++++++++--------- Assets/Behaviours/BoneBehaviour.cs | 87 +++--- Assets/Models/PoseLandmark.cs | 3 + Assets/Models/RotationNode.cs | 29 ++ Assets/Models/RotationNode.cs.meta | 3 + .../PoseTransformHandlers/ZAxisHandler.cs | 3 +- 6 files changed, 226 insertions(+), 194 deletions(-) create mode 100644 Assets/Models/RotationNode.cs create mode 100644 Assets/Models/RotationNode.cs.meta diff --git a/Assets/Behaviours/BallStickBehaviour.cs b/Assets/Behaviours/BallStickBehaviour.cs index 2823943..62e20f8 100644 --- a/Assets/Behaviours/BallStickBehaviour.cs +++ b/Assets/Behaviours/BallStickBehaviour.cs @@ -3,178 +3,181 @@ using UnityEngine; using Utils.PoseTransformHandlers; -public class MainBehaviour : MonoBehaviour +namespace Behaviours { - private readonly GameObject[] _nodes = new GameObject[PoseLandmarkType.MaxValue]; - private readonly List _bonds = new List(); - private readonly PoseTransform[] _poseTransforms = new PoseTransform[PoseLandmarkType.MaxValue]; - private readonly UdpListener _listener = new UdpListener(); - private const float Scale = 0.2f; - - // Start is called before the first frame update - private void Start() + public class BallStickBehaviour : MonoBehaviour { - PoseTransform.AverageLength = 3; + private readonly GameObject[] _nodes = new GameObject[PoseLandmarkType.MaxValue]; + private readonly List _bonds = new List(); + private readonly PoseTransform[] _poseTransforms = new PoseTransform[PoseLandmarkType.MaxValue]; + private readonly UdpListener _listener = new UdpListener(); + private const float Scale = 0.2f; - CreateNodes(); - CreateBonds(); - - _listener.AddHandler(OnReceive); - _listener.Connect(5000); - } - - // Update is called once per frame - private void Update() - { - for (var i = 0; i < PoseLandmarkType.MaxValue; i++) + // Start is called before the first frame update + private void Start() { - _nodes[i].transform.position = _poseTransforms[i].ResultPosition * -5; + PoseTransform.AverageLength = 3; + + CreateNodes(); + CreateBonds(); + + _listener.AddHandler(OnReceive); + _listener.Connect(5000); } - foreach (var bond in _bonds) + // Update is called once per frame + private void Update() { - bond.UpdateBond(); - } - } + for (var i = 0; i < PoseLandmarkType.MaxValue; i++) + { + _nodes[i].transform.position = _poseTransforms[i].ResultPosition * -5; + } - private void OnDisable() - { - _listener.DisConnect(); - } + foreach (var bond in _bonds) + { + bond.UpdateBond(); + } + } + + private void OnDisable() + { + _listener.DisConnect(); + } - private void OnReceive(List landmarks) - { - foreach (var landmark in landmarks) + private void OnReceive(List landmarks) { - PoseTransform.UpdatePosition(ref _poseTransforms[landmark.Type.Value], landmark); + foreach (var landmark in landmarks) + { + PoseTransform.UpdatePosition(ref _poseTransforms[landmark.Type.Value], landmark); + } } - } - private void CreateNodes() - { - for (var i = 0; i < PoseLandmarkType.MaxValue; i++) + private void CreateNodes() { - var ball = GameObject.CreatePrimitive(PrimitiveType.Sphere); - - ball.transform.localScale = new Vector3(Scale, Scale, Scale); - - if (i <= 10) + for (var i = 0; i < PoseLandmarkType.MaxValue; i++) { - ball.GetComponent().material.color = Color.red;//给头部添加颜色 - } - else if (i <= 22) - { - ball.GetComponent().material.color = Color.blue;//给手部添加颜色 - } - else if (i <= 32) - { - ball.GetComponent().material.color = Color.green;//给脚部添加颜色 - } + var ball = GameObject.CreatePrimitive(PrimitiveType.Sphere); - _nodes[i] = ball; - _poseTransforms[i] = new PoseTransform(i, new AverageHandler()); + ball.transform.localScale = new Vector3(Scale, Scale, Scale); + + if (i <= 10) + { + ball.GetComponent().material.color = Color.red;//给头部添加颜色 + } + else if (i <= 22) + { + ball.GetComponent().material.color = Color.blue;//给手部添加颜色 + } + else if (i <= 32) + { + ball.GetComponent().material.color = Color.green;//给脚部添加颜色 + } + + _nodes[i] = ball; + _poseTransforms[i] = new PoseTransform(i, new AverageHandler()); + } } - } - private void CreateBonds() - { - var headBonds = new[] + private void CreateBonds() { - PoseLandmarkType.RightEar, - PoseLandmarkType.RightEyeOuter, - PoseLandmarkType.RightEye, - PoseLandmarkType.RightEyeInner, - PoseLandmarkType.Nose, - PoseLandmarkType.LeftEyeInner, - PoseLandmarkType.LeftEye, - PoseLandmarkType.LeftEyeOuter, - PoseLandmarkType.LeftEar - }; - _bonds.AddRange(GenerateBondsList(headBonds)); + 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 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 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 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 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)); + 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)); - } - - /// - /// 创建棍子列表 - /// - /// 需要连接起来的关键点 需要按顺序设置 - /// - private List GenerateBondsList(int[] nodes) - { - var bonds = new List(); - - for (var i = 0; i < nodes.Length - 1; i++) - { - bonds.Add(new Bond( - _nodes[nodes[i]], - _nodes[nodes[i + 1]], + // 最后手动添加身体上的两条横线 + _bonds.Add(new Bond( + _nodes[PoseLandmarkType.LeftShoulder], + _nodes[PoseLandmarkType.RightShoulder], + Scale)); + _bonds.Add(new Bond( + _nodes[PoseLandmarkType.LeftHip], + _nodes[PoseLandmarkType.RightHip], Scale)); } - return bonds; + /// + /// 创建棍子列表 + /// + /// 需要连接起来的关键点 需要按顺序设置 + /// + private List GenerateBondsList(int[] nodes) + { + var bonds = new List(); + + for (var i = 0; i < nodes.Length - 1; i++) + { + bonds.Add(new Bond( + _nodes[nodes[i]], + _nodes[nodes[i + 1]], + Scale)); + } + + return bonds; + } } } diff --git a/Assets/Behaviours/BoneBehaviour.cs b/Assets/Behaviours/BoneBehaviour.cs index d86b700..eb816ea 100644 --- a/Assets/Behaviours/BoneBehaviour.cs +++ b/Assets/Behaviours/BoneBehaviour.cs @@ -1,38 +1,35 @@ -using System; using System.Collections.Generic; using Models; using UnityEngine; -using Utils.PoseTransformHandlers; namespace Behaviours { public class BoneBehaviour : MonoBehaviour { private readonly UdpListener _listener = new UdpListener(); - private readonly PoseTransform[] _transforms = new PoseTransform[PoseLandmarkType.MaxValue]; - + private Dictionary _rotationNodes; + private Animator _animator; private void Start() { _animator = GetComponent(); + CreateRotationNodes(); - for (var i = 0; i < PoseLandmarkType.MaxValue; i++) - { - _transforms[i] = new PoseTransform(i, new ZAxisHandler()); - } - _listener.AddHandler(OnReceive); _listener.Connect(5000); } private void Update() { - HipUpdate(); - - RShoulderUpdate(); + foreach (var node in _rotationNodes) + { + _animator.GetBoneTransform(node.Key).rotation = node.Value.Rotation; + } } + + private void OnDisable() { _listener.DisConnect(); @@ -40,51 +37,47 @@ private void OnDisable() private void OnReceive(List landmarks) { - foreach (var landmark in 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]); + landmarks[PoseLandmarkType.RightHip].Vector3 - landmarks[PoseLandmarkType.LeftHip].Vector3, + landmarks[PoseLandmarkType.RightShoulder].Vector3 - landmarks[PoseLandmarkType.LeftHip].Vector3); var frontRight = Vector3.Cross( - _transforms[PoseLandmarkType.LeftShoulder] - _transforms[PoseLandmarkType.RightHip], - _transforms[PoseLandmarkType.LeftHip] - _transforms[PoseLandmarkType.RightHip]); + landmarks[PoseLandmarkType.LeftShoulder].Vector3 - landmarks[PoseLandmarkType.RightHip].Vector3, + landmarks[PoseLandmarkType.LeftHip].Vector3 - landmarks[PoseLandmarkType.LeftHip].Vector3); var front = frontLeft + frontRight; front.Normalize(); - _animator.GetBoneTransform(HumanBodyBones.Hips).rotation = Quaternion.LookRotation(front); + var oldRotation = _rotationNodes[HumanBodyBones.Hips]; + _rotationNodes[HumanBodyBones.Hips] = new RotationNode( + oldRotation.UnityName, + oldRotation.Rotation, + Quaternion.LookRotation(front)); } - - private void RShoulderUpdate() - { - var direction = _transforms[PoseLandmarkType.RightElbow] - - _transforms[PoseLandmarkType.RightShoulder]; - var absolutelyRotation = Quaternion.LookRotation(direction); - _animator.GetBoneTransform(HumanBodyBones.RightUpperArm).rotation - = absolutelyRotation * Quaternion.Inverse(_animator.GetBoneTransform(HumanBodyBones.Hips).rotation); - } - - - //计算向量a到向量b的旋转角 - //参数 a：起始向量; b：目标向量; n：旋转方向 (0, 1, 0)顺时针 (0, -1, 0)逆时针 - private static float SignedAngleBetween(Vector3 a, Vector3 b, Vector3 n) + private void CreateRotationNodes() { - 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; + _rotationNodes = new Dictionary(); + + var bonesArray = new[] + { + HumanBodyBones.Hips, + HumanBodyBones.LeftUpperArm, + HumanBodyBones.LeftLowerArm, + HumanBodyBones.RightUpperArm, + HumanBodyBones.RightLowerArm, + HumanBodyBones.LeftUpperLeg, + HumanBodyBones.LeftLowerLeg, + HumanBodyBones.RightUpperLeg, + HumanBodyBones.RightLowerLeg + }; + + foreach (var bone in bonesArray) + { + var rotation = _animator.GetBoneTransform(bone).rotation; + _rotationNodes.Add(bone, new RotationNode(bone, rotation, rotation)); + } } } - - - } diff --git a/Assets/Models/PoseLandmark.cs b/Assets/Models/PoseLandmark.cs index a68a987..c0f4e2c 100644 --- a/Assets/Models/PoseLandmark.cs +++ b/Assets/Models/PoseLandmark.cs @@ -1,6 +1,7 @@ using System; using System.Collections.Generic; using System.Text; +using UnityEngine; namespace Models { @@ -39,6 +40,8 @@ public class PoseLandmark /// public float Z { get; } + public Vector3 Vector3 => new Vector3(X, Y, Z); + /// /// 该坐标点估计的可见性 /// [0,1] diff --git a/Assets/Models/RotationNode.cs b/Assets/Models/RotationNode.cs new file mode 100644 index 0000000..2eedcee --- /dev/null +++ b/Assets/Models/RotationNode.cs @@ -0,0 +1,29 @@ +using UnityEngine; + +namespace Models +{ + public struct RotationNode + { + public HumanBodyBones UnityName; + public Quaternion PreviousRotation; + public Quaternion Rotation; + + public Quaternion CalculateRotate => PreviousRotation * Quaternion.Inverse(PreviousRotation); + + public RotationNode(HumanBodyBones humanBodyBone, Quaternion previousRotation, Quaternion rotation) + { + UnityName = humanBodyBone; + PreviousRotation = previousRotation; + Rotation = rotation; + } + + public RotationNode(RotationNode node, PoseLandmark begin, PoseLandmark end) + { + UnityName = node.UnityName; + PreviousRotation = node.Rotation; + + var forward = new Vector3(end.X - begin.X, end.Y - begin.Y, end.Z - begin.Z); + Rotation = Quaternion.LookRotation(forward); + } + } +} \ No newline at end of file diff --git a/Assets/Models/RotationNode.cs.meta b/Assets/Models/RotationNode.cs.meta new file mode 100644 index 0000000..20d9271 --- /dev/null +++ b/Assets/Models/RotationNode.cs.meta @@ -0,0 +1,3 @@ +fileFormatVersion: 2 +guid: 87e238cb51f34339a39945af13f43248 +timeCreated: 1680090845 \ No newline at end of file diff --git a/Assets/Utils/PoseTransformHandlers/ZAxisHandler.cs b/Assets/Utils/PoseTransformHandlers/ZAxisHandler.cs index 00d523e..918e581 100644 --- a/Assets/Utils/PoseTransformHandlers/ZAxisHandler.cs +++ b/Assets/Utils/PoseTransformHandlers/ZAxisHandler.cs @@ -9,7 +9,8 @@ public class ZAxisHandler : IPoseTransformHandler public void ReceivePoseLandmark(PoseLandmark landmark) { - _result = new Vector3(landmark.X, landmark.Y, landmark.Z * 0.5f); + _result = new Vector3(landmark.X, landmark.Y, landmark.Z * 0.2f); + _result = _result * -1; } public Vector3 GetResultPosition()