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

Compare commits

base: glax:18872ad5bc38b2ad5f8243afe5140e1b078584d7
Branches Tags
glax:master
glax:1.0
...
compare: glax:7142e57a4a8e1b6bd915929266de38d405167cfb
Branches Tags
glax:master
glax:1.0

6 Commits
18872ad5bc ... 7142e57a4a

Author SHA1 Message Date
glax
7142e57a4a json config file 2023-11-30 16:01:06 +01:00
glax
f0ccd7be48 Console output 2023-11-30 15:50:55 +01:00
glax
75e0c85b61 naming convention 2023-11-30 15:42:17 +01:00
glax
45a10abb61 More info on setup 2023-11-30 15:41:34 +01:00
glax
c7e6ab83e6 Allow more flexible setup with radius at startup, 
calculate constants once
 2023-11-30 15:39:12 +01:00
glax
b98154ffcc Reduced main method 2023-11-30 15:27:52 +01:00
3 changed files with 88 additions and 73 deletions
Show Stats Expand all files Collapse all files

2
OSCCollar/GPS.cs
View File

@ -4,7 +4,7 @@ public class GPS
{ {
public double X { get; init; } public double X { get; init; }
public double Y { get; init; } public double Y { get; init; }
public float Distance { get; set; } public double Distance { get; set; }
public GPS(double x, double y) public GPS(double x, double y)
{ {

158
OSCCollar/OSCCollar.cs
View File

@ -1,5 +1,5 @@
using System.Net; using BuildSoft.OscCore;
using BuildSoft.OscCore; using Newtonsoft.Json.Linq;
namespace VRC_Console; namespace VRC_Console;
@ -8,50 +8,79 @@ public class OSCCollar
public static void Main(string[] args) public static void Main(string[] args)
{ {
OSCCollar collar; string configFilePath = Path.Combine(Directory.GetCurrentDirectory(), "config.json");
if (args.Length == 3) if (File.Exists(configFilePath))
{ {
string ip = args[0]; var jObject = JObject.Parse(File.ReadAllText(configFilePath));
if(!IPAddress.TryParse(args[0], out IPAddress? _) || string ip = jObject.GetValue("ip")?.ToObject<string>() ?? "127.0.0.1";
!int.TryParse(args[1], out var portReceive) || int portReceive = jObject.GetValue("portReceive")?.ToObject<int>() ?? 9001;
!int.TryParse(args[2], out var portSend)) int portSend = jObject.GetValue("portReceive")?.ToObject<int>() ?? 9000;
{ double radius = jObject.GetValue("radius")?.ToObject<int>() ?? 100;
Console.WriteLine("Invalid parameter.\n\rParameters <ip> <receivePort> <sendPort>"); double calibrationX = jObject.GetValue("calibrationX")?.ToObject<int>() ?? 0;
return; double calibrationY = jObject.GetValue("calibrationY")?.ToObject<int>() ?? 0;
} bool skipSetup = jObject.GetValue("skipSetup")?.ToObject<bool>() ?? true;
collar = new OSCCollar(ip, portReceive, portSend); var _ = new OSCCollar(ip, portReceive, portSend, radius, calibrationX, calibrationY, skipSetup);
}else }else
collar = new OSCCollar(); {
var _ = new OSCCollar();
}
} }
private OscServer Server { get; init; } private OscServer Server { get; init; }
private OscClient Client { get; init; } private OscClient Client { get; init; }
private float _leashStretch; private float _leashStretch;
private double _posX, _posY, _verticalMovement, _horizontalMovement; private double _posX, _posY, _verticalMovement, _horizontalMovement;
private bool _allowMoving; private bool _allowMoving = true;
private uint _nilSent; private uint _nilSent;
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 UpdateInterval = TimeSpan.FromMilliseconds(1);
private static readonly TimeSpan NilMessageMaxTimeout = TimeSpan.FromMilliseconds(400); private static readonly TimeSpan MessageMinInterval = TimeSpan.FromMilliseconds(400);
private readonly GPS _gps1 = new GPS(0, -100); private readonly double _radius;
private readonly GPS _gps2 = new GPS(-86.6, 50); private GPS GPS1 { get; init; }
private readonly GPS _gps3 = new GPS(86.6, 50); private GPS GPS2 { get; init; }
private static readonly double calibrationX = -.33; private GPS GPS3 { get; init; }
private static readonly double calibrationY = -1.1; private double CalibrationX { get; init; }
private const double PowerFactor = 1f; private double CalibrationY { get; init; }
private string debugValue = ""; 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}");
Console.WriteLine($"Radius of each receiver (sphere): {radius * 2}");
}
this.Server = new OscServer(portReceive); this.Server = new OscServer(portReceive);
this.Client = new OscClient(ip, portSend); this.Client = new OscClient(ip, portSend);
Server.TryAddMethod("/avatar/parameters/GPS1", GPS1Handle); this.Server.TryAddMethod("/avatar/parameters/GPS1", GPS1Handle);
Server.TryAddMethod("/avatar/parameters/GPS2", GPS2Handle); this.Server.TryAddMethod("/avatar/parameters/GPS2", GPS2Handle);
Server.TryAddMethod("/avatar/parameters/GPS3", GPS3Handle); this.Server.TryAddMethod("/avatar/parameters/GPS3", GPS3Handle);
Server.TryAddMethod("/avatar/parameters/Leash_Stretch", CollarStretchHandle); this.Server.TryAddMethod("/avatar/parameters/Leash_Stretch", CollarStretchHandle);
Server.TryAddMethod("/avatar/parameters/Leash_Toggle", AllowMovingHandle); this.Server.TryAddMethod("/avatar/parameters/Leash_Toggle", AllowMovingHandle);
this.Server.Start(); this.Server.Start();
Thread runningThread = new Thread(RunningThread); Thread runningThread = new Thread(RunningThread);
@ -64,16 +93,17 @@ public class OSCCollar
Console.ForegroundColor = ConsoleColor.White; 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($"GPS 1:...............{_gps1.Distance:0.000000}"); Console.WriteLine($"GPS 1:................{GPS1.Distance:0.000000}");
Console.WriteLine($"GPS 2:...............{_gps2.Distance:0.000000}"); Console.WriteLine($"GPS 2:................{GPS2.Distance:0.000000}");
Console.WriteLine($"GPS 3:...............{_gps3.Distance:0.000000}"); Console.WriteLine($"GPS 3:................{GPS3.Distance:0.000000}");
Console.WriteLine($"Position X:..........{_posX:0.000000}"); Console.WriteLine($"Position X:.........{_posX:'.'0.00000;'-'0.00000}");
Console.WriteLine($"Position Y:..........{_posY:0.000000}"); Console.WriteLine($"Position Y:.........{_posY:'.'0.00000;'-'0.00000}");
Console.WriteLine($"Stretch:.............{_leashStretch:0.0000}"); Console.WriteLine($"Stretch:..............{_leashStretch:0.00000}");
Console.WriteLine($"Vertical Movement:...{verticalMovement:0.0000}"); Console.WriteLine($"Vertical Movement:..{verticalMovement:' '0.00000;'-'0.00000}");
Console.WriteLine($"Horizontal Movement:.{horizontalMovement:0.0000}"); Console.WriteLine($"Horizontal Movement:.{horizontalMovement:' '0.00000;'-'0.00000}");
Console.SetCursorPosition(0, Console.WindowHeight - 2); string nilString = $"/input nil {_nilSent}";
Console.Write($"/input nil sent {_nilSent}"); Console.SetCursorPosition(Console.WindowWidth - nilString.Length - 1, 0);
Console.Write(nilString);
if (Console.WindowHeight < 13) if (Console.WindowHeight < 13)
return; return;
@ -87,8 +117,8 @@ public class OSCCollar
Console.Write("|"); Console.Write("|");
} }
int centerX = 50; const int centerX = 50;
int centerY = 5; const 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 * 4)); int consoleY = Convert.ToInt32(-Math.Floor(_verticalMovement * 4));
double position = (_verticalMovement * 10) % 10; double position = (_verticalMovement * 10) % 10;
@ -104,6 +134,7 @@ public class OSCCollar
Console.WriteLine(debugValue); Console.WriteLine(debugValue);
} }
#region Handle OSC-Messages
private void AllowMovingHandle(OscMessageValues messageValues) private void AllowMovingHandle(OscMessageValues messageValues)
{ {
this._allowMoving = messageValues.ReadBooleanElement(0); this._allowMoving = messageValues.ReadBooleanElement(0);
@ -111,55 +142,37 @@ public class OSCCollar
private void GPS1Handle(OscMessageValues messageValues) private void GPS1Handle(OscMessageValues messageValues)
{ {
this._gps1.Distance = messageValues.ReadFloatElement(0) * 100; this.GPS1.Distance = messageValues.ReadFloatElement(0) * _radius;
} }
private void GPS2Handle(OscMessageValues messageValues) private void GPS2Handle(OscMessageValues messageValues)
{ {
this._gps2.Distance = messageValues.ReadFloatElement(0) * 100; this.GPS2.Distance = messageValues.ReadFloatElement(0) * _radius;
} }
private void GPS3Handle(OscMessageValues messageValues) private void GPS3Handle(OscMessageValues messageValues)
{ {
this._gps3.Distance = messageValues.ReadFloatElement(0) * 100; this.GPS3.Distance = messageValues.ReadFloatElement(0) * _radius;
} }
private void CollarStretchHandle(OscMessageValues messageValues) private void CollarStretchHandle(OscMessageValues messageValues)
{ {
this._leashStretch = messageValues.ReadFloatElement(0); this._leashStretch = messageValues.ReadFloatElement(0);
} }
#endregion
private void CalculatePositionFromGPS() private void CalculatePositionFromGPS()
{ {
double a = 2 * _gps2.X - 2 * _gps1.X; double gpsFactorA = Math.Pow(GPS1.Distance, 2) - Math.Pow(GPS2.Distance, 2) - GPSConstantC;
double b = 2 * _gps2.Y - 2 * _gps1.Y; double gpsFactorB = Math.Pow(GPS2.Distance, 2) - Math.Pow(GPS3.Distance, 2) - GPSConstantE;
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 = (gpsFactorB / GPSConstantD) + CalibrationX;
_posX = (c * e - f * b) / (e * a - b * d); _posY = (gpsFactorA * GPSConstantD - GPSConstantA * gpsFactorB) / (GPSConstantB * GPSConstantD) + CalibrationY;
_posY = (c * d - a * f) / (b * d - a * e);*/
/* double inverseLength = 1 / Math.Sqrt(Math.Pow(_posX, 2) + Math.Pow(_posY, 2));
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 * inverseLength * _leashStretch;
_horizontalMovement = _posX * inverseLength * -1 * _leashStretch;
_verticalMovement = _posY * factor * _leashStretch * PowerFactor;
_horizontalMovement = _posX * factor * -1 * _leashStretch * PowerFactor;
} }
private void RunningThread() private void RunningThread()
@ -168,7 +181,7 @@ public class OSCCollar
{ {
this.Server.Update(); this.Server.Update();
CalculatePositionFromGPS(); CalculatePositionFromGPS();
if (_lastNilMessageSent.Add(NilMessageMaxTimeout) < DateTime.Now) if (_lastNilMessageSent.Add(MessageMinInterval) < DateTime.Now)
{ {
this.Client.Send("/input/Vertical", 0f); this.Client.Send("/input/Vertical", 0f);
this.Client.Send("/input/Horizontal", 0f); this.Client.Send("/input/Horizontal", 0f);
@ -183,6 +196,7 @@ public class OSCCollar
{ {
this.Client.Send("/input/Vertical", Convert.ToSingle(_verticalMovement)); this.Client.Send("/input/Vertical", Convert.ToSingle(_verticalMovement));
this.Client.Send("/input/Horizontal", Convert.ToSingle(_horizontalMovement)); this.Client.Send("/input/Horizontal", Convert.ToSingle(_horizontalMovement));
this._lastNilMessageSent = DateTime.Now;
} }
} }
@ -192,7 +206,7 @@ public class OSCCollar
_lastConsoleOutput = DateTime.Now; _lastConsoleOutput = DateTime.Now;
} }
Thread.Sleep(UpdateFieldsTimeout); Thread.Sleep(UpdateInterval);
} }
} }
} }

1
OSCCollar/OSCCollar.csproj
View File

@ -10,6 +10,7 @@
<ItemGroup> <ItemGroup>
<PackageReference Include="BuildSoft.OscCore" Version="1.2.1.1" /> <PackageReference Include="BuildSoft.OscCore" Version="1.2.1.1" />
<PackageReference Include="Newtonsoft.Json" Version="13.0.3" />
</ItemGroup> </ItemGroup>
</Project> </Project>