MotionCapture/Assets/Models/PoseLandmark.cs

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坐标
    /// 以米为单位
    /// 以臀部为重心 hip
    /// </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)
    {
        Type = new PoseLandmarkType(type);
        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);
        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),
            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),
                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
                   && 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();
    }
    }
}