96 lines
3.0 KiB
C#
96 lines
3.0 KiB
C#
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()
|
||
{
|
||
|
||
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;
|
||
}
|
||
}
|
||
|
||
|
||
|
||
}
|