charade_experiment/vrcharades-unity/vrcharades-master/Assets/Scripts/TrackWeights.cs

using UnityEngine;
using TMPro;
using UnityEngine.Assertions;
using static OVRFaceExpressions;
using System.Net;
using System.Net.Sockets;
using System.Text;
using System;
using System.Globalization;
using System.Collections.Generic;
using System.Collections.Concurrent;
using System.Threading;

[RequireComponent(typeof(TMP_Text))]
[DefaultExecutionOrder(140)]
public class TrackWeights : MonoBehaviour
{
    private UdpClient sender;
    public IPEndPoint serverEndpoint;
    private float timeUntilNextUpdate = 0.0f;

    [SerializeField]
    private OVRBody ovrBody;

    [SerializeField]
    private Transform leftEye;
    [SerializeField]
    private Transform rightEye;

    [SerializeField]
    private Transform[] bones;

    [SerializeField]
    private Transform centerEyeAnchor;

    [SerializeField]
    private Transform leftHandAnchor;

    [SerializeField]
    private Transform rightHandAnchor;

    private ConcurrentQueue<string> sendQueue = new ConcurrentQueue<string>();


    // void FixedUpdate()
    // {
    //     EnsureInitialize();
    //     if (!IsValid)
    //     {
    //         return;
    //     }

    //     StringBuilder sb = new(1024);
    //     for (int i = (int)FaceExpression.BrowLowererL; i < (int)FaceExpression.Max; i++)
    //     {
    //         // float v = _ovrFaceExpressions[(FaceExpression)i];
    //         sb.Append(_allWeights[i].ToString("F4"));
    //         sb.Append(';');
    //     }

    //     byte[] data = Encoding.UTF8.GetBytes(sb.ToString());
    //     sender.Send(data, data.Length, serverEndpoint);
    // }



    private TMP_Text text;


    float[] _allWeights = null;
    string[] _weightNames = null;

    private Thread senderThread;

    /// <summary>
    /// The face expressions provider to source from.
    /// </summary>
    [SerializeField]
    protected OVRFaceExpressions _ovrFaceExpressions;
    /// <inheritdoc cref="_ovrFaceExpressions"/>
    public OVRFaceExpressions OVRFaceExpressionComp
    {
        get => _ovrFaceExpressions;
        set => _ovrFaceExpressions = value;
    }

    private void Awake()
    {
        Assert.IsNotNull(_ovrFaceExpressions);
    }

    private void EnsureInitialize()
    {
        if (_allWeights == null)
        {
            _allWeights = new float[(int)FaceExpression.Max];
        }
        if (_weightNames == null)
        {
            _weightNames = new string[(int)FaceExpression.Max];
            for (int i = (int)FaceExpression.BrowLowererL; i < (int)FaceExpression.Max; i++)
            {
                _weightNames[i] = ((FaceExpression)i).ToString();
            }
        }
    }

    /// <inheritdoc />
    public bool IsValid =>
        _ovrFaceExpressions.enabled &&
        _ovrFaceExpressions.FaceTrackingEnabled &&
        _ovrFaceExpressions.ValidExpressions;


    // Start is called once before the first execution of Update after the MonoBehaviour is created
    private void Start()
    {
        text = gameObject.GetComponent<TMP_Text>();

        sender = new UdpClient();
        sender.Client.Bind(new IPEndPoint(IPAddress.Any, 5001));
        serverEndpoint = new IPEndPoint(IPAddress.Parse("0.0.0.0"), 5000);

        // senderThread = new Thread(SendData);
        // senderThread.IsBackground = true;
        // senderThread.Start();
    }

    void SendData()
    {
        byte[] data = Encoding.UTF8.GetBytes(sb.ToString());
        sender.Send(data, data.Length, serverEndpoint);
    }

    private StringBuilder sb = new(1024 * 10);

    // Update is called once per frame
    private void Update()
    {
        EnsureInitialize();
        if (!IsValid)
        {
            return;
        }

        timeUntilNextUpdate -= Time.deltaTime;
        if (timeUntilNextUpdate <= 0.0f)
        {
            timeUntilNextUpdate = 1.0f / 60.0f;
            sb.Clear();
            for (int i = (int)FaceExpression.BrowLowererL; i < (int)FaceExpression.Max; i++)
            {
                _allWeights[i] = _ovrFaceExpressions[(FaceExpression)i];
                if (i > 0)
                {
                    sb.Append(';');
                }
                sb.Append(_allWeights[i].ToString("F4"));
            }

            OVRSkeleton.IOVRSkeletonDataProvider dataProvider = ovrBody as OVRSkeleton.IOVRSkeletonDataProvider;
            OVRSkeleton.SkeletonPoseData poseData = dataProvider.GetSkeletonPoseData();

            OVRPlugin.Quatf rootOrientation = poseData.RootPose.Orientation;
            OVRPlugin.Vector3f rootPosition = poseData.RootPose.Position;

            sb.Append(';');
            sb.Append(rootOrientation.w.ToString("F4"));
            sb.Append(';');
            sb.Append(rootOrientation.x.ToString("F4"));
            sb.Append(';');
            sb.Append(rootOrientation.y.ToString("F4"));
            sb.Append(';');
            sb.Append(rootOrientation.z.ToString("F4"));

            sb.Append(';');
            sb.Append(rootPosition.x.ToString("F4"));
            sb.Append(';');
            sb.Append(rootPosition.y.ToString("F4"));
            sb.Append(';');
            sb.Append(rootPosition.z.ToString("F4"));

            sb.Append(';');
            sb.Append(poseData.RootScale.ToString("F4"));

            sb.Append(';');
            sb.Append(poseData.BoneRotations.Length.ToString());
            foreach (OVRPlugin.Quatf rot in poseData.BoneRotations)
            {
                sb.Append(';');
                sb.Append(rot.w.ToString("F4"));
                sb.Append(';');
                sb.Append(rot.x.ToString("F4"));
                sb.Append(';');
                sb.Append(rot.y.ToString("F4"));
                sb.Append(';');
                sb.Append(rot.z.ToString("F4"));
            }

            sb.Append(';');
            sb.Append((poseData.IsDataValid ? 1 : 0).ToString());

            sb.Append(';');
            sb.Append((poseData.IsDataHighConfidence ? 1 : 0).ToString());

            sb.Append(';');
            sb.Append(poseData.BoneTranslations.Length.ToString());
            foreach (OVRPlugin.Vector3f trans in poseData.BoneTranslations)
            {
                sb.Append(';');
                sb.Append(trans.x.ToString("F4"));
                sb.Append(';');
                sb.Append(trans.y.ToString("F4"));
                sb.Append(';');
                sb.Append(trans.z.ToString("F4"));
            }

            sb.Append(';');
            sb.Append(poseData.SkeletonChangedCount.ToString());

            sb.Append(';');
            sb.Append(leftEye.localRotation.w.ToString("F4"));
            sb.Append(';');
            sb.Append(leftEye.localRotation.x.ToString("F4"));
            sb.Append(';');
            sb.Append(leftEye.localRotation.y.ToString("F4"));
            sb.Append(';');
            sb.Append(leftEye.localRotation.z.ToString("F4"));

            sb.Append(';');
            sb.Append(rightEye.localRotation.w.ToString("F4"));
            sb.Append(';');
            sb.Append(rightEye.localRotation.x.ToString("F4"));
            sb.Append(';');
            sb.Append(rightEye.localRotation.y.ToString("F4"));
            sb.Append(';');
            sb.Append(rightEye.localRotation.z.ToString("F4"));

            // Center Eye Camera (Position + Rotation)
            if (centerEyeAnchor != null)
            {
                sb.Append(';');
                sb.Append(centerEyeAnchor.position.x.ToString("F4"));
                sb.Append(';');
                sb.Append(centerEyeAnchor.position.y.ToString("F4"));
                sb.Append(';');
                sb.Append(centerEyeAnchor.position.z.ToString("F4"));

                sb.Append(';');
                sb.Append(centerEyeAnchor.rotation.w.ToString("F4"));
                sb.Append(';');
                sb.Append(centerEyeAnchor.rotation.x.ToString("F4"));
                sb.Append(';');
                sb.Append(centerEyeAnchor.rotation.y.ToString("F4"));
                sb.Append(';');
                sb.Append(centerEyeAnchor.rotation.z.ToString("F4"));
            }
            else
            {
                // Send zeros if centerEyeAnchor is not set
                for (int i = 0; i < 7; i++)
                {
                    sb.Append(';');
                    sb.Append("0.0000");
                }
            }

            // Left Hand Controller (Position + Rotation)
            if (leftHandAnchor != null)
            {
                sb.Append(';');
                sb.Append(leftHandAnchor.position.x.ToString("F4"));
                sb.Append(';');
                sb.Append(leftHandAnchor.position.y.ToString("F4"));
                sb.Append(';');
                sb.Append(leftHandAnchor.position.z.ToString("F4"));

                sb.Append(';');
                sb.Append(leftHandAnchor.rotation.w.ToString("F4"));
                sb.Append(';');
                sb.Append(leftHandAnchor.rotation.x.ToString("F4"));
                sb.Append(';');
                sb.Append(leftHandAnchor.rotation.y.ToString("F4"));
                sb.Append(';');
                sb.Append(leftHandAnchor.rotation.z.ToString("F4"));
            }
            else
            {
                // Send zeros if leftHandAnchor is not set
                for (int i = 0; i < 7; i++)
                {
                    sb.Append(';');
                    sb.Append("0.0000");
                }
            }

            // Right Hand Controller (Position + Rotation)
            if (rightHandAnchor != null)
            {
                sb.Append(';');
                sb.Append(rightHandAnchor.position.x.ToString("F4"));
                sb.Append(';');
                sb.Append(rightHandAnchor.position.y.ToString("F4"));
                sb.Append(';');
                sb.Append(rightHandAnchor.position.z.ToString("F4"));

                sb.Append(';');
                sb.Append(rightHandAnchor.rotation.w.ToString("F4"));
                sb.Append(';');
                sb.Append(rightHandAnchor.rotation.x.ToString("F4"));
                sb.Append(';');
                sb.Append(rightHandAnchor.rotation.y.ToString("F4"));
                sb.Append(';');
                sb.Append(rightHandAnchor.rotation.z.ToString("F4"));
            }
            else
            {
                // Send zeros if rightHandAnchor is not set
                for (int i = 0; i < 7; i++)
                {
                    sb.Append(';');
                    sb.Append("0.0000");
                }
            }

            // foreach (var bone in bones)
            // {
            //     sb.Append(';');
            //     sb.Append(bone.localPosition.x.ToString("F4"));
            //     sb.Append(';');
            //     sb.Append(bone.localPosition.y.ToString("F4"));
            //     sb.Append(';');
            //     sb.Append(bone.localPosition.z.ToString("F4"));

            //     sb.Append(';');
            //     sb.Append(bone.localRotation.w.ToString("F4"));
            //     sb.Append(';');
            //     sb.Append(bone.localRotation.x.ToString("F4"));
            //     sb.Append(';');
            //     sb.Append(bone.localRotation.y.ToString("F4"));
            //     sb.Append(';');
            //     sb.Append(bone.localRotation.z.ToString("F4"));

            //     sb.Append(';');
            //     sb.Append(bone.localScale.x.ToString("F4"));
            //     sb.Append(';');
            //     sb.Append(bone.localScale.y.ToString("F4"));
            //     sb.Append(';');
            //     sb.Append(bone.localScale.z.ToString("F4"));
            // }


            // sendQueue.Enqueue(sb.ToString());

            byte[] data = Encoding.UTF8.GetBytes(sb.ToString());
            sender.Send(data, data.Length, serverEndpoint);
        }

        // text.text = "";
        // for (int i = (int)FaceExpression.BrowLowererL; i < (int)FaceExpression.Max; i++)
        // {
        //     _allWeights[i] = _ovrFaceExpressions[(FaceExpression)i];

        //     text.text += $"{_weightNames[i]}: {_allWeights[i]:F3}\n";
        // }
    }
    
    void OnApplicationQuit()
    {
        senderThread?.Abort();
        sender?.Close();
    }
}