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