MotionCapture/Assets/Models/PoseLandmark.cs

using System;
using System.Collections.Generic;
using System.Text;
using UnityEngine;

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; }

        public Vector3 Position => new Vector3(-X, -Y, -0.2f * Z);

        /// <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();
        }
    }
}