282 lines
12 KiB
C#
282 lines
12 KiB
C#
using System.Net;
|
|
using BuildSoft.OscCore;
|
|
using Newtonsoft.Json.Linq;
|
|
using VRC.OSCQuery;
|
|
using Extensions = VRC.OSCQuery.Extensions;
|
|
|
|
namespace VRC_Console;
|
|
|
|
public class OSCCollar
|
|
{
|
|
|
|
public static void Main(string[] args)
|
|
{
|
|
string configFilePath = Path.Combine(Directory.GetCurrentDirectory(), "config.json");
|
|
if (File.Exists(configFilePath))
|
|
{
|
|
var jObject = JObject.Parse(File.ReadAllText(configFilePath));
|
|
string ip = jObject.GetValue("ip")?.ToObject<string>() ?? "127.0.0.1";
|
|
int portSend = jObject.GetValue("portSend")?.ToObject<int>() ?? 9000;
|
|
double radius = jObject.GetValue("radius")?.ToObject<double>() ?? 100;
|
|
double calibrationX = jObject.GetValue("calibrationX")?.ToObject<double>() ?? 0;
|
|
double calibrationY = jObject.GetValue("calibrationY")?.ToObject<double>() ?? 0;
|
|
double walkStretchDeadzone = jObject.GetValue("walkStretchDeadzone")?.ToObject<double>() ?? 0.1;
|
|
double runStretch = jObject.GetValue("runStretch")?.ToObject<double>() ?? 0.4;
|
|
bool skipSetup = jObject.GetValue("skipSetup")?.ToObject<bool>() ?? true;
|
|
var _ = new OSCCollar(ip, portSend, radius, calibrationX, calibrationY, walkStretchDeadzone, runStretch, skipSetup);
|
|
}else
|
|
{
|
|
var _ = new OSCCollar();
|
|
}
|
|
}
|
|
|
|
private readonly string _ip;
|
|
private readonly int _portReceive, _portSend;
|
|
|
|
private OSCQueryService _oscQueryService { get; init; }
|
|
private OscServer Server { get; init; }
|
|
private OscClient Client { get; init; }
|
|
private float _leashStretch;
|
|
private readonly Vector _unitVectorLeash = new(1, 0);
|
|
private Vector _movementVector = new(0, 0);
|
|
private bool _allowMoving = true;
|
|
private uint _nilSentCount;
|
|
private DateTime _lastNilMessageSent = DateTime.UnixEpoch;
|
|
private DateTime _lastConsoleOutput = DateTime.UnixEpoch;
|
|
private static readonly TimeSpan ConsoleUpdateInterval = TimeSpan.FromMilliseconds(500);
|
|
private static readonly TimeSpan UpdateInterval = TimeSpan.FromMilliseconds(10);
|
|
private static readonly TimeSpan MessageMinInterval = TimeSpan.FromMilliseconds(400);
|
|
private readonly double _radius, _walkStretch, _runStretch;
|
|
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 readonly Queue<ValueTuple<double, double>> _calibrationAverage = new();
|
|
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 portSend = 9000, double radius = 100, double calibrationX = 0, double calibrationY = 0, double walkStretch = 0.1, double runStretch = 0.4, bool skipSetup = true)
|
|
{
|
|
this._ip = ip;
|
|
var tcpPort = Extensions.GetAvailableTcpPort();
|
|
var udpPort = Extensions.GetAvailableUdpPort();
|
|
this._oscQueryService = new OSCQueryServiceBuilder()
|
|
.WithHostIP(IPAddress.Parse(ip))
|
|
.WithOscIP(IPAddress.Parse(ip))
|
|
.WithDiscovery(new MeaModDiscovery())
|
|
.WithTcpPort(tcpPort)
|
|
.WithUdpPort(udpPort)
|
|
.WithServiceName("Collar")
|
|
.StartHttpServer()
|
|
.AdvertiseOSC()
|
|
.AdvertiseOSCQuery()
|
|
.Build();
|
|
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);
|
|
this._portReceive = this._oscQueryService.OscPort;
|
|
this._portSend = portSend;
|
|
this._walkStretch = walkStretch;
|
|
this._runStretch = runStretch;
|
|
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($"OSC Port: {this._portReceive} TCP Port: {this._oscQueryService.TcpPort}");
|
|
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}");
|
|
Console.ReadKey();
|
|
}
|
|
|
|
this.Server = new OscServer(this._portReceive);
|
|
this.Client = new OscClient(this._ip, this._portSend);
|
|
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();
|
|
|
|
this._oscQueryService.RefreshServices();
|
|
}
|
|
|
|
private void PrintOutput()
|
|
{
|
|
Console.Clear();
|
|
Console.ForegroundColor = ConsoleColor.White;
|
|
Console.WriteLine($"OSC Collar - Status: {(_allowMoving ? "enabled" : "disabled")} IP: {_ip} Send: {_portSend} Receive: {_portReceive} HTTP: {this._oscQueryService.TcpPort}");
|
|
Console.WriteLine("==============================");
|
|
Console.WriteLine($"GPS 1:................{GPS1.Distance:00.00000}");
|
|
Console.WriteLine($"GPS 2:................{GPS2.Distance:00.00000}");
|
|
Console.WriteLine($"GPS 3:................{GPS3.Distance:00.00000}");
|
|
Console.ForegroundColor = ConsoleColor.Cyan;
|
|
Console.WriteLine($"Position Vector:......{_unitVectorLeash}");
|
|
Console.ForegroundColor = ConsoleColor.White;
|
|
Console.WriteLine($"Stretch:..............{_leashStretch:0.00000}");
|
|
Console.ForegroundColor = ConsoleColor.Red;
|
|
Console.WriteLine($"Movement Vector:......{_movementVector}");
|
|
Console.ForegroundColor = ConsoleColor.Gray;
|
|
ValueTuple<double, double> calibrationValues = GetCalibrationValues();
|
|
Console.WriteLine($"Calibration (x y): ...({calibrationValues.Item1:' '00.0000;'-'00.0000} {calibrationValues.Item2:' '00.0000;'-'00.0000})");
|
|
Console.ForegroundColor = ConsoleColor.White;
|
|
|
|
|
|
string nilString = $"/input nil {_nilSentCount}";
|
|
Console.SetCursorPosition(Console.WindowWidth - nilString.Length - 1, 0);
|
|
Console.Write(nilString);
|
|
|
|
Console.ForegroundColor = ConsoleColor.DarkGray;
|
|
if (Console.WindowHeight < 13)
|
|
return;
|
|
Console.SetCursorPosition(43, 5);
|
|
Console.WriteLine("----------+----------");
|
|
for (int i = 1; i < 10; i++)
|
|
{
|
|
if(i == 5)
|
|
continue;
|
|
Console.SetCursorPosition(53,i);
|
|
Console.Write("|");
|
|
}
|
|
|
|
const int centerX = 53;
|
|
const int centerY = 5;
|
|
int positionX = Convert.ToInt32(Math.Floor(_unitVectorLeash.X * 10));
|
|
int positionY = Convert.ToInt32(-Math.Floor(_unitVectorLeash.Y * 4));
|
|
double positionPrecision = (_unitVectorLeash.Y * 10) % 10;
|
|
char positionPrecisionChar = positionPrecision < 3 ? '.' : positionPrecision > 7 ? '\'' : 'x';
|
|
Console.SetCursorPosition(centerX + positionX, centerY + positionY);
|
|
Console.ForegroundColor = ConsoleColor.Cyan;
|
|
Console.Write(positionPrecisionChar);
|
|
|
|
int movementX = Convert.ToInt32(Math.Floor(_movementVector.X * 10));
|
|
int movementY = Convert.ToInt32(-Math.Floor(_movementVector.Y * 4));
|
|
double movementPrecision = (_movementVector.Y * 10) % 10;
|
|
char movementPrecisionChar = movementPrecision < 3 ? '.' : movementPrecision > 7 ? '\'' : 'x';
|
|
Console.SetCursorPosition(centerX + movementX, centerY + movementY);
|
|
Console.ForegroundColor = ConsoleColor.Red;
|
|
Console.Write(movementPrecisionChar);
|
|
|
|
Console.SetCursorPosition(Console.WindowWidth - _debugValue.Length - 1, Console.WindowHeight - 2);
|
|
Console.ForegroundColor = ConsoleColor.Gray;
|
|
Console.WriteLine(_debugValue);
|
|
}
|
|
|
|
#region Handle OSC-Messages
|
|
private void AllowMovingHandle(OscMessageValues messageValues)
|
|
{
|
|
this._allowMoving = messageValues.ReadBooleanElement(0);
|
|
}
|
|
|
|
private void GPS1Handle(OscMessageValues messageValues)
|
|
{
|
|
this.GPS1.Distance = messageValues.ReadFloatElement(0) * _radius;
|
|
}
|
|
|
|
private void GPS2Handle(OscMessageValues messageValues)
|
|
{
|
|
this.GPS2.Distance = messageValues.ReadFloatElement(0) * _radius;
|
|
}
|
|
|
|
private void GPS3Handle(OscMessageValues messageValues)
|
|
{
|
|
this.GPS3.Distance = messageValues.ReadFloatElement(0) * _radius;
|
|
}
|
|
|
|
private void CollarStretchHandle(OscMessageValues messageValues)
|
|
{
|
|
this._leashStretch = messageValues.ReadFloatElement(0);
|
|
}
|
|
#endregion
|
|
|
|
private void CalculatePositionFromGPS()
|
|
{
|
|
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;
|
|
|
|
double gpsPosX = (gpsFactorB / GPSConstantD);
|
|
double gpsPosY = (gpsFactorA * GPSConstantD - GPSConstantA * gpsFactorB) / (GPSConstantB * GPSConstantD);
|
|
|
|
AddCalibrationValues(gpsPosX, gpsPosY);
|
|
|
|
double posX = gpsPosX + CalibrationX;
|
|
double posY = gpsPosY + CalibrationY;
|
|
|
|
double inverseLength = 1 / Math.Sqrt(Math.Pow(posX, 2) + Math.Pow(posY, 2));
|
|
|
|
_unitVectorLeash.X = posX * inverseLength * -1;
|
|
_unitVectorLeash.Y = posY * inverseLength;
|
|
|
|
if (_leashStretch < _walkStretch) //Below Deadzone
|
|
_movementVector = new();
|
|
else if (_leashStretch < _runStretch) //Walk
|
|
_movementVector = _unitVectorLeash.MultipliedWith(0.5);
|
|
else //Run
|
|
_movementVector = _unitVectorLeash.MultipliedWith(1);
|
|
}
|
|
|
|
private void RunningThread()
|
|
{
|
|
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._lastNilMessageSent = DateTime.Now;
|
|
_nilSentCount++;
|
|
}else if (_allowMoving)
|
|
{
|
|
this.Client.Send("/input/Vertical", Convert.ToSingle(_movementVector.Y));
|
|
this.Client.Send("/input/Horizontal", Convert.ToSingle(_movementVector.X));
|
|
}
|
|
|
|
if (_lastConsoleOutput.Add(ConsoleUpdateInterval) < DateTime.Now)
|
|
{
|
|
PrintOutput();
|
|
_lastConsoleOutput = DateTime.Now;
|
|
}
|
|
|
|
Thread.Sleep(UpdateInterval);
|
|
}
|
|
}
|
|
|
|
private void AddCalibrationValues(double x, double y)
|
|
{
|
|
this._calibrationAverage.Enqueue(new ValueTuple<double, double>(x, y));
|
|
if (this._calibrationAverage.Count > 50)
|
|
this._calibrationAverage.Dequeue();
|
|
}
|
|
|
|
private ValueTuple<double, double> GetCalibrationValues()
|
|
{
|
|
double averageX = -this._calibrationAverage.Average(value => value.Item1);
|
|
double averageY = -this._calibrationAverage.Average(value => value.Item2);
|
|
return new ValueTuple<double, double>(averageX, averageY);
|
|
}
|
|
} |