diff --git a/OSCCollar/Config.cs b/OSCCollar/Config.cs new file mode 100644 index 0000000..b425c72 --- /dev/null +++ b/OSCCollar/Config.cs @@ -0,0 +1,12 @@ +namespace VRC_Console; + +internal class Config +{ + internal string Ip = "127.0.0.1"; + internal int PortSend = 9000; + internal double Radius = 100; + internal double CalibrationX = 0; + internal double CalibrationY = 0; + internal double WalkStretchDeadzone = 0.1; + internal double RunStretch = 0.4; +} \ No newline at end of file diff --git a/OSCCollar/OSCCollar.cs b/OSCCollar/OSCCollar.cs index 071cb42..a2bc74f 100644 --- a/OSCCollar/OSCCollar.cs +++ b/OSCCollar/OSCCollar.cs @@ -1,6 +1,6 @@ using System.Net; using BuildSoft.OscCore; -using Newtonsoft.Json.Linq; +using Newtonsoft.Json; using VRC.OSCQuery; using Extensions = VRC.OSCQuery.Extensions; @@ -8,34 +8,35 @@ namespace VRC_Console; public class OSCCollar { + private static readonly string ConfigFilePath = Path.Combine(Directory.GetCurrentDirectory(), "config.json"); public static void Main(string[] args) { - string configFilePath = Path.Combine(Directory.GetCurrentDirectory(), "config.json"); - if (File.Exists(configFilePath)) + OSCCollar _; + if (File.Exists(ConfigFilePath)) { - var jObject = JObject.Parse(File.ReadAllText(configFilePath)); - string ip = jObject.GetValue("ip")?.ToObject() ?? "127.0.0.1"; - int portSend = jObject.GetValue("portSend")?.ToObject() ?? 9000; - double radius = jObject.GetValue("radius")?.ToObject() ?? 100; - double calibrationX = jObject.GetValue("calibrationX")?.ToObject() ?? 0; - double calibrationY = jObject.GetValue("calibrationY")?.ToObject() ?? 0; - double walkStretchDeadzone = jObject.GetValue("walkStretchDeadzone")?.ToObject() ?? 0.1; - double runStretch = jObject.GetValue("runStretch")?.ToObject() ?? 0.4; - bool skipSetup = jObject.GetValue("skipSetup")?.ToObject() ?? true; - var _ = new OSCCollar(ip, portSend, radius, calibrationX, calibrationY, walkStretchDeadzone, runStretch, skipSetup); + Config? config = JsonConvert.DeserializeObject(File.ReadAllText(ConfigFilePath)); + if(config is { }) + { + _ = new OSCCollar(config); + } + else + { + _ = new OSCCollar(); + } }else { - var _ = new OSCCollar(); + _ = new OSCCollar(); } } - private readonly string _ip; - private readonly int _portReceive, _portSend; + private Config _config; + private readonly int _portReceive; private OSCQueryService? OscQueryService { get; set; } - private OscServer Server { get; init; } - private OscClient Client { get; init; } + private OscServer _server = null!; + private OscClient _client = null!; + private float _leashStretch; private readonly Vector _unitVectorLeash = new(1, 0); private Vector _movementVector = new(0, 0); @@ -46,73 +47,57 @@ public class OSCCollar private static readonly TimeSpan ConsoleUpdateInterval = TimeSpan.FromMilliseconds(500); private static readonly TimeSpan UpdateInterval = TimeSpan.FromMilliseconds(10); private static readonly TimeSpan MessageMinInterval = TimeSpan.FromMilliseconds(400); - private readonly double _radius, _walkStretch, _runStretch; - 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 GPS _gps1 = null!, _gps2 = null!, _gps3 = null!; private readonly Queue> _calibrationAverage = new(); 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 double _constantA, _constantB, _constantC, _constantD, _constantE; - private OSCCollar(string ip = "127.0.0.1", int portSend = 9000, double radius = 100, double calibrationX = 0, double calibrationY = 0, double walkStretch = 0.1, double runStretch = 0.4, bool skipSetup = true) + private OSCCollar(Config config, bool skipSetup = true) { - this._ip = ip; - this._portReceive = Extensions.GetAvailableUdpPort(); - this._portSend = portSend; - this._walkStretch = walkStretch; - this._runStretch = runStretch; - 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); - + this._config = config; + File.WriteAllText(ConfigFilePath, JsonConvert.SerializeObject(this._config)); + + this.SetupGPSVars(); 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}"); + 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): {this._config.Radius * 2}"); Console.ReadKey(); } - this.Server = new OscServer(this._portReceive); - this.Client = new OscClient(this._ip, this._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(); - + this._portReceive = Extensions.GetAvailableUdpPort(); + this.SetupOSCServer(); this.SetupOSCQuery(); } + 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, + bool skipSetup = true) : this(new Config() + { + Ip = ip, + PortSend = portSend, + WalkStretchDeadzone = walkStretchDeadzone, + RunStretch = runStretch, + Radius = radius, + CalibrationX = calibrationX, + CalibrationY = calibrationY + }, skipSetup) + { + + } + private void PrintOutput() { Console.Clear(); Console.ForegroundColor = ConsoleColor.White; - Console.WriteLine($"OSC Collar - Status: {(_allowMoving ? "enabled" : "disabled")} IP: {_ip} Send-OSC: {_portSend} Receive-OSC: {_portReceive} OSC-HTTP: {this.OscQueryService?.TcpPort}"); + Console.WriteLine($"OSC Collar - Status: {(_allowMoving ? "enabled" : "disabled")} IP: {this._config.Ip} Send-OSC: {this._config.PortSend} Receive-OSC: {_portReceive} OSC-HTTP: {this.OscQueryService?.TcpPort}"); Console.WriteLine("=============================="); - Console.WriteLine($"GPS 1:................{GPS1.Distance:00.00000}"); - Console.WriteLine($"GPS 2:................{GPS2.Distance:00.00000}"); - Console.WriteLine($"GPS 3:................{GPS3.Distance:00.00000}"); + Console.WriteLine($"GPS 1:................{_gps1.Distance:00.00000}"); + Console.WriteLine($"GPS 2:................{_gps2.Distance:00.00000}"); + Console.WriteLine($"GPS 3:................{_gps3.Distance:00.00000}"); Console.ForegroundColor = ConsoleColor.Cyan; Console.WriteLine($"Position Vector:......{_unitVectorLeash}"); Console.ForegroundColor = ConsoleColor.White; @@ -165,12 +150,40 @@ public class OSCCollar Console.WriteLine(_debugValue); } + #region Setup + private void SetupGPSVars() + { + this._gps1 = new(0, -this._config.Radius); + this._gps2 = new(-(this._config.Radius * 0.866), this._config.Radius / 2); + this._gps3 = new(this._config.Radius * 0.866, this._config.Radius / 2); + this._constantA = 2 * _gps2.X - 2 * _gps1.X; + this._constantB = 2 * _gps2.Y - 2 * _gps1.Y; + this._constantC = Math.Pow(_gps1.X, 2) + Math.Pow(_gps2.X, 2) - Math.Pow(_gps1.Y, 2) + Math.Pow(_gps2.Y, 2); + 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/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._ip)) - .WithOscIP(IPAddress.Parse(this._ip)) + .WithHostIP(IPAddress.Parse(this._config.Ip)) + .WithOscIP(IPAddress.Parse(this._config.Ip)) .WithDiscovery(new MeaModDiscovery()) .WithTcpPort(tcpPort) .WithUdpPort(this._portReceive) @@ -186,6 +199,7 @@ public class OSCCollar this.OscQueryService.AddEndpoint("/avatar/parameters/Leash_Toggle", Attributes.AccessValues.WriteOnly); this.OscQueryService.RefreshServices(); } + #endregion #region Handle OSC-Messages private void AllowMovingHandle(OscMessageValues messageValues) @@ -195,17 +209,17 @@ public class OSCCollar private void GPS1Handle(OscMessageValues messageValues) { - this.GPS1.Distance = messageValues.ReadFloatElement(0) * _radius; + this._gps1.Distance = messageValues.ReadFloatElement(0) * this._config.Radius; } private void GPS2Handle(OscMessageValues messageValues) { - this.GPS2.Distance = messageValues.ReadFloatElement(0) * _radius; + this._gps2.Distance = messageValues.ReadFloatElement(0) * this._config.Radius; } private void GPS3Handle(OscMessageValues messageValues) { - this.GPS3.Distance = messageValues.ReadFloatElement(0) * _radius; + this._gps3.Distance = messageValues.ReadFloatElement(0) * this._config.Radius; } private void CollarStretchHandle(OscMessageValues messageValues) @@ -216,25 +230,25 @@ public class OSCCollar 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; + 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 / GPSConstantD); - double gpsPosY = (gpsFactorA * GPSConstantD - GPSConstantA * gpsFactorB) / (GPSConstantB * GPSConstantD); + double gpsPosX = (gpsFactorB / _constantD); + double gpsPosY = (gpsFactorA * _constantD - _constantA * gpsFactorB) / (_constantB * _constantD); AddCalibrationValues(gpsPosX, gpsPosY); - double posX = gpsPosX + CalibrationX; - double posY = gpsPosY + CalibrationY; + double posX = gpsPosX + this._config.CalibrationX; + double posY = gpsPosY + this._config.CalibrationY; double inverseLength = 1 / Math.Sqrt(Math.Pow(posX, 2) + Math.Pow(posY, 2)); _unitVectorLeash.X = posX * inverseLength * -1; _unitVectorLeash.Y = posY * inverseLength; - if (_leashStretch < _walkStretch) //Below Deadzone + if (_leashStretch < this._config.WalkStretchDeadzone) //Below Deadzone _movementVector = new(); - else if (_leashStretch < _runStretch) //Walk + else if (_leashStretch < this._config.RunStretch) //Walk _movementVector = _unitVectorLeash.MultipliedWith(0.5); else //Run _movementVector = _unitVectorLeash.MultipliedWith(1); @@ -244,18 +258,18 @@ public class OSCCollar { while (true) { - this.Server.Update(); + this._server.Update(); CalculatePositionFromGPS(); if (_lastNilMessageSent.Add(MessageMinInterval) < DateTime.Now) { - this.Client.Send("/input/Vertical", 0f); - this.Client.Send("/input/Horizontal", 0f); + this._client.Send("/input/Vertical", 0f); + this._client.Send("/input/Horizontal", 0f); this._lastNilMessageSent = DateTime.Now; _nilSentCount++; }else if (_allowMoving) { - this.Client.Send("/input/Vertical", Convert.ToSingle(_movementVector.Y)); - this.Client.Send("/input/Horizontal", Convert.ToSingle(_movementVector.X)); + this._client.Send("/input/Vertical", Convert.ToSingle(_movementVector.Y)); + this._client.Send("/input/Horizontal", Convert.ToSingle(_movementVector.X)); } if (_lastConsoleOutput.Add(ConsoleUpdateInterval) < DateTime.Now) diff --git a/OSCCollar/OSCCollar.csproj b/OSCCollar/OSCCollar.csproj index 977234d..be666b2 100644 --- a/OSCCollar/OSCCollar.csproj +++ b/OSCCollar/OSCCollar.csproj @@ -13,6 +13,7 @@ +