MotionCapture/Assets/Models/PoseLandmark.cs

160 lines
4.3 KiB
C#
Raw Normal View History

2023-02-10 15:28:47 +08:00
using System;
using System.Collections.Generic;
using System.Text;
namespace Models
{
public class PoseLandmark
{
private const double Tolerance = 1E-6;
/// <summary>
/// 单个包的长度
/// </summary>
public const int PacketLength = 28;
/// <summary>
/// 坐标点的种类
/// </summary>
public PoseLandmarkType Type { get; }
/// <summary>
/// 真实世界x坐标
/// 以米为单位
2023-02-12 16:12:02 +08:00
/// 以臀部为重心 hip
2023-02-10 15:28:47 +08:00
/// </summary>
public float X { get; }
/// <summary>
/// 真实世界y坐标
/// 以米为单位
/// 以臀部为重心
/// </summary>
public float Y { get; }
/// <summary>
/// 真实世界z坐标
/// 以米为单位
/// 以臀部为重心
/// </summary>
public float Z { get; }
/// <summary>
/// 该坐标点估计的可见性
/// [0,1]
/// </summary>
public float Visibility { get; }
public long TimeStamp { get; }
public PoseLandmark(int type, float x, float y, float z, float visibility, long timeStamp)
2023-02-10 15:28:47 +08:00
{
Type = new PoseLandmarkType(type);
2023-02-10 15:28:47 +08:00
X = x;
Y = y;
Z = z;
Visibility = visibility;
TimeStamp = timeStamp;
}
/// <summary>
/// 转换成字节数组
/// 便于UDP发送
/// </summary>
/// <returns></returns>
public byte[] ToByteArray()
{
var result = new byte[PacketLength];
BitConverter.GetBytes(Type.Value).CopyTo(result, 0);
2023-02-10 15:28:47 +08:00
BitConverter.GetBytes(X).CopyTo(result, 4);
BitConverter.GetBytes(Y).CopyTo(result, 8);
BitConverter.GetBytes(Z).CopyTo(result, 12);
BitConverter.GetBytes(Visibility).CopyTo(result, 16);
BitConverter.GetBytes(TimeStamp).CopyTo(result, 20);
return result;
}
/// <summary>
/// 解析字节数组
/// </summary>
/// <param name="bytes">收到的字节数组</param>
/// <returns>字节数组中代表的坐标对象</returns>
public static PoseLandmark ValueOf(byte[] bytes)
{
var result = new PoseLandmark(
BitConverter.ToInt32(bytes, 0),
2023-02-10 15:28:47 +08:00
BitConverter.ToSingle(bytes, 4),
BitConverter.ToSingle(bytes, 8),
BitConverter.ToSingle(bytes, 12),
BitConverter.ToSingle(bytes, 16),
BitConverter.ToInt64(bytes, 20));
return result;
}
/// <summary>
/// 解析字节数组为对象列表
/// 单个
/// </summary>
/// <param name="bytes">字节数组</param>
/// <returns></returns>
public static List<PoseLandmark> ArrayOf(byte[] bytes)
{
var result = new List<PoseLandmark>();
for (var i = 0; i < bytes.Length; i = i + PacketLength)
{
var landmark = new PoseLandmark(BitConverter.ToInt32(bytes, i),
2023-02-10 15:28:47 +08:00
BitConverter.ToSingle(bytes, i + 4),
BitConverter.ToSingle(bytes, i + 8),
BitConverter.ToSingle(bytes, i + 12),
BitConverter.ToSingle(bytes, i + 16),
BitConverter.ToInt64(bytes, i + 20));
result.Add(landmark);
}
return result;
}
public override bool Equals(object obj)
{
if (obj is not PoseLandmark landmark)
{
return false;
}
else
{
return Type.Value == landmark.Type.Value
2023-02-10 15:28:47 +08:00
&& Math.Abs(X - landmark.X) < Tolerance
&& Math.Abs(Y - landmark.Y) < Tolerance
&& Math.Abs(Z - landmark.Z) < Tolerance
&& Math.Abs(Visibility - landmark.Visibility) < Tolerance
&& TimeStamp == landmark.TimeStamp;
}
}
public override int GetHashCode()
{
var hashCode = Type.GetHashCode();
hashCode ^= X.GetHashCode();
hashCode ^= Y.GetHashCode();
hashCode ^= Z.GetHashCode();
hashCode ^= Visibility.GetHashCode();
hashCode ^= TimeStamp.GetHashCode();
return hashCode;
}
public override string ToString()
{
var builder = new StringBuilder();
builder.Append($"Time: {TimeStamp}, Type: {Type}:\n");
builder.Append($"\tX:{X}, Y:{Y}, Z:{Z}, Visibility: {Visibility}\n");
return builder.ToString();
}
}
}