From c7e6ab83e6d85c9c96c83bb9d4388c4ea9914276 Mon Sep 17 00:00:00 2001 From: glax Date: Thu, 30 Nov 2023 15:28:15 +0100 Subject: [PATCH] Allow more flexible setup with radius at startup, calculate constants once --- OSCCollar/GPS.cs | 2 +- OSCCollar/OSCCollar.cs | 114 ++++++++++++++++++++++------------------- 2 files changed, 62 insertions(+), 54 deletions(-) diff --git a/OSCCollar/GPS.cs b/OSCCollar/GPS.cs index 920895f..1993347 100644 --- a/OSCCollar/GPS.cs +++ b/OSCCollar/GPS.cs @@ -4,7 +4,7 @@ public class GPS { public double X { get; init; } public double Y { get; init; } - public float Distance { get; set; } + public double Distance { get; set; } public GPS(double x, double y) { diff --git a/OSCCollar/OSCCollar.cs b/OSCCollar/OSCCollar.cs index f6bdfef..e226de1 100644 --- a/OSCCollar/OSCCollar.cs +++ b/OSCCollar/OSCCollar.cs @@ -1,5 +1,4 @@ -using System.Net; -using BuildSoft.OscCore; +using BuildSoft.OscCore; namespace VRC_Console; @@ -15,30 +14,57 @@ public class OSCCollar private OscClient Client { get; init; } private float _leashStretch; private double _posX, _posY, _verticalMovement, _horizontalMovement; - private bool _allowMoving; + 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 UpdateFieldsTimeout = TimeSpan.FromMilliseconds(1); - private static readonly TimeSpan NilMessageMaxTimeout = TimeSpan.FromMilliseconds(400); - private readonly GPS _gps1 = new GPS(0, -100); - private readonly GPS _gps2 = new GPS(-86.6, 50); - private readonly GPS _gps3 = new GPS(86.6, 50); - private static readonly double calibrationX = -.33; - private static readonly double calibrationY = -1.1; - private const double PowerFactor = 1f; + 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) + 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}"); + } + this.Server = new OscServer(portReceive); this.Client = new OscClient(ip, portSend); - Server.TryAddMethod("/avatar/parameters/GPS1", GPS1Handle); - Server.TryAddMethod("/avatar/parameters/GPS2", GPS2Handle); - Server.TryAddMethod("/avatar/parameters/GPS3", GPS3Handle); - Server.TryAddMethod("/avatar/parameters/Leash_Stretch", CollarStretchHandle); - Server.TryAddMethod("/avatar/parameters/Leash_Toggle", AllowMovingHandle); + 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); @@ -51,9 +77,9 @@ public class OSCCollar 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($"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}"); @@ -74,8 +100,8 @@ public class OSCCollar Console.Write("|"); } - int centerX = 50; - int centerY = 5; + 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; @@ -98,17 +124,17 @@ public class OSCCollar private void GPS1Handle(OscMessageValues messageValues) { - this._gps1.Distance = messageValues.ReadFloatElement(0) * 100; + this.GPS1.Distance = messageValues.ReadFloatElement(0) * radius; } private void GPS2Handle(OscMessageValues messageValues) { - this._gps2.Distance = messageValues.ReadFloatElement(0) * 100; + this.GPS2.Distance = messageValues.ReadFloatElement(0) * radius; } private void GPS3Handle(OscMessageValues messageValues) { - this._gps3.Distance = messageValues.ReadFloatElement(0) * 100; + this.GPS3.Distance = messageValues.ReadFloatElement(0) * radius; } private void CollarStretchHandle(OscMessageValues messageValues) @@ -118,35 +144,16 @@ public class OSCCollar private void CalculatePositionFromGPS() { - double a = 2 * _gps2.X - 2 * _gps1.X; - double b = 2 * _gps2.Y - 2 * _gps1.Y; - double c = Math.Pow(_gps1.Distance, 2) - Math.Pow(_gps2.Distance, 2) - Math.Pow(_gps1.X, 2) + - Math.Pow(_gps2.X, 2) - Math.Pow(_gps1.Y, 2) + Math.Pow(_gps2.Y, 2); - double log = Math.Pow(_gps1.X, 2) + - Math.Pow(_gps2.X, 2) - Math.Pow(_gps1.Y, 2) + Math.Pow(_gps2.Y, 2); - double d = 2 * _gps3.X - 2 * _gps2.X; - //double e = 2 * _gps3.Y - 2 * _gps2.Y; - double f = Math.Pow(_gps2.Distance, 2) - Math.Pow(_gps3.Distance, 2) - Math.Pow(_gps2.X, 2) + - Math.Pow(_gps3.X, 2) - Math.Pow(_gps2.Y, 2) + Math.Pow(_gps3.Y, 2); + 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 = (c * e - f * b) / (e * a - b * d); - _posY = (c * d - a * f) / (b * d - a * e);*/ + _posX = (gpsFactorB / GPSConstantD) + CalibrationX; + _posY = (gpsFactorA * GPSConstantD - GPSConstantA * gpsFactorB) / (GPSConstantB * GPSConstantD) + CalibrationY; - /* - double a = -173.19; - double b = 300; - double c = Math.Pow(_gps1.Distance, 2) - Math.Pow(_gps2.Distance, 2) - 0.44; - double d = 346.39; - double f = Math.Pow(_gps2.Distance, 2) - Math.Pow(_gps3.Distance, 2) - 3758.45;*/ - - _posX = (f / d) + calibrationX; - _posY = (c * d - a * f) / (b * d) + calibrationY; + double inverseLength = 1 / Math.Sqrt(Math.Pow(_posX, 2) + Math.Pow(_posY, 2)); - double factor = 1 / Math.Sqrt(Math.Pow(_posX, 2) + Math.Pow(_posY, 2)); - - _verticalMovement = _posY * factor * _leashStretch * PowerFactor; - _horizontalMovement = _posX * factor * -1 * _leashStretch * PowerFactor; + _verticalMovement = _posY * inverseLength * _leashStretch; + _horizontalMovement = _posX * inverseLength * -1 * _leashStretch; } private void RunningThread() @@ -155,7 +162,7 @@ public class OSCCollar { this.Server.Update(); CalculatePositionFromGPS(); - if (_lastNilMessageSent.Add(NilMessageMaxTimeout) < DateTime.Now) + if (_lastNilMessageSent.Add(MessageMinInterval) < DateTime.Now) { this.Client.Send("/input/Vertical", 0f); this.Client.Send("/input/Horizontal", 0f); @@ -170,6 +177,7 @@ public class OSCCollar { this.Client.Send("/input/Vertical", Convert.ToSingle(_verticalMovement)); this.Client.Send("/input/Horizontal", Convert.ToSingle(_horizontalMovement)); + this._lastNilMessageSent = DateTime.Now; } } @@ -179,7 +187,7 @@ public class OSCCollar _lastConsoleOutput = DateTime.Now; } - Thread.Sleep(UpdateFieldsTimeout); + Thread.Sleep(UpdateInterval); } } } \ No newline at end of file