From 9ab3d75e7bc200c5da6a94f4220817f017f6e745 Mon Sep 17 00:00:00 2001 From: Glax Date: Mon, 26 Feb 2024 02:43:29 +0100 Subject: [PATCH] Complete Rework Fix wrong/inversed values for Distance Fix OSCQuery Implementation --- OSCCollar/Config.cs | 3 +- OSCCollar/ConsoleOutput.cs | 23 +++-- OSCCollar/OSCCollar.cs | 164 ++++++++++++-------------------- OSCCollar/OSCCollar.csproj | 4 + VRC_Collar.sln.DotSettings.user | 1 + 5 files changed, 79 insertions(+), 116 deletions(-) diff --git a/OSCCollar/Config.cs b/OSCCollar/Config.cs index 855f836..33d01e0 100644 --- a/OSCCollar/Config.cs +++ b/OSCCollar/Config.cs @@ -5,8 +5,7 @@ internal class Config public string Ip = "127.0.0.1"; public int PortSend = 9000; public double Radius = 100; - public double CalibrationX = 0; - public double CalibrationY = 0; public double WalkStretchDeadzone = 0.1; public double RunStretch = 0.4; + public int CalibrationValues = 128; } \ No newline at end of file diff --git a/OSCCollar/ConsoleOutput.cs b/OSCCollar/ConsoleOutput.cs index c01f077..9e88c68 100644 --- a/OSCCollar/ConsoleOutput.cs +++ b/OSCCollar/ConsoleOutput.cs @@ -13,24 +13,21 @@ public partial class OSCCollar { new ValueTuple("IP", this._config.Ip), new ValueTuple("Send-Osc-Port", this._config.PortSend.ToString()), - new ValueTuple("Receive-Osc-Port", _portReceive.ToString()), - new ValueTuple("HTTP", this.OscQueryService?.TcpPort.ToString() ?? ""), - new ValueTuple("CalibrationX", $"{this._config.CalibrationX}"), - new ValueTuple("CalibrationY", $"{this._config.CalibrationY}"), + new ValueTuple("Receive-Osc-Port", this._osc.OSCPort.ToString()), + new ValueTuple("Calibration Values", $"{this._calibrationAverage.Count}/{this._config.CalibrationValues}"), }; ValueTuple[] variableValues = new[] { new ValueTuple("Status", $"{(_allowMoving ? Resources.OSCCollar_ConsoleOutput_StatusEnabled : Resources.OSCCollar_ConsoleOutput_StatusDisabled)}"), - new ValueTuple("GPS 1", $"{_gps1.Distance:00.00000}"), - new ValueTuple("GPS 2", $"{_gps2.Distance:00.00000}"), - new ValueTuple("GPS 3", $"{_gps3.Distance:00.00000}"), - new ValueTuple("GPS 1", $"{_gps1.Distance:00.00000}"), + new ValueTuple("GPS 1", $"{_gps1.Distance: 0.00000}"), + new ValueTuple("GPS 2", $"{_gps2.Distance: 0.00000}"), + new ValueTuple("GPS 3", $"{_gps3.Distance: 0.00000}"), new ValueTuple(Resources.OSCCollar_ConsoleOutput_PositionVector, _unitVectorLeash.ToString()), - new ValueTuple(Resources.OSCCollar_ConsoleOutput_LeashStretch, _leashStretch.ToString()), + new ValueTuple(Resources.OSCCollar_ConsoleOutput_LeashStretch, _leashStretch.ToString(" 0.00000")), new ValueTuple(Resources.OSCCollar_ConsoleOutput_MovementVector, _movementVector.ToString()), - new ValueTuple("CalibrationX", $"{GetCalibrationValues().Item1}"), - new ValueTuple("CalibrationY", $"{GetCalibrationValues().Item2}") + new ValueTuple("CalibrationX", $"{GetCalibrationValues().Item1: 00.00000;-00.00000}"), + new ValueTuple("CalibrationY", $"{GetCalibrationValues().Item2: 00.00000;-00.00000}") }; Console.Write(GetCoordinateSystem()); @@ -42,6 +39,7 @@ public partial class OSCCollar { Console.SetCursorPosition(coordOffset, i); Console.Write($"{configValues[i].Item1}:"); + Console.Write(new string('.', valueOffset - configValues[i].Item1.Length)); Console.SetCursorPosition(coordOffset + valueOffset, i); Console.Write(configValues[i].Item2); } @@ -53,12 +51,13 @@ public partial class OSCCollar { Console.SetCursorPosition(varOffset, i); Console.Write($"{variableValues[i].Item1}:"); + Console.Write(new string('.', valueOffset - variableValues[i].Item1.Length)); Console.SetCursorPosition(varOffset + valueOffset, i); Console.Write(variableValues[i].Item2); } Console.SetCursorPosition(0, Console.WindowHeight - 2); - Console.Write($"{_lastNilMessageSent} AvatarId: {_avatarId} Runtime: {DateTime.Now.Subtract(_programStarted)}"); + Console.Write($"Last Hand Reset: {DateTime.Now.Subtract(_lastNilMessageSent):ss\\.fff}\t\tAvatarId: {(_avatarId.Length < 1 ? "Reset Avatar to show." : _avatarId)}\t\tRuntime: {DateTime.Now.Subtract(_programStarted):hh\\:mm\\:ss\\.fff}"); } private String GetCoordinateSystem() diff --git a/OSCCollar/OSCCollar.cs b/OSCCollar/OSCCollar.cs index 7a14e59..0bcb512 100644 --- a/OSCCollar/OSCCollar.cs +++ b/OSCCollar/OSCCollar.cs @@ -1,8 +1,5 @@ -using System.Net; -using BuildSoft.OscCore; -using Newtonsoft.Json; -using VRC.OSCQuery; -using Extensions = VRC.OSCQuery.Extensions; +using Newtonsoft.Json; +using GlaxOSC; namespace VRC_Console; @@ -31,11 +28,8 @@ public partial class OSCCollar } private readonly Config _config; - private readonly int _portReceive; - - private OSCQueryService? OscQueryService { get; set; } - private OscServer _server = null!; - private OscClient _client = null!; + private readonly OSC _osc; + private Task _consoleOutputTask; private string _avatarId = ""; @@ -55,7 +49,16 @@ public partial class OSCCollar { this._config = config; File.WriteAllText(ConfigFilePath, JsonConvert.SerializeObject(this._config, Formatting.Indented)); - + + this._osc = new OSC("OSCCollar", new Dictionary() + { + { "/avatar/parameters/GPS1", typeof(float) }, + { "/avatar/parameters/GPS2", typeof(float) }, + { "/avatar/parameters/GPS3", typeof(float) }, + { "/avatar/parameters/Leash_Stretch", typeof(float) }, + { "/avatar/parameters/Leash_Toggle", typeof(bool) }, + }); + this._osc.OnParameterChangeEvent += OnOscParameterChangeEvent; this.SetupGPSVars(); if (!skipSetup) @@ -68,13 +71,36 @@ public partial class OSCCollar Console.ReadKey(); } - this._portReceive = Extensions.GetAvailableUdpPort(); - this.SetupOSCServer(); - this.SetupOSCQuery(); + Thread runningThread = new(RunningThread); + runningThread.Start(); } - private OSCCollar(string ip = "127.0.0.1", int portSend = 9000, double radius = 100, double calibrationX = 0, - double calibrationY = 0, double walkStretchDeadzone = 0.1, double runStretch = 0.4, + private void OnOscParameterChangeEvent(string endpoint, object? oldvalue, object? newvalue) + { + switch (endpoint) + { + case "/avatar/parameters/GPS1": + this._gps1.Distance = 1 - (float)(newvalue ?? 1); + break; + case "/avatar/parameters/GPS2": + this._gps2.Distance = 1 - (float)(newvalue ?? 1); + break; + case "/avatar/parameters/GPS3": + this._gps3.Distance = 1 - (float)(newvalue ?? 1); + break; + case "/avatar/parameters/Leash_Stretch": + this._leashStretch = (float)(newvalue ?? 0); + break; + case "/avatar/parameters/Leash_Toggle": + this._allowMoving = (bool)(newvalue ?? false); + break; + case "/avatar/change": + this._avatarId = (string)(newvalue ?? ""); + break; + } + } + + private OSCCollar(string ip = "127.0.0.1", int portSend = 9000, double radius = 100, double walkStretchDeadzone = 0.1, double runStretch = 0.4, int calibrationValues = 128, bool skipSetup = true) : this(new Config() { Ip = ip, @@ -82,8 +108,7 @@ public partial class OSCCollar WalkStretchDeadzone = walkStretchDeadzone, RunStretch = runStretch, Radius = radius, - CalibrationX = calibrationX, - CalibrationY = calibrationY + CalibrationValues = calibrationValues }, skipSetup) { @@ -101,96 +126,30 @@ public partial class OSCCollar this._constantD = 2 * _gps3.X - 2 * _gps2.X; this._constantE = Math.Pow(_gps2.X, 2) + Math.Pow(_gps3.X, 2) - Math.Pow(_gps2.Y, 2) + Math.Pow(_gps3.Y, 2); } - - private void SetupOSCServer() - { - this._server = new OscServer(this._portReceive); - this._client = new OscClient(this._config.Ip, this._config.PortSend); - this._server.TryAddMethod("/avatar/change", AvatarChangeHandle); - 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 SetupOSCQuery() - { - var tcpPort = Extensions.GetAvailableTcpPort(); - this.OscQueryService = new OSCQueryServiceBuilder() - .WithHostIP(IPAddress.Parse(this._config.Ip)) - .WithOscIP(IPAddress.Parse(this._config.Ip)) - .WithDiscovery(new MeaModDiscovery()) - .WithTcpPort(tcpPort) - .WithUdpPort(this._portReceive) - .WithServiceName("Collar") - .StartHttpServer() - .AdvertiseOSC() - .AdvertiseOSCQuery() - .Build(); - this.OscQueryService.AddEndpoint("/avatar/change", Attributes.AccessValues.WriteOnly); - this.OscQueryService.AddEndpoint("/avatar/parameters/GPS1", Attributes.AccessValues.WriteOnly); - this.OscQueryService.AddEndpoint("/avatar/parameters/GPS2", Attributes.AccessValues.WriteOnly); - this.OscQueryService.AddEndpoint("/avatar/parameters/GPS3", Attributes.AccessValues.WriteOnly); - this.OscQueryService.AddEndpoint("/avatar/parameters/Leash_Stretch", Attributes.AccessValues.WriteOnly); - this.OscQueryService.AddEndpoint("/avatar/parameters/Leash_Toggle", Attributes.AccessValues.WriteOnly); - } - #endregion - - #region Handle OSC-Messages - - private void AvatarChangeHandle(OscMessageValues messageValues) - { - this._avatarId = messageValues.ReadStringElement(0); - } - - private void AllowMovingHandle(OscMessageValues messageValues) - { - this._allowMoving = messageValues.ReadBooleanElement(0); - } - - private void GPS1Handle(OscMessageValues messageValues) - { - this._gps1.Distance = messageValues.ReadFloatElement(0) * this._config.Radius; - } - - private void GPS2Handle(OscMessageValues messageValues) - { - this._gps2.Distance = messageValues.ReadFloatElement(0) * this._config.Radius; - } - - private void GPS3Handle(OscMessageValues messageValues) - { - this._gps3.Distance = messageValues.ReadFloatElement(0) * this._config.Radius; - } - - private void CollarStretchHandle(OscMessageValues messageValues) - { - this._leashStretch = messageValues.ReadFloatElement(0); - } #endregion private void CalculatePositionFromGPS() { + if (this._gps1.Distance == 0 || this._gps2.Distance == 0 || this._gps3.Distance == 0) + return; double gpsFactorA = Math.Pow(_gps1.Distance, 2) - Math.Pow(_gps2.Distance, 2) - _constantC; double gpsFactorB = Math.Pow(_gps2.Distance, 2) - Math.Pow(_gps3.Distance, 2) - _constantE; double gpsPosX = (gpsFactorB / _constantD); - double gpsPosY = (gpsFactorA * _constantD - _constantA * gpsFactorB) / (_constantB * _constantD); + double gpsPosY = -1 * (gpsFactorA * _constantD - _constantA * gpsFactorB) / (_constantB * _constantD); - AddCalibrationValues(gpsPosX, gpsPosY); + if (this._leashStretch == 0) + AddCalibrationValues(gpsPosX, gpsPosY); - double posX = gpsPosX + this._config.CalibrationX; - double posY = gpsPosY + this._config.CalibrationY; + ValueTuple calibrationValues = GetCalibrationValues(); + + double posX = gpsPosX + calibrationValues.Item1; + double posY = gpsPosY + calibrationValues.Item2; double inverseLength = 1 / Math.Sqrt(Math.Pow(posX, 2) + Math.Pow(posY, 2)); - _unitVectorLeash.X = posX * inverseLength * -1; - _unitVectorLeash.Y = posY * inverseLength; + _unitVectorLeash.X = inverseLength is not Double.PositiveInfinity ? posX * inverseLength : 0; + _unitVectorLeash.Y = inverseLength is not Double.PositiveInfinity ? posY * inverseLength : 0; if (_leashStretch < this._config.WalkStretchDeadzone) //Below Deadzone _movementVector = new(); @@ -204,20 +163,19 @@ public partial class OSCCollar { 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._osc.Client.Send("/input/Vertical", 0f); + this._osc.Client.Send("/input/Horizontal", 0f); this._lastNilMessageSent = DateTime.Now; - PrintConsole(); }else if (_allowMoving) { - this._client.Send("/input/Vertical", Convert.ToSingle(_movementVector.Y)); - this._client.Send("/input/Horizontal", Convert.ToSingle(_movementVector.X)); + this._osc.Client.Send("/input/Vertical", Convert.ToSingle(_movementVector.Y)); + this._osc.Client.Send("/input/Horizontal", Convert.ToSingle(_movementVector.X)); } + PrintConsole(); Thread.Sleep(UpdateInterval); } } @@ -225,12 +183,14 @@ public partial class OSCCollar private void AddCalibrationValues(double x, double y) { this._calibrationAverage.Enqueue(new ValueTuple(x, y)); - if (this._calibrationAverage.Count > 50) + while (this._calibrationAverage.Count > this._config.CalibrationValues) this._calibrationAverage.Dequeue(); } private ValueTuple GetCalibrationValues() { + if (this._calibrationAverage.Count < 1) + return new ValueTuple(0, 0); double averageX = -this._calibrationAverage.Average(value => value.Item1); double averageY = -this._calibrationAverage.Average(value => value.Item2); return new ValueTuple(averageX, averageY); diff --git a/OSCCollar/OSCCollar.csproj b/OSCCollar/OSCCollar.csproj index c83dfa2..4e0a0d2 100644 --- a/OSCCollar/OSCCollar.csproj +++ b/OSCCollar/OSCCollar.csproj @@ -11,6 +11,7 @@ + @@ -18,6 +19,9 @@ + + ..\..\GlaxOSC\GlaxOSC\bin\Debug\net7.0\GlaxOSC.dll + ..\..\vrc-oscquery-lib\vrc-oscquery-lib\bin\Debug\net6.0\vrc-oscquery-lib.dll diff --git a/VRC_Collar.sln.DotSettings.user b/VRC_Collar.sln.DotSettings.user index ddaf78c..ea4c8b1 100644 --- a/VRC_Collar.sln.DotSettings.user +++ b/VRC_Collar.sln.DotSettings.user @@ -1,4 +1,5 @@  + True True True True \ No newline at end of file