OSCCollar/OSCCollar/OSCCollar.cs

using BuildSoft.OscCore;

namespace VRC_Console;

public class OSCCollar
{

    public static void Main(string[] args)
    {
        OSCCollar collar = new OSCCollar();
    }
    
    private OscServer Server { get; init; }
    private OscClient Client { get; init; }
    private float _leashStretch;
    private double _posX, _posY, _verticalMovement, _horizontalMovement;
    private bool _allowMoving = true;
    private uint _nilSent;
    private DateTime _lastNilMessageSent = DateTime.UnixEpoch;
    private DateTime _lastConsoleOutput = DateTime.UnixEpoch;
    private static readonly TimeSpan ConsoleUpdateInterval = TimeSpan.FromMilliseconds(100);
    private static readonly TimeSpan UpdateInterval = TimeSpan.FromMilliseconds(1);
    private static readonly TimeSpan MessageMinInterval = TimeSpan.FromMilliseconds(400);
    private double radius;
    private GPS GPS1 { get; init; }
    private GPS GPS2 { get; init; }
    private GPS GPS3 { get; init; }
    private double CalibrationX { get; init; }
    private double CalibrationY { get; init; }
    private string debugValue = "";
    private double GPSConstantA { get; init; }
    private double GPSConstantB { get; init; }
    private double GPSConstantC { get; init; }
    private double GPSConstantD { get; init; }
    private double GPSConstantE { get; init; }

    private OSCCollar(string ip = "127.0.0.1", int portReceive = 9001, int portSend = 9000, double radius = 100, double calibrationX = 0, double calibrationY = 0, bool skipSetup = true)
    {
        this.radius = radius;
        this.GPS1 = new(0, -radius);
        this.GPS2 = new(-(radius * 0.866), radius / 2);
        this.GPS3 = new(radius * 0.866, radius / 2);
        this.CalibrationX = calibrationX;
        this.CalibrationY = calibrationY;
        this.GPSConstantA = 2 * GPS2.X - 2 * GPS1.X;
        this.GPSConstantB = 2 * GPS2.Y - 2 * GPS1.Y;
        this.GPSConstantC = Math.Pow(GPS1.X, 2) + Math.Pow(GPS2.X, 2) - Math.Pow(GPS1.Y, 2) + Math.Pow(GPS2.Y, 2);
        this.GPSConstantD = 2 * GPS3.X - 2 * GPS2.X;
        this.GPSConstantE = Math.Pow(GPS2.X, 2) + Math.Pow(GPS3.X, 2) - Math.Pow(GPS2.Y, 2) + Math.Pow(GPS3.Y, 2);
        

        if (!skipSetup)
        {
            Console.WriteLine("Position your GPS receivers:");
            Console.WriteLine($"GPS 1 x: {GPS1.X} y: {GPS1.Y}");
            Console.WriteLine($"GPS 2 x: {GPS2.X} y: {GPS2.Y}");
            Console.WriteLine($"GPS 3 x: {GPS3.X} y: {GPS3.Y}");
            Console.WriteLine($"Radius of each receiver (sphere): {radius * 2}");
        }
        
        this.Server = new OscServer(portReceive);
        this.Client = new OscClient(ip, portSend);
        this.Server.TryAddMethod("/avatar/parameters/GPS1", GPS1Handle);
        this.Server.TryAddMethod("/avatar/parameters/GPS2", GPS2Handle);
        this.Server.TryAddMethod("/avatar/parameters/GPS3", GPS3Handle);
        this.Server.TryAddMethod("/avatar/parameters/Leash_Stretch", CollarStretchHandle);
        this.Server.TryAddMethod("/avatar/parameters/Leash_Toggle", AllowMovingHandle);
        this.Server.Start();

        Thread runningThread = new Thread(RunningThread);
        runningThread.Start();
    }

    private void PrintOutput(double verticalMovement, double horizontalMovement)
    {
        Console.Clear();
        Console.ForegroundColor = ConsoleColor.White;
        Console.WriteLine($"OSC Collar - Status: {(_allowMoving ? "enabled" : "disabled")}");
        Console.WriteLine("==============================");
        Console.WriteLine($"GPS 1:...............{GPS1.Distance:0.000000}");
        Console.WriteLine($"GPS 2:...............{GPS2.Distance:0.000000}");
        Console.WriteLine($"GPS 3:...............{GPS3.Distance:0.000000}");
        Console.WriteLine($"Position X:..........{_posX:0.000000}");
        Console.WriteLine($"Position Y:..........{_posY:0.000000}");
        Console.WriteLine($"Stretch:.............{_leashStretch:0.0000}");
        Console.WriteLine($"Vertical Movement:...{verticalMovement:0.0000}");
        Console.WriteLine($"Horizontal Movement:.{horizontalMovement:0.0000}");
        Console.SetCursorPosition(0, Console.WindowHeight - 2);
        Console.Write($"/input nil sent {_nilSent}");

        if (Console.WindowHeight < 13)
            return;
        Console.SetCursorPosition(40, 5);
        Console.WriteLine("----------+----------");
        for (int i = 1; i < 10; i++)
        {
            if(i == 5)
                continue;
            Console.SetCursorPosition(50,i);
            Console.Write("|");
        }

        const int centerX = 50;
        const int centerY = 5;
        int consoleX = Convert.ToInt32(Math.Floor(_horizontalMovement * 10));
        int consoleY = Convert.ToInt32(-Math.Floor(_verticalMovement * 4));
        double position = (_verticalMovement * 10) % 10;
        char c = position < 3 ? '.' : position > 7 ? '\'' : 'x';
        Console.SetCursorPosition(centerX + consoleX, centerY + consoleY);
        Console.ForegroundColor = ConsoleColor.Cyan;
        Console.Write(c);
        Console.SetCursorPosition(Console.WindowWidth - 1, Console.WindowHeight - 1);
        
        
        Console.SetCursorPosition(Console.WindowWidth - debugValue.Length - 1, Console.WindowHeight - 2);
        Console.ForegroundColor = ConsoleColor.Gray;
        Console.WriteLine(debugValue);
    }

    #region Handle OSC-Messages
    private void AllowMovingHandle(OscMessageValues messageValues)
    {
        this._allowMoving = messageValues.ReadBooleanElement(0);
    }

    private void GPS1Handle(OscMessageValues messageValues)
    {
        this.GPS1.Distance = messageValues.ReadFloatElement(0) * radius;
    }
    
    private void GPS2Handle(OscMessageValues messageValues)
    {
        this.GPS2.Distance = messageValues.ReadFloatElement(0) * radius;
    }
    
    private void GPS3Handle(OscMessageValues messageValues)
    {
        this.GPS3.Distance = messageValues.ReadFloatElement(0) * radius;
    }
    
    private void CollarStretchHandle(OscMessageValues messageValues)
    {
        this._leashStretch = messageValues.ReadFloatElement(0);
    }
    #endregion

    private void CalculatePositionFromGPS()
    {
        double gpsFactorA = Math.Pow(GPS1.Distance, 2) - Math.Pow(GPS2.Distance, 2) - GPSConstantC;
        double gpsFactorB = Math.Pow(GPS2.Distance, 2) - Math.Pow(GPS3.Distance, 2) - GPSConstantE;
        
        _posX = (gpsFactorB / GPSConstantD) + CalibrationX;
        _posY = (gpsFactorA * GPSConstantD - GPSConstantA * gpsFactorB) / (GPSConstantB * GPSConstantD) + CalibrationY;

        double inverseLength = 1 / Math.Sqrt(Math.Pow(_posX, 2) + Math.Pow(_posY, 2));

        _verticalMovement = _posY * inverseLength * _leashStretch;
        _horizontalMovement = _posX * inverseLength * -1 * _leashStretch;
    }

    private void RunningThread()
    {
        while (true)
        {
            this.Server.Update();
            CalculatePositionFromGPS();
            if (_lastNilMessageSent.Add(MessageMinInterval) < DateTime.Now)
            {
                this.Client.Send("/input/Vertical", 0f);
                this.Client.Send("/input/Horizontal", 0f);
                this._lastNilMessageSent = DateTime.Now;
                _nilSent++;
            }

            if (_allowMoving)
            {
                Thread.Sleep(1);
                if (_leashStretch > 0.1)
                {
                    this.Client.Send("/input/Vertical", Convert.ToSingle(_verticalMovement));
                    this.Client.Send("/input/Horizontal", Convert.ToSingle(_horizontalMovement));
                    this._lastNilMessageSent = DateTime.Now;
                }
            }

            if (_lastConsoleOutput.Add(ConsoleUpdateInterval) < DateTime.Now)
            {
                PrintOutput(_verticalMovement, _horizontalMovement);
                _lastConsoleOutput = DateTime.Now;
            }
            
            Thread.Sleep(UpdateInterval);
        }
    }
}