diff --git a/OSCCollar/OSCCollar.cs b/OSCCollar/OSCCollar.cs index 18733b4..5f7a198 100644 --- a/OSCCollar/OSCCollar.cs +++ b/OSCCollar/OSCCollar.cs @@ -21,7 +21,7 @@ public class OSCCollar double walkStretchDeadzone = jObject.GetValue("walkStretchDeadzone")?.ToObject() ?? 0.1; double runStretch = jObject.GetValue("runStretch")?.ToObject() ?? 0.4; bool skipSetup = jObject.GetValue("skipSetup")?.ToObject() ?? true; - var _ = new OSCCollar(ip, portReceive, portSend, radius, calibrationX, calibrationY, skipSetup); + var _ = new OSCCollar(ip, portReceive, portSend, radius, calibrationX, calibrationY, walkStretchDeadzone, runStretch, skipSetup); }else { var _ = new OSCCollar(); @@ -31,12 +31,13 @@ public class OSCCollar private OscServer Server { get; init; } private OscClient Client { get; init; } private float _leashStretch; - private double _posX, _posY, _verticalMovement, _horizontalMovement; + 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(100); + 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; @@ -45,7 +46,8 @@ public class OSCCollar private GPS GPS3 { get; init; } private double CalibrationX { get; init; } private double CalibrationY { get; init; } - private string debugValue = ""; + private readonly Queue> _calibrationAverage = new(); + private string _debugValue = ""; private double GPSConstantA { get; init; } private double GPSConstantB { get; init; } private double GPSConstantC { get; init; } @@ -91,7 +93,7 @@ public class OSCCollar runningThread.Start(); } - private void PrintOutput(double verticalMovement, double horizontalMovement) + private void PrintOutput() { Console.Clear(); Console.ForegroundColor = ConsoleColor.White; @@ -99,43 +101,57 @@ public class OSCCollar Console.WriteLine("=============================="); Console.WriteLine($"GPS 1:................{GPS1.Distance:00.00000}"); Console.WriteLine($"GPS 2:................{GPS2.Distance:00.00000}"); - Console.WriteLine($"GPS 3:................{GPS3.Distance:0.00000}"); - Console.WriteLine($"Position X:..........{_posX:'.'0.00000;'-'0.00000}"); - Console.WriteLine($"Position Y:..........{_posY:'.'0.00000;'-'0.00000}"); - Console.WriteLine($"Stretch:...............{_leashStretch:0.00000}"); - Console.WriteLine($"Vertical Movement:...{verticalMovement:' '0.00000;'-'0.00000}"); - Console.WriteLine($"Horizontal Movement:..{horizontalMovement:' '0.00000;'-'0.00000}"); - string nilString = $"/input nil {_nilSent}"; + 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 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(40, 5); + Console.SetCursorPosition(43, 5); Console.WriteLine("----------+----------"); for (int i = 1; i < 10; i++) { if(i == 5) continue; - Console.SetCursorPosition(50,i); + Console.SetCursorPosition(53,i); Console.Write("|"); } - const int centerX = 50; + const int centerX = 53; const int centerY = 5; - int consoleX = Convert.ToInt32(Math.Floor(_horizontalMovement * 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); + 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(c); - Console.SetCursorPosition(Console.WindowWidth - 1, Console.WindowHeight - 1); + 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.SetCursorPosition(Console.WindowWidth - _debugValue.Length - 1, Console.WindowHeight - 2); Console.ForegroundColor = ConsoleColor.Gray; - Console.WriteLine(debugValue); + Console.WriteLine(_debugValue); } #region Handle OSC-Messages @@ -169,14 +185,26 @@ public class OSCCollar { 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); - _posX = (gpsFactorB / GPSConstantD) + CalibrationX; - _posY = (gpsFactorA * GPSConstantD - GPSConstantA * gpsFactorB) / (GPSConstantB * GPSConstantD) + CalibrationY; + double posX = gpsPosX + CalibrationX; + double posY = gpsPosY + CalibrationY; + + double inverseLength = 1 / Math.Sqrt(Math.Pow(posX, 2) + Math.Pow(posY, 2)); - double inverseLength = 1 / Math.Sqrt(Math.Pow(_posX, 2) + Math.Pow(_posY, 2)); - - _verticalMovement = _posY * inverseLength * _leashStretch; - _horizontalMovement = _posX * inverseLength * -1 * _leashStretch; + _unitVectorLeash.X = posX * inverseLength * -1; + _unitVectorLeash.Y = posY * inverseLength; + + if (_leashStretch < _walkStretch) //Below Deadzone + _movementVector = new(); + else if (_walkStretch < _runStretch) //Walk + _movementVector = _unitVectorLeash.MultipliedWith(0.5); + else //Run + _movementVector = _unitVectorLeash.MultipliedWith(1); } private void RunningThread() @@ -189,30 +217,35 @@ public class OSCCollar { this.Client.Send("/input/Vertical", 0f); this.Client.Send("/input/Horizontal", 0f); - this.Client.Send("/input/Run", 0); this._lastNilMessageSent = DateTime.Now; _nilSentCount++; }else if (_allowMoving) { - if (_leashStretch > 0.1) - { - this.Client.Send("/input/Vertical", Convert.ToSingle(_verticalMovement)); - this.Client.Send("/input/Horizontal", Convert.ToSingle(_horizontalMovement)); - } - - if (_leashStretch > 0.4) - this.Client.Send("/input/Run", 1); - else - this.Client.Send("/input/Run", 0); + 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(_verticalMovement, _horizontalMovement); + PrintOutput(); _lastConsoleOutput = DateTime.Now; } Thread.Sleep(UpdateInterval); } } + + private void AddCalibrationValues(double x, double y) + { + this._calibrationAverage.Enqueue(new ValueTuple(x, y)); + if (this._calibrationAverage.Count > 50) + this._calibrationAverage.Dequeue(); + } + + private ValueTuple GetCalibrationValues() + { + double averageX = -this._calibrationAverage.Average(value => value.Item1); + double averageY = -this._calibrationAverage.Average(value => value.Item2); + return new ValueTuple(averageX, averageY); + } } \ No newline at end of file diff --git a/OSCCollar/Vector.cs b/OSCCollar/Vector.cs new file mode 100644 index 0000000..8874a63 --- /dev/null +++ b/OSCCollar/Vector.cs @@ -0,0 +1,29 @@ +namespace VRC_Console; + +public class Vector +{ + public double X { get; set; } + public double Y { get; set; } + + public Vector() + { + this.X = 0; + this.Y = 0; + } + + public Vector(double x, double y) + { + this.X = x; + this.Y = y; + } + + public override string ToString() + { + return $"({this.X:' '0.00000;'-'0.00000} {this.Y:' '0.00000;'-'0.00000})"; + } + + public Vector MultipliedWith(double factor) + { + return new Vector(this.X * factor, this.Y * factor); + } +} \ No newline at end of file