glax/OSCCollar
1
0
Fork 0
Code Issues Releases 1 Activity

shit just works.gps

Browse Source
This commit is contained in:
glax 2023-11-30 00:53:54 +01:00
parent 891d8f3b73
commit 18872ad5bc
3 changed files with 102 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
OSCCollar/GPS.cs Normal file
View File

@ -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;
}
}

118
OSCCollar/OSCCollar.cs
View File

@ -12,7 +12,7 @@ public class OSCCollar
if (args.Length == 3) if (args.Length == 3)
{ {
string ip = args[0]; 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[1], out var portReceive) ||
!int.TryParse(args[2], out var portSend)) !int.TryParse(args[2], out var portSend))
{ {
@ -26,28 +26,32 @@ public class OSCCollar
private OscServer Server { get; init; } private OscServer Server { get; init; }
private OscClient Client { get; init; } private OscClient Client { get; init; }
private float _leashAngle, _leashStretch; private float _leashStretch;
private double _leftOrRight = 1; private double _posX, _posY, _verticalMovement, _horizontalMovement;
private double _verticalMovement = 0; private bool _allowMoving;
private double _horizontalMovement = 0; private uint _nilSent;
private bool _allowMoving = false;
private uint _nilSent = 0;
private DateTime _lastNilMessageSent = DateTime.UnixEpoch; private DateTime _lastNilMessageSent = DateTime.UnixEpoch;
private DateTime _lastConsoleOutput = DateTime.UnixEpoch; private DateTime _lastConsoleOutput = DateTime.UnixEpoch;
private static readonly TimeSpan ConsoleUpdateInterval = TimeSpan.FromMilliseconds(100); private static readonly TimeSpan ConsoleUpdateInterval = TimeSpan.FromMilliseconds(100);
private static readonly TimeSpan UpdateFieldsTimeout = TimeSpan.FromMilliseconds(1); private static readonly TimeSpan UpdateFieldsTimeout = TimeSpan.FromMilliseconds(1);
private static readonly TimeSpan NilMessageMaxTimeout = TimeSpan.FromMilliseconds(400); 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) private OSCCollar(string ip = "127.0.0.1", int portReceive = 9001, int portSend = 9000)
{ {
this.Server = new OscServer(portReceive); this.Server = new OscServer(portReceive);
this.Client = new OscClient(ip, portSend); this.Client = new OscClient(ip, portSend);
this._leashAngle = 0; Server.TryAddMethod("/avatar/parameters/GPS1", GPS1Handle);
this._leashStretch = 0; Server.TryAddMethod("/avatar/parameters/GPS2", GPS2Handle);
Server.TryAddMethod("/avatar/parameters/LeashDirection_Angle", CollarAngleHandle); Server.TryAddMethod("/avatar/parameters/GPS3", GPS3Handle);
Server.TryAddMethod("/avatar/parameters/LeashRight", CollarLeftRightHandle); Server.TryAddMethod("/avatar/parameters/Leash_Stretch", CollarStretchHandle);
Server.TryAddMethod("/avatar/parameters/Leash_Toggle", AllowMovingHandle); Server.TryAddMethod("/avatar/parameters/Leash_Toggle", AllowMovingHandle);
Server.TryAddMethodPair("/avatar/parameters/Leash_Stretch", CollarStretchHandle, CalculateMovement);
this.Server.Start(); this.Server.Start();
Thread runningThread = new Thread(RunningThread); Thread runningThread = new Thread(RunningThread);
@ -57,36 +61,47 @@ public class OSCCollar
private void PrintOutput(double verticalMovement, double horizontalMovement) private void PrintOutput(double verticalMovement, double horizontalMovement)
{ {
Console.Clear(); Console.Clear();
Console.ForegroundColor = ConsoleColor.White;
Console.WriteLine($"OSC Collar - Status: {(_allowMoving ? "enabled" : "disabled")}"); Console.WriteLine($"OSC Collar - Status: {(_allowMoving ? "enabled" : "disabled")}");
Console.WriteLine("==============================================================="); Console.WriteLine("==============================");
Console.WriteLine($"Angle:...............{_leashAngle:0.0000}"); Console.WriteLine($"GPS 1:...............{_gps1.Distance:0.000000}");
Console.WriteLine($"Left/Right:..........{(_leftOrRight < 0 ? "left" : "right")}"); 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($"Stretch:.............{_leashStretch:0.0000}");
Console.WriteLine($"Vertical Movement:...{verticalMovement:0.0000}"); Console.WriteLine($"Vertical Movement:...{verticalMovement:0.0000}");
Console.WriteLine($"Horizontal Movement:.{horizontalMovement:0.0000}"); Console.WriteLine($"Horizontal Movement:.{horizontalMovement:0.0000}");
Console.SetCursorPosition(0, Console.WindowHeight - 2); Console.SetCursorPosition(0, Console.WindowHeight - 2);
Console.Write($"/input nil sent {_nilSent}"); Console.Write($"/input nil sent {_nilSent}");
Console.SetCursorPosition(28, 8); if (Console.WindowHeight < 13)
return;
Console.SetCursorPosition(40, 5);
Console.WriteLine("----------+----------"); Console.WriteLine("----------+----------");
for (int i = 4; i < 13; i++) for (int i = 1; i < 10; i++)
{ {
if(i == 8) if(i == 5)
continue; continue;
Console.SetCursorPosition(38,i); Console.SetCursorPosition(50,i);
Console.Write("|"); Console.Write("|");
} }
int centerX = 38; int centerX = 50;
int centerY = 8; int centerY = 5;
int consoleX = Convert.ToInt32(Math.Floor(_horizontalMovement * 10)); int consoleX = Convert.ToInt32(Math.Floor(_horizontalMovement * 10));
int consoleY = Convert.ToInt32(-Math.Floor(_verticalMovement * 5)); int consoleY = Convert.ToInt32(-Math.Floor(_verticalMovement * 4));
double position = (_verticalMovement * 100) % 10; double position = (_verticalMovement * 10) % 10;
char c = position < 3 ? '.' : position > 7 ? '\'' : 'x'; char c = position < 3 ? '.' : position > 7 ? '\'' : 'x';
Console.SetCursorPosition(centerX + consoleX, centerY + consoleY); Console.SetCursorPosition(centerX + consoleX, centerY + consoleY);
Console.ForegroundColor = ConsoleColor.Cyan; Console.ForegroundColor = ConsoleColor.Cyan;
Console.Write(c); Console.Write(c);
Console.SetCursorPosition(Console.WindowWidth - 1, Console.WindowHeight - 1); 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) private void AllowMovingHandle(OscMessageValues messageValues)
@ -94,14 +109,19 @@ public class OSCCollar
this._allowMoving = messageValues.ReadBooleanElement(0); 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) private void CollarStretchHandle(OscMessageValues messageValues)
@ -109,13 +129,37 @@ public class OSCCollar
this._leashStretch = messageValues.ReadFloatElement(0); this._leashStretch = messageValues.ReadFloatElement(0);
} }
private const float PowerAdjust = 0.25f; private void CalculatePositionFromGPS()
private void CalculateMovement()
{ {
float degrees = _leashAngle * 180; double a = 2 * _gps2.X - 2 * _gps1.X;
double radians = degrees * Math.PI / 180; double b = 2 * _gps2.Y - 2 * _gps1.Y;
_verticalMovement = Math.Cos(radians) * this._leashStretch * PowerAdjust; double c = Math.Pow(_gps1.Distance, 2) - Math.Pow(_gps2.Distance, 2) - Math.Pow(_gps1.X, 2) +
_horizontalMovement = Math.Sin(radians) * this._leashStretch * PowerAdjust * _leftOrRight; 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() private void RunningThread()
@ -123,6 +167,7 @@ public class OSCCollar
while (true) while (true)
{ {
this.Server.Update(); this.Server.Update();
CalculatePositionFromGPS();
if (_lastNilMessageSent.Add(NilMessageMaxTimeout) < DateTime.Now) if (_lastNilMessageSent.Add(NilMessageMaxTimeout) < DateTime.Now)
{ {
this.Client.Send("/input/Vertical", 0f); this.Client.Send("/input/Vertical", 0f);
@ -134,8 +179,11 @@ public class OSCCollar
if (_allowMoving) if (_allowMoving)
{ {
Thread.Sleep(1); Thread.Sleep(1);
this.Client.Send("/input/Vertical", Convert.ToSingle(_verticalMovement)); if (_leashStretch > 0.1)
this.Client.Send("/input/Horizontal", Convert.ToSingle(_horizontalMovement)); {
this.Client.Send("/input/Vertical", Convert.ToSingle(_verticalMovement));
this.Client.Send("/input/Horizontal", Convert.ToSingle(_horizontalMovement));
}
} }
if (_lastConsoleOutput.Add(ConsoleUpdateInterval) < DateTime.Now) if (_lastConsoleOutput.Add(ConsoleUpdateInterval) < DateTime.Now)

1
VRC_Collar.sln.DotSettings
View File

@ -1,3 +1,4 @@
<wpf:ResourceDictionary xml:space="preserve" xmlns:x="http://schemas.microsoft.com/winfx/2006/xaml" xmlns:s="clr-namespace:System;assembly=mscorlib" xmlns:ss="urn:shemas-jetbrains-com:settings-storage-xaml" xmlns:wpf="http://schemas.microsoft.com/winfx/2006/xaml/presentation"> <wpf:ResourceDictionary xml:space="preserve" xmlns:x="http://schemas.microsoft.com/winfx/2006/xaml" xmlns:s="clr-namespace:System;assembly=mscorlib" xmlns:ss="urn:shemas-jetbrains-com:settings-storage-xaml" xmlns:wpf="http://schemas.microsoft.com/winfx/2006/xaml/presentation">
<s:String x:Key="/Default/CodeStyle/Naming/CSharpNaming/Abbreviations/=GPS/@EntryIndexedValue">GPS</s:String>
<s:String x:Key="/Default/CodeStyle/Naming/CSharpNaming/Abbreviations/=OSC/@EntryIndexedValue">OSC</s:String> <s:String x:Key="/Default/CodeStyle/Naming/CSharpNaming/Abbreviations/=OSC/@EntryIndexedValue">OSC</s:String>
<s:Boolean x:Key="/Default/UserDictionary/Words/=Deadzone/@EntryIndexedValue">True</s:Boolean></wpf:ResourceDictionary> <s:Boolean x:Key="/Default/UserDictionary/Words/=Deadzone/@EntryIndexedValue">True</s:Boolean></wpf:ResourceDictionary>