using System.Collections.Generic;
using UnityEngine;
using System;
namespace Models
{
///
/// 捕捉骨骼的动作转换
///
public struct PoseTransform
{
///
/// MediaPipe中动捕节点标识
///
public readonly PoseLandmarkType MediaPipeName;
///
/// Unity中绑定骨骼名称
///
public readonly HumanBodyBones UnityName;
///
/// MediaPipe捕捉空间坐标
///
public Vector3 MediaPipePos;
///
/// 节点的父节点列表
///
public readonly List Parent;
///
/// 骨骼节点的初始角度
///
public Quaternion PreviousQuaternion;
///
/// 骨骼节点的当前角度
///
public Quaternion CurrentQuaternion;
public PoseTransform(
PoseLandmarkType type
)
{
MediaPipeName = type;
UnityName = GetRelatedBone(type);
MediaPipePos = new Vector3();
Parent = new List();
PreviousQuaternion = new Quaternion();
CurrentQuaternion = new Quaternion();
}
///
/// 计算当前的旋转四元数
///
///
public void CalculateCurrentQuaternion(PoseLandmark landmark)
{
//var temp = _posLandmarks.Find(t=>t.mediaPipeName == landmark.Type.ToString());
var sum = new Vector3(0,0,0);
if (landmark.Visibility > 0.6)
{
var temp = new Vector4(landmark.X, landmark.Y, landmark.Z, 1);
MediaPipePos = new Vector3(-temp.x, -temp.y, -temp.z);
}
if (UnityName != HumanBodyBones.LastBone)
{
Debug.Log(UnityName+".MediaPipePos:"+MediaPipePos);
}
foreach (var parents in Parent)
{
sum += parents.MediaPipePos;
//Debug.Log("parents:"+parents.UnityName+".MediaPipePos:"+parents.MediaPipePos);
}
//Debug.Log("sum:"+sum);
var currentPos = sum / Parent.Count;
CurrentQuaternion = Quaternion.LookRotation(MediaPipePos - currentPos);
}
///
/// 获得同相关捕捉点关联的骨骼
///
/// 捕捉点的种类
/// 关联骨骼的种类
private static HumanBodyBones GetRelatedBone(PoseLandmarkType type)
{
switch (type)
{
case PoseLandmarkType.LeftHip:
return HumanBodyBones.LeftUpperLeg;
case PoseLandmarkType.RightHip:
return HumanBodyBones.RightUpperLeg;
case PoseLandmarkType.LeftShoulder:
return HumanBodyBones.LeftUpperArm;
case PoseLandmarkType.RightShoulder:
return HumanBodyBones.RightUpperArm;
case PoseLandmarkType.Nose:
return HumanBodyBones.Head;
case PoseLandmarkType.LeftElbow:
return HumanBodyBones.LeftLowerArm;
case PoseLandmarkType.RightElbow:
return HumanBodyBones.RightLowerArm;
case PoseLandmarkType.LeftWrist:
return HumanBodyBones.LeftHand;
case PoseLandmarkType.RightWrist:
return HumanBodyBones.RightHand;
case PoseLandmarkType.LeftKnee:
return HumanBodyBones.LeftLowerLeg;
case PoseLandmarkType.RightKnee:
return HumanBodyBones.RightLowerLeg;
case PoseLandmarkType.LeftAnkle:
return HumanBodyBones.LeftFoot;
case PoseLandmarkType.RightAnkle:
return HumanBodyBones.RightFoot;
case PoseLandmarkType.LeftFootIndex:
return HumanBodyBones.LeftToes;
case PoseLandmarkType.RightFootIndex:
return HumanBodyBones.RightToes;
default:
return HumanBodyBones.LastBone;
}
}
//绕X轴旋转
private Matrix4x4 RotateAxisX(float rot)
{
var matrix = new Matrix4x4();
float cosrot = MathF.Cos(rot);
float sinrot = MathF.Sin(rot);
matrix.SetRow(0, new Vector4(1,0,0,0));
matrix.SetRow(1, new Vector4(0,cosrot,-sinrot,0));
matrix.SetRow(2, new Vector4(0,sinrot,cosrot,0));
matrix.SetRow(3, new Vector4(0,0,0,1));
return matrix;
}
//绕Y轴旋转
private Matrix4x4 RotateAxisY(float rot)
{
var matrix = new Matrix4x4();
float cosrot = MathF.Cos(rot);
float sinrot = MathF.Sin(rot);
matrix.SetRow(0, new Vector4(cosrot,0,sinrot,0));
matrix.SetRow(1, new Vector4(0,1,0,0));
matrix.SetRow(2, new Vector4(-sinrot,0,cosrot,0));
matrix.SetRow(3, new Vector4(0,0,0,1));
return matrix;
}
//绕Z轴旋转
private Matrix4x4 RotateAxisZ(float rot)
{
var matrix = new Matrix4x4();
float cosrot = MathF.Cos(rot);
float sinrot = MathF.Sin(rot);
matrix.SetRow(0, new Vector4(cosrot,-sinrot,0,0));
matrix.SetRow(1, new Vector4(sinrot,cosrot,0,0));
matrix.SetRow(2, new Vector4(0,0,1,0));
matrix.SetRow(3, new Vector4(0,0,0,1));
return matrix;
}
}
}