diff --git a/OSCCollar/GPS.cs b/OSCCollar/GPS.cs new file mode 100644 index 0000000..920895f --- /dev/null +++ b/OSCCollar/GPS.cs @@ -0,0 +1,15 @@ +namespace VRC_Console; + +public class GPS +{ + public double X { get; init; } + public double Y { get; init; } + public float Distance { get; set; } + + public GPS(double x, double y) + { + this.X = x; + this.Y = y; + this.Distance = 0; + } +} \ No newline at end of file diff --git a/OSCCollar/OSCCollar.cs b/OSCCollar/OSCCollar.cs index 909b5de..e73bcb4 100644 --- a/OSCCollar/OSCCollar.cs +++ b/OSCCollar/OSCCollar.cs @@ -12,7 +12,7 @@ public class OSCCollar if (args.Length == 3) { string ip = args[0]; - if(!IPAddress.TryParse(args[0], out IPAddress? test) || + if(!IPAddress.TryParse(args[0], out IPAddress? _) || !int.TryParse(args[1], out var portReceive) || !int.TryParse(args[2], out var portSend)) { @@ -26,28 +26,32 @@ public class OSCCollar private OscServer Server { get; init; } private OscClient Client { get; init; } - private float _leashAngle, _leashStretch; - private double _leftOrRight = 1; - private double _verticalMovement = 0; - private double _horizontalMovement = 0; - private bool _allowMoving = false; - private uint _nilSent = 0; + private float _leashStretch; + private double _posX, _posY, _verticalMovement, _horizontalMovement; + private bool _allowMoving; + 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 string debugValue = ""; private OSCCollar(string ip = "127.0.0.1", int portReceive = 9001, int portSend = 9000) { this.Server = new OscServer(portReceive); this.Client = new OscClient(ip, portSend); - this._leashAngle = 0; - this._leashStretch = 0; - Server.TryAddMethod("/avatar/parameters/LeashDirection_Angle", CollarAngleHandle); - Server.TryAddMethod("/avatar/parameters/LeashRight", CollarLeftRightHandle); + 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); - Server.TryAddMethodPair("/avatar/parameters/Leash_Stretch", CollarStretchHandle, CalculateMovement); this.Server.Start(); Thread runningThread = new Thread(RunningThread); @@ -57,36 +61,47 @@ public class OSCCollar 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($"Angle:...............{_leashAngle:0.0000}"); - Console.WriteLine($"Left/Right:..........{(_leftOrRight < 0 ? "left" : "right")}"); + 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}"); - - Console.SetCursorPosition(28, 8); + + if (Console.WindowHeight < 13) + return; + Console.SetCursorPosition(40, 5); Console.WriteLine("----------+----------"); - for (int i = 4; i < 13; i++) + for (int i = 1; i < 10; i++) { - if(i == 8) + if(i == 5) continue; - Console.SetCursorPosition(38,i); + Console.SetCursorPosition(50,i); Console.Write("|"); } - int centerX = 38; - int centerY = 8; + int centerX = 50; + int centerY = 5; int consoleX = Convert.ToInt32(Math.Floor(_horizontalMovement * 10)); - int consoleY = Convert.ToInt32(-Math.Floor(_verticalMovement * 5)); - double position = (_verticalMovement * 100) % 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); } private void AllowMovingHandle(OscMessageValues messageValues) @@ -94,28 +109,57 @@ public class OSCCollar this._allowMoving = messageValues.ReadBooleanElement(0); } - private void CollarAngleHandle(OscMessageValues messageValues) + private void GPS1Handle(OscMessageValues messageValues) { - this._leashAngle = messageValues.ReadFloatElement(0); + this._gps1.Distance = messageValues.ReadFloatElement(0) * 100; } - - private void CollarLeftRightHandle(OscMessageValues messageValues) + + private void GPS2Handle(OscMessageValues messageValues) { - this._leftOrRight = messageValues.ReadBooleanElement(0) ? 1 : -1; + this._gps2.Distance = messageValues.ReadFloatElement(0) * 100; } - + + private void GPS3Handle(OscMessageValues messageValues) + { + this._gps3.Distance = messageValues.ReadFloatElement(0) * 100; + } + private void CollarStretchHandle(OscMessageValues messageValues) { this._leashStretch = messageValues.ReadFloatElement(0); } - private const float PowerAdjust = 0.25f; - private void CalculateMovement() + private void CalculatePositionFromGPS() { - float degrees = _leashAngle * 180; - double radians = degrees * Math.PI / 180; - _verticalMovement = Math.Cos(radians) * this._leashStretch * PowerAdjust; - _horizontalMovement = Math.Sin(radians) * this._leashStretch * PowerAdjust * _leftOrRight; + 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); + + /* + _posX = (c * e - f * b) / (e * a - b * d); + _posY = (c * d - a * f) / (b * d - a * e);*/ + + /* + 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 factor = 1 / Math.Sqrt(Math.Pow(_posX, 2) + Math.Pow(_posY, 2)); + + _verticalMovement = _posY * factor * _leashStretch * PowerFactor; + _horizontalMovement = _posX * factor * -1 * _leashStretch * PowerFactor; } private void RunningThread() @@ -123,6 +167,7 @@ public class OSCCollar while (true) { this.Server.Update(); + CalculatePositionFromGPS(); if (_lastNilMessageSent.Add(NilMessageMaxTimeout) < DateTime.Now) { this.Client.Send("/input/Vertical", 0f); @@ -134,8 +179,11 @@ public class OSCCollar if (_allowMoving) { Thread.Sleep(1); - this.Client.Send("/input/Vertical", Convert.ToSingle(_verticalMovement)); - this.Client.Send("/input/Horizontal", Convert.ToSingle(_horizontalMovement)); + if (_leashStretch > 0.1) + { + this.Client.Send("/input/Vertical", Convert.ToSingle(_verticalMovement)); + this.Client.Send("/input/Horizontal", Convert.ToSingle(_horizontalMovement)); + } } if (_lastConsoleOutput.Add(ConsoleUpdateInterval) < DateTime.Now) diff --git a/VRC_Collar.sln.DotSettings b/VRC_Collar.sln.DotSettings index 9d7df3f..b5b2df5 100644 --- a/VRC_Collar.sln.DotSettings +++ b/VRC_Collar.sln.DotSettings @@ -1,3 +1,4 @@  + GPS OSC True \ No newline at end of file