MotionCapture/Assets/Behaviours/BoneBehaviour.cs

91 lines
2.9 KiB
C#
Raw Normal View History

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 Animator _animator;
private void Start()
{
_animator = GetComponent<Animator>();
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();
}
private void OnDisable()
{
_listener.DisConnect();
}
private void OnReceive(List<PoseLandmark> 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]);
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()
{
2023-03-27 13:11:01 +08:00
var direction = _transforms[PoseLandmarkType.RightElbow]
- _transforms[PoseLandmarkType.RightShoulder];
var absolutelyRotation = Quaternion.LookRotation(direction);
2023-03-27 13:11:01 +08:00
_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)
{
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;
}
}
}