Complete Rework

Fix wrong/inversed values for Distance
Fix OSCQuery Implementation
This commit is contained in:
Glax 2024-02-26 02:43:29 +01:00
parent b7a314cae3
commit 9ab3d75e7b
5 changed files with 79 additions and 116 deletions

View File

@ -5,8 +5,7 @@ internal class Config
public string Ip = "127.0.0.1";
public int PortSend = 9000;
public double Radius = 100;
public double CalibrationX = 0;
public double CalibrationY = 0;
public double WalkStretchDeadzone = 0.1;
public double RunStretch = 0.4;
public int CalibrationValues = 128;
}

View File

@ -13,24 +13,21 @@ public partial class OSCCollar
{
new ValueTuple<string, string>("IP", this._config.Ip),
new ValueTuple<string, string>("Send-Osc-Port", this._config.PortSend.ToString()),
new ValueTuple<string, string>("Receive-Osc-Port", _portReceive.ToString()),
new ValueTuple<string, string>("HTTP", this.OscQueryService?.TcpPort.ToString() ?? ""),
new ValueTuple<string, string>("CalibrationX", $"{this._config.CalibrationX}"),
new ValueTuple<string, string>("CalibrationY", $"{this._config.CalibrationY}"),
new ValueTuple<string, string>("Receive-Osc-Port", this._osc.OSCPort.ToString()),
new ValueTuple<string, string>("Calibration Values", $"{this._calibrationAverage.Count}/{this._config.CalibrationValues}"),
};
ValueTuple<string, string>[] variableValues = new[]
{
new ValueTuple<string, string>("Status", $"{(_allowMoving ? Resources.OSCCollar_ConsoleOutput_StatusEnabled : Resources.OSCCollar_ConsoleOutput_StatusDisabled)}"),
new ValueTuple<string, string>("GPS 1", $"{_gps1.Distance:00.00000}"),
new ValueTuple<string, string>("GPS 2", $"{_gps2.Distance:00.00000}"),
new ValueTuple<string, string>("GPS 3", $"{_gps3.Distance:00.00000}"),
new ValueTuple<string, string>("GPS 1", $"{_gps1.Distance:00.00000}"),
new ValueTuple<string, string>("GPS 1", $"{_gps1.Distance: 0.00000}"),
new ValueTuple<string, string>("GPS 2", $"{_gps2.Distance: 0.00000}"),
new ValueTuple<string, string>("GPS 3", $"{_gps3.Distance: 0.00000}"),
new ValueTuple<string, string>(Resources.OSCCollar_ConsoleOutput_PositionVector, _unitVectorLeash.ToString()),
new ValueTuple<string, string>(Resources.OSCCollar_ConsoleOutput_LeashStretch, _leashStretch.ToString()),
new ValueTuple<string, string>(Resources.OSCCollar_ConsoleOutput_LeashStretch, _leashStretch.ToString(" 0.00000")),
new ValueTuple<string, string>(Resources.OSCCollar_ConsoleOutput_MovementVector, _movementVector.ToString()),
new ValueTuple<string, string>("CalibrationX", $"{GetCalibrationValues().Item1}"),
new ValueTuple<string, string>("CalibrationY", $"{GetCalibrationValues().Item2}")
new ValueTuple<string, string>("CalibrationX", $"{GetCalibrationValues().Item1: 00.00000;-00.00000}"),
new ValueTuple<string, string>("CalibrationY", $"{GetCalibrationValues().Item2: 00.00000;-00.00000}")
};
Console.Write(GetCoordinateSystem());
@ -42,6 +39,7 @@ public partial class OSCCollar
{
Console.SetCursorPosition(coordOffset, i);
Console.Write($"{configValues[i].Item1}:");
Console.Write(new string('.', valueOffset - configValues[i].Item1.Length));
Console.SetCursorPosition(coordOffset + valueOffset, i);
Console.Write(configValues[i].Item2);
}
@ -53,12 +51,13 @@ public partial class OSCCollar
{
Console.SetCursorPosition(varOffset, i);
Console.Write($"{variableValues[i].Item1}:");
Console.Write(new string('.', valueOffset - variableValues[i].Item1.Length));
Console.SetCursorPosition(varOffset + valueOffset, i);
Console.Write(variableValues[i].Item2);
}
Console.SetCursorPosition(0, Console.WindowHeight - 2);
Console.Write($"{_lastNilMessageSent} AvatarId: {_avatarId} Runtime: {DateTime.Now.Subtract(_programStarted)}");
Console.Write($"Last Hand Reset: {DateTime.Now.Subtract(_lastNilMessageSent):ss\\.fff}\t\tAvatarId: {(_avatarId.Length < 1 ? "Reset Avatar to show." : _avatarId)}\t\tRuntime: {DateTime.Now.Subtract(_programStarted):hh\\:mm\\:ss\\.fff}");
}
private String GetCoordinateSystem()

View File

@ -1,8 +1,5 @@
using System.Net;
using BuildSoft.OscCore;
using Newtonsoft.Json;
using VRC.OSCQuery;
using Extensions = VRC.OSCQuery.Extensions;
using Newtonsoft.Json;
using GlaxOSC;
namespace VRC_Console;
@ -31,11 +28,8 @@ public partial class OSCCollar
}
private readonly Config _config;
private readonly int _portReceive;
private OSCQueryService? OscQueryService { get; set; }
private OscServer _server = null!;
private OscClient _client = null!;
private readonly OSC _osc;
private Task _consoleOutputTask;
private string _avatarId = "";
@ -55,7 +49,16 @@ public partial class OSCCollar
{
this._config = config;
File.WriteAllText(ConfigFilePath, JsonConvert.SerializeObject(this._config, Formatting.Indented));
this._osc = new OSC("OSCCollar", new Dictionary<string, Type>()
{
{ "/avatar/parameters/GPS1", typeof(float) },
{ "/avatar/parameters/GPS2", typeof(float) },
{ "/avatar/parameters/GPS3", typeof(float) },
{ "/avatar/parameters/Leash_Stretch", typeof(float) },
{ "/avatar/parameters/Leash_Toggle", typeof(bool) },
});
this._osc.OnParameterChangeEvent += OnOscParameterChangeEvent;
this.SetupGPSVars();
if (!skipSetup)
@ -68,13 +71,36 @@ public partial class OSCCollar
Console.ReadKey();
}
this._portReceive = Extensions.GetAvailableUdpPort();
this.SetupOSCServer();
this.SetupOSCQuery();
Thread runningThread = new(RunningThread);
runningThread.Start();
}
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,
private void OnOscParameterChangeEvent(string endpoint, object? oldvalue, object? newvalue)
{
switch (endpoint)
{
case "/avatar/parameters/GPS1":
this._gps1.Distance = 1 - (float)(newvalue ?? 1);
break;
case "/avatar/parameters/GPS2":
this._gps2.Distance = 1 - (float)(newvalue ?? 1);
break;
case "/avatar/parameters/GPS3":
this._gps3.Distance = 1 - (float)(newvalue ?? 1);
break;
case "/avatar/parameters/Leash_Stretch":
this._leashStretch = (float)(newvalue ?? 0);
break;
case "/avatar/parameters/Leash_Toggle":
this._allowMoving = (bool)(newvalue ?? false);
break;
case "/avatar/change":
this._avatarId = (string)(newvalue ?? "");
break;
}
}
private OSCCollar(string ip = "127.0.0.1", int portSend = 9000, double radius = 100, double walkStretchDeadzone = 0.1, double runStretch = 0.4, int calibrationValues = 128,
bool skipSetup = true) : this(new Config()
{
Ip = ip,
@ -82,8 +108,7 @@ public partial class OSCCollar
WalkStretchDeadzone = walkStretchDeadzone,
RunStretch = runStretch,
Radius = radius,
CalibrationX = calibrationX,
CalibrationY = calibrationY
CalibrationValues = calibrationValues
}, skipSetup)
{
@ -101,96 +126,30 @@ public partial class OSCCollar
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/change", AvatarChangeHandle);
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._config.Ip))
.WithOscIP(IPAddress.Parse(this._config.Ip))
.WithDiscovery(new MeaModDiscovery())
.WithTcpPort(tcpPort)
.WithUdpPort(this._portReceive)
.WithServiceName("Collar")
.StartHttpServer()
.AdvertiseOSC()
.AdvertiseOSCQuery()
.Build();
this.OscQueryService.AddEndpoint<string>("/avatar/change", Attributes.AccessValues.WriteOnly);
this.OscQueryService.AddEndpoint<float>("/avatar/parameters/GPS1", Attributes.AccessValues.WriteOnly);
this.OscQueryService.AddEndpoint<float>("/avatar/parameters/GPS2", Attributes.AccessValues.WriteOnly);
this.OscQueryService.AddEndpoint<float>("/avatar/parameters/GPS3", Attributes.AccessValues.WriteOnly);
this.OscQueryService.AddEndpoint<float>("/avatar/parameters/Leash_Stretch", Attributes.AccessValues.WriteOnly);
this.OscQueryService.AddEndpoint<bool>("/avatar/parameters/Leash_Toggle", Attributes.AccessValues.WriteOnly);
}
#endregion
#region Handle OSC-Messages
private void AvatarChangeHandle(OscMessageValues messageValues)
{
this._avatarId = messageValues.ReadStringElement(0);
}
private void AllowMovingHandle(OscMessageValues messageValues)
{
this._allowMoving = messageValues.ReadBooleanElement(0);
}
private void GPS1Handle(OscMessageValues messageValues)
{
this._gps1.Distance = messageValues.ReadFloatElement(0) * this._config.Radius;
}
private void GPS2Handle(OscMessageValues messageValues)
{
this._gps2.Distance = messageValues.ReadFloatElement(0) * this._config.Radius;
}
private void GPS3Handle(OscMessageValues messageValues)
{
this._gps3.Distance = messageValues.ReadFloatElement(0) * this._config.Radius;
}
private void CollarStretchHandle(OscMessageValues messageValues)
{
this._leashStretch = messageValues.ReadFloatElement(0);
}
#endregion
private void CalculatePositionFromGPS()
{
if (this._gps1.Distance == 0 || this._gps2.Distance == 0 || this._gps3.Distance == 0)
return;
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 / _constantD);
double gpsPosY = (gpsFactorA * _constantD - _constantA * gpsFactorB) / (_constantB * _constantD);
double gpsPosY = -1 * (gpsFactorA * _constantD - _constantA * gpsFactorB) / (_constantB * _constantD);
AddCalibrationValues(gpsPosX, gpsPosY);
if (this._leashStretch == 0)
AddCalibrationValues(gpsPosX, gpsPosY);
double posX = gpsPosX + this._config.CalibrationX;
double posY = gpsPosY + this._config.CalibrationY;
ValueTuple<double, double> calibrationValues = GetCalibrationValues();
double posX = gpsPosX + calibrationValues.Item1;
double posY = gpsPosY + calibrationValues.Item2;
double inverseLength = 1 / Math.Sqrt(Math.Pow(posX, 2) + Math.Pow(posY, 2));
_unitVectorLeash.X = posX * inverseLength * -1;
_unitVectorLeash.Y = posY * inverseLength;
_unitVectorLeash.X = inverseLength is not Double.PositiveInfinity ? posX * inverseLength : 0;
_unitVectorLeash.Y = inverseLength is not Double.PositiveInfinity ? posY * inverseLength : 0;
if (_leashStretch < this._config.WalkStretchDeadzone) //Below Deadzone
_movementVector = new();
@ -204,20 +163,19 @@ public partial class OSCCollar
{
while (true)
{
this._server.Update();
CalculatePositionFromGPS();
if (_lastNilMessageSent.Add(MessageMinInterval) < DateTime.Now)
{
this._client.Send("/input/Vertical", 0f);
this._client.Send("/input/Horizontal", 0f);
this._osc.Client.Send("/input/Vertical", 0f);
this._osc.Client.Send("/input/Horizontal", 0f);
this._lastNilMessageSent = DateTime.Now;
PrintConsole();
}else if (_allowMoving)
{
this._client.Send("/input/Vertical", Convert.ToSingle(_movementVector.Y));
this._client.Send("/input/Horizontal", Convert.ToSingle(_movementVector.X));
this._osc.Client.Send("/input/Vertical", Convert.ToSingle(_movementVector.Y));
this._osc.Client.Send("/input/Horizontal", Convert.ToSingle(_movementVector.X));
}
PrintConsole();
Thread.Sleep(UpdateInterval);
}
}
@ -225,12 +183,14 @@ public partial class OSCCollar
private void AddCalibrationValues(double x, double y)
{
this._calibrationAverage.Enqueue(new ValueTuple<double, double>(x, y));
if (this._calibrationAverage.Count > 50)
while (this._calibrationAverage.Count > this._config.CalibrationValues)
this._calibrationAverage.Dequeue();
}
private ValueTuple<double, double> GetCalibrationValues()
{
if (this._calibrationAverage.Count < 1)
return new ValueTuple<double, double>(0, 0);
double averageX = -this._calibrationAverage.Average(value => value.Item1);
double averageY = -this._calibrationAverage.Average(value => value.Item2);
return new ValueTuple<double, double>(averageX, averageY);

View File

@ -11,6 +11,7 @@
<ItemGroup>
<PackageReference Include="BuildSoft.OscCore" Version="1.2.1.1" />
<PackageReference Include="GlaxLogger" Version="1.0.6" />
<PackageReference Include="MeaMod.DNS" Version="1.0.70" />
<PackageReference Include="Microsoft.Extensions.Logging" Version="8.0.0" />
<PackageReference Include="Newtonsoft.Json" Version="13.0.3" />
@ -18,6 +19,9 @@
</ItemGroup>
<ItemGroup>
<Reference Include="GlaxOSC">
<HintPath>..\..\GlaxOSC\GlaxOSC\bin\Debug\net7.0\GlaxOSC.dll</HintPath>
</Reference>
<Reference Include="vrc-oscquery-lib">
<HintPath>..\..\vrc-oscquery-lib\vrc-oscquery-lib\bin\Debug\net6.0\vrc-oscquery-lib.dll</HintPath>
</Reference>

View File

@ -1,4 +1,5 @@
<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:Boolean x:Key="/Default/AddReferences/RecentPaths/=C_003A_005CUsers_005CGlax_005CRiderProjects_005CGlaxOSC_005CGlaxOSC_005Cbin_005CDebug_005Cnet7_002E0_005CGlaxOSC_002Edll/@EntryIndexedValue">True</s:Boolean>
<s:Boolean x:Key="/Default/AddReferences/RecentPaths/=C_003A_005CUsers_005CGlax_005CRiderProjects_005Cvrc_002Doscquery_002Dlib_005Cvrc_002Doscquery_002Dlib_005Cbin_005CDebug_005Cnet6_002E0_005Cvrc_002Doscquery_002Dlib_002Edll/@EntryIndexedValue">True</s:Boolean>
<s:Boolean x:Key="/Default/ResxEditorPersonal/CheckedGroups/=OSCCollar_002FResources/@EntryIndexedValue">True</s:Boolean>
<s:Boolean x:Key="/Default/ResxEditorPersonal/Initialized/@EntryValue">True</s:Boolean></wpf:ResourceDictionary>