Skip to content

Commit

Permalink
Merge pull request #1 from dfgHiatus/geenz
Browse files Browse the repository at this point in the history 
Geenz
  • Loading branch information
@dfgHiatus
dfgHiatus authored Nov 28, 2022
2 parents 08c6734 + 5eab87d commit 48adb48
Show file tree
Hide file tree
Showing 7 changed files with 260 additions and 71 deletions.
152 changes: 109 additions & 43 deletions Interface/ALXR/ALXRModule.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,7 @@
using System.Net;
using System.Net.Sockets;
using System.Threading;
using System.Threading.Tasks;

namespace QuestProModule.ALXR
{
Expand All @@ -25,34 +26,56 @@ public class ALXRModule : IQuestProModule

private double pitch_L, yaw_L, pitch_R, yaw_R; // Eye rotations

public bool Initialize()
public async Task<bool> Initialize(string ipconfig)
{
localAddr = QuestProMod.config.GetValue(QuestProMod.QuestProIP);
cancellationTokenSource = new CancellationTokenSource();
ConnectToTCP(); // Will this block the main thread?
try
{
localAddr = IPAddress.Parse(ipconfig);

tcpThread = new Thread(Update);
tcpThread.Start();
cancellationTokenSource = new CancellationTokenSource();

return true;
UniLog.Log("Attempting to connect to TCP socket.");
var connected = await ConnectToTCP(); // This should not block the main thread anymore...?
}
catch (Exception e)
{
UniLog.Error(e.Message);
return false;
}

if (connected)
{
tcpThread = new Thread(Update);
tcpThread.Start();
return true;
}

return false;
}

private bool ConnectToTCP()
private async Task<bool> ConnectToTCP()
{
try
{
localAddr = QuestProMod.config.GetValue(QuestProMod.QuestProIP);

client = new TcpClient();
UniLog.Log($"Trying to establish a Quest Pro connection at {localAddr}:{DEFAULT_PORT}...");

client.Connect(localAddr, DEFAULT_PORT);
UniLog.Log("Connected to Quest Pro!");
await client.ConnectAsync(localAddr, DEFAULT_PORT);

stream = client.GetStream();
connected = true;
if (client.Connected)
{
UniLog.Log("Connected to Quest Pro!");

return true;
stream = client.GetStream();
connected = true;

return true;
}
else
{
UniLog.Error("Couldn't connect!");
return false;
}
}
catch (Exception e)
{
Expand All @@ -70,7 +93,7 @@ public void Update()
// Attempt reconnection if needed
if (!connected || stream == null)
{
ConnectToTCP();
ConnectToTCP().RunSynchronously();
}

// If the connection was unsuccessful, wait a bit and try again
Expand All @@ -82,7 +105,7 @@ public void Update()

if (!stream.CanRead)
{
UniLog.Warning("Can't read from network stream just yet! Trying again...");
UniLog.Warning("Can't read from the Quest Pro network stream just yet! Trying again...");
return;
}

Expand Down Expand Up @@ -113,6 +136,7 @@ public void Update()

// We receive information from the stream as a byte array 63*4 bytes long, since floats are 32 bits long and we have 63 expressions.
// We then need to convert these bytes to a floats. I've opted to use Buffer.BlockCopy instead of BitConverter.ToSingle, since it's faster.
// Future note: Testing this against Array.Copy() doesn't seem to show any difference in performance, so I'll stick to this
Buffer.BlockCopy(rawExpressions, 0, expressions, 0, NATURAL_EXPRESSIONS_COUNT * 4 + (8 * 2 * 4));

// Preprocess our expressions per Meta's Documentation
Expand All @@ -123,18 +147,17 @@ public void Update()
UniLog.Error(e.Message);
Thread.Sleep(1000);
}

}
}

private void PrepareUpdate()
{
// Eye Expressions

double q_x = expressions[64];
double q_y = expressions[65];
double q_z = expressions[66];
double q_w = expressions[67];
double q_x = expressions[FBExpression.LeftRot_x];
double q_y = expressions[FBExpression.LeftRot_y];
double q_z = expressions[FBExpression.LeftRot_z];
double q_w = expressions[FBExpression.LeftRot_w];

double yaw = Math.Atan2(2.0 * (q_y * q_z + q_w * q_x), q_w * q_w - q_x * q_x - q_y * q_y + q_z * q_z);
double pitch = Math.Asin(-2.0 * (q_x * q_z - q_w * q_y));
Expand All @@ -145,10 +168,10 @@ private void PrepareUpdate()
pitch_L = 180.0 / Math.PI * pitch;
yaw_L = 180.0 / Math.PI * yaw;

q_x = expressions[72];
q_y = expressions[73];
q_z = expressions[74];
q_w = expressions[75];
q_x = expressions[FBExpression.RightRot_x];
q_y = expressions[FBExpression.RightRot_y];
q_z = expressions[FBExpression.RightRot_z];
q_w = expressions[FBExpression.RightRot_w];

yaw = Math.Atan2(2.0 * (q_y * q_z + q_w * q_x), q_w * q_w - q_x * q_x - q_y * q_y + q_z * q_z);
pitch = Math.Asin(-2.0 * (q_x * q_z - q_w * q_y));
Expand Down Expand Up @@ -276,41 +299,84 @@ public void Teardown()
client.Dispose();
}

public void GetEyeExpressions(FBEye fbEye, in FrooxEngine.Eye frooxEye)
bool IsValid(float3 value) => IsValid(value.x) && IsValid(value.y) && IsValid(value.z);

bool IsValid(float value) => !float.IsInfinity(value) && !float.IsNaN(value);

public struct EyeGazeData
{
frooxEye.IsDeviceActive = Engine.Current.InputInterface.VR_Active;
frooxEye.IsTracking = Engine.Current.InputInterface.VR_Active;
frooxEye.PupilDiameter = 0.0035f;
public bool isValid;
public float3 position;
public floatQ rotation;
public float open;
public float squeeze;
public float wide;
public float gazeConfidence;
}

public EyeGazeData GetEyeData(FBEye fbEye)
{
EyeGazeData eyeRet = new EyeGazeData();
switch (fbEye)
{
case FBEye.Left:
eyeRet.position = new float3(expressions[FBExpression.LeftPos_x], expressions[FBExpression.LeftPos_y], expressions[FBExpression.LeftPos_z]);
eyeRet.rotation = new floatQ(-expressions[FBExpression.LeftRot_x], -expressions[FBExpression.LeftRot_z], -expressions[FBExpression.LeftRot_y], expressions[FBExpression.LeftRot_w]);
eyeRet.open = MathX.Max(0, expressions[FBExpression.Eyes_Closed_L]);
eyeRet.squeeze = expressions[FBExpression.Lid_Tightener_L];
eyeRet.wide = expressions[FBExpression.Upper_Lid_Raiser_L];
eyeRet.isValid = IsValid(eyeRet.position);
return eyeRet;
case FBEye.Right:
eyeRet.position = new float3(expressions[FBExpression.RightPos_x], expressions[FBExpression.RightPos_y], expressions[FBExpression.RightPos_z]);
eyeRet.rotation = new floatQ(-expressions[FBExpression.RightRot_x], -expressions[FBExpression.RightRot_z], -expressions[FBExpression.RightRot_y], expressions[FBExpression.RightRot_w]);
eyeRet.open = MathX.Max(0, expressions[FBExpression.Eyes_Closed_R]);
eyeRet.squeeze = expressions[FBExpression.Lid_Tightener_R];
eyeRet.wide = expressions[FBExpression.Upper_Lid_Raiser_R];
eyeRet.isValid = IsValid(eyeRet.position);
return eyeRet;
default:
throw new Exception($"Invalid eye argument: {fbEye}");
}
}

public void GetEyeExpressions(FBEye fbEye, FrooxEngine.Eye frooxEye)
{
frooxEye.PupilDiameter = 0.004f;
frooxEye.Frown = 0f;

switch (fbEye)
{
case FBEye.Left:
frooxEye.RawPosition = new float3(expressions[68], expressions[69], expressions[70]);
frooxEye.RawRotation = new floatQ(expressions[64], expressions[65], expressions[66], expressions[67]);
frooxEye.Openness = expressions[FBExpression.Eyes_Closed_L];
frooxEye.UpdateWithRotation(new floatQ(-expressions[FBExpression.LeftRot_x], -expressions[FBExpression.LeftRot_z], -expressions[FBExpression.LeftRot_y], expressions[FBExpression.LeftRot_w]));
frooxEye.RawPosition = new float3(expressions[FBExpression.LeftPos_x], expressions[FBExpression.LeftPos_y], expressions[FBExpression.LeftPos_z]);
frooxEye.Openness = MathX.Max(0, expressions[FBExpression.Eyes_Closed_L]);
frooxEye.Squeeze = expressions[FBExpression.Lid_Tightener_L];
frooxEye.Widen = expressions[FBExpression.Upper_Lid_Raiser_L];
break;
case FBEye.Right:
frooxEye.RawPosition = new float3(expressions[76], expressions[77], expressions[78]);
frooxEye.RawRotation = new floatQ(expressions[72], expressions[73], expressions[74], expressions[75]);
frooxEye.Openness = expressions[FBExpression.Eyes_Closed_R];
frooxEye.UpdateWithRotation(new floatQ(-expressions[FBExpression.RightRot_x], -expressions[FBExpression.RightRot_z], -expressions[FBExpression.RightRot_y], expressions[FBExpression.RightRot_w]));
frooxEye.RawPosition = new float3(expressions[FBExpression.RightPos_x], expressions[FBExpression.RightPos_y], expressions[FBExpression.RightPos_z]);
frooxEye.Openness = MathX.Max(0, expressions[FBExpression.Eyes_Closed_R]);
frooxEye.Squeeze = expressions[FBExpression.Lid_Tightener_R];
frooxEye.Widen = expressions[FBExpression.Upper_Lid_Raiser_R];
break;
case FBEye.Combined:
frooxEye.RawPosition = MathX.Average(new float3(expressions[68], expressions[69], expressions[70]), new float3(expressions[76], expressions[77], expressions[78]));
frooxEye.RawRotation = MathX.Slerp(new floatQ(expressions[64], expressions[65], expressions[66], expressions[67]), new floatQ(expressions[72], expressions[73], expressions[74], expressions[75]), 0.5f); // Compute the midpoint by slerping from one quaternion to the other
frooxEye.Openness = (expressions[FBExpression.Eyes_Closed_R] + expressions[FBExpression.Eyes_Closed_R]) / 2.0f;
frooxEye.UpdateWithRotation(MathX.Slerp(new floatQ(expressions[FBExpression.LeftRot_x], expressions[FBExpression.LeftRot_y], expressions[FBExpression.LeftRot_z], expressions[FBExpression.LeftRot_w]), new floatQ(expressions[FBExpression.RightRot_x], expressions[FBExpression.RightRot_y], expressions[FBExpression.RightRot_z], expressions[FBExpression.RightRot_w]), 0.5f));
frooxEye.RawPosition = MathX.Average(new float3(expressions[FBExpression.LeftPos_x], expressions[FBExpression.LeftPos_z], expressions[FBExpression.LeftPos_y]), new float3(expressions[FBExpression.RightPos_x], expressions[FBExpression.RightPos_z], expressions[FBExpression.RightPos_y]));
frooxEye.Openness = MathX.Max(0, expressions[FBExpression.Eyes_Closed_R] + expressions[FBExpression.Eyes_Closed_R]) / 2.0f;
frooxEye.Squeeze = (expressions[FBExpression.Lid_Tightener_R] + expressions[FBExpression.Lid_Tightener_R]) / 2.0f;
frooxEye.Widen = (expressions[FBExpression.Upper_Lid_Raiser_R] + expressions[FBExpression.Upper_Lid_Raiser_R]) / 2.0f;
break;
}

frooxEye.IsTracking = IsValid(frooxEye.RawPosition);
frooxEye.IsTracking = IsValid(frooxEye.Direction);
frooxEye.IsTracking = IsValid(frooxEye.Openness);
}

// TODO: Double check jaw movements and mappings
public void GetFacialExpressions(in FrooxEngine.Mouth mouth)
public void GetFacialExpressions(FrooxEngine.Mouth mouth)
{
mouth.IsDeviceActive = Engine.Current.InputInterface.VR_Active;
mouth.IsTracking = Engine.Current.InputInterface.VR_Active;
Expand All @@ -332,8 +398,8 @@ public void GetFacialExpressions(in FrooxEngine.Mouth mouth)
mouth.LipUpperHorizontal = stretch;
mouth.LipLowerHorizontal = stretch;

mouth.MouthLeftSmileFrown = Math.Min(1, expressions[FBExpression.Lip_Corner_Puller_L] * 1.2f) - Math.Min(1, (expressions[FBExpression.Lip_Corner_Depressor_L] + expressions[FBExpression.Lip_Stretcher_L]) * 0.75f);//Math.Min(1, (expressions[FBExpression.Lip_Corner_Depressor_L]) * 1.5f);;
mouth.MouthRightSmileFrown = Math.Min(1, expressions[FBExpression.Lip_Corner_Puller_R] * 1.2f) - Math.Min(1, (expressions[FBExpression.Lip_Corner_Depressor_R] + expressions[FBExpression.Lip_Stretcher_R]) * 0.75f);//Math.Min(1, (expressions[FBExpression.Lip_Corner_Depressor_R]) * 1.5f);;
mouth.MouthLeftSmileFrown = Math.Min(1, expressions[FBExpression.Lip_Corner_Puller_L] * 1.2f) - Math.Min(1, (expressions[FBExpression.Lip_Corner_Depressor_L] + expressions[FBExpression.Lip_Stretcher_L]) * SRANIPAL_NORMALIZER);//Math.Min(1, (expressions[FBExpression.Lip_Corner_Depressor_L]) * 1.5f);;
mouth.MouthRightSmileFrown = Math.Min(1, expressions[FBExpression.Lip_Corner_Puller_R] * 1.2f) - Math.Min(1, (expressions[FBExpression.Lip_Corner_Depressor_R] + expressions[FBExpression.Lip_Stretcher_R]) * SRANIPAL_NORMALIZER);//Math.Min(1, (expressions[FBExpression.Lip_Corner_Depressor_R]) * 1.5f);;

mouth.MouthPout = (expressions[FBExpression.Lip_Pucker_L] + expressions[FBExpression.Lip_Pucker_R]) / 3;

Expand Down
59 changes: 53 additions & 6 deletions Interface/EyeDevice.cs
View file
Original file line number Diff line number Diff line change
@@ -1,12 +1,16 @@
using BaseX;
using FrooxEngine;
using QuestProModule.ALXR;
using System;
using System.Numerics;
using static QuestProModule.ALXR.ALXRModule;

namespace QuestProModule
{
public class EyeDevice : IInputDriver
{
private Eyes _eyes;
InputInterface input;
public int UpdateOrder => 100;

public EyeDevice()
Expand All @@ -31,17 +35,60 @@ public void CollectDeviceInfos(DataTreeList list)
public void RegisterInputs(InputInterface inputInterface)
{
_eyes = new Eyes(inputInterface, "Quest Pro Eye Tracking");
input = inputInterface;
}

public void UpdateInputs(float deltaTime)
{
_eyes.IsEyeTrackingActive = Engine.Current.InputInterface.VR_Active;
_eyes.IsTracking = Engine.Current.InputInterface.VR_Active;
_eyes.IsEyeTrackingActive = input.VR_Active;

_eyes.LeftEye.IsTracking = input.VR_Active;

var leftEyeData = QuestProMod.qpm.GetEyeData(FBEye.Left);
var rightEyeData = QuestProMod.qpm.GetEyeData(FBEye.Right);

_eyes.LeftEye.IsTracking = leftEyeData.isValid;
_eyes.LeftEye.RawPosition = leftEyeData.position;
_eyes.LeftEye.UpdateWithRotation(leftEyeData.rotation);
_eyes.LeftEye.PupilDiameter = 0.004f;
_eyes.LeftEye.Widen = leftEyeData.wide;
_eyes.LeftEye.Squeeze = leftEyeData.squeeze;
_eyes.LeftEye.Openness = leftEyeData.open;

_eyes.RightEye.IsTracking = rightEyeData.isValid;
_eyes.RightEye.RawPosition = rightEyeData.position;
_eyes.RightEye.UpdateWithRotation(rightEyeData.rotation);
_eyes.RightEye.PupilDiameter = 0.004f;
_eyes.RightEye.Widen = rightEyeData.wide;
_eyes.RightEye.Squeeze = rightEyeData.squeeze;
_eyes.RightEye.Openness = rightEyeData.open;

if (_eyes.LeftEye.IsTracking || _eyes.RightEye.IsTracking && (!_eyes.LeftEye.IsTracking || !_eyes.RightEye.IsTracking))
{
if (_eyes.LeftEye.IsTracking)
{
_eyes.CombinedEye.RawPosition = _eyes.LeftEye.RawPosition;
_eyes.CombinedEye.UpdateWithRotation(_eyes.LeftEye.RawRotation);
} else
{
_eyes.CombinedEye.RawPosition = _eyes.RightEye.RawPosition;
_eyes.CombinedEye.UpdateWithRotation(_eyes.RightEye.RawRotation);
}
_eyes.CombinedEye.IsTracking = true;
}
else
{
_eyes.CombinedEye.IsTracking = false;
}

_eyes.CombinedEye.IsTracking = _eyes.LeftEye.IsTracking || _eyes.RightEye.IsTracking;
_eyes.CombinedEye.RawPosition = (_eyes.LeftEye.RawPosition + _eyes.RightEye.RawPosition) * 0.5f;
_eyes.CombinedEye.UpdateWithRotation(MathX.Slerp(_eyes.LeftEye.RawRotation, _eyes.RightEye.RawRotation, 0.5f));
_eyes.CombinedEye.PupilDiameter = 0.004f;

_eyes.LeftEye.Openness = MathX.Pow(_eyes.LeftEye.Openness, QuestProMod.EyeOpenExponent);
_eyes.RightEye.Openness = MathX.Pow(_eyes.RightEye.Openness, QuestProMod.EyeOpenExponent);

QuestProMod.qpm.GetEyeExpressions(FBEye.Left, in _eyes.LeftEye);
QuestProMod.qpm.GetEyeExpressions(FBEye.Right, in _eyes.RightEye);
QuestProMod.qpm.GetEyeExpressions(FBEye.Combined, in _eyes.CombinedEye);

_eyes.ComputeCombinedEyeParameters();
_eyes.ConvergenceDistance = 0f;
_eyes.Timestamp += deltaTime;
Expand Down
19 changes: 18 additions & 1 deletion Interface/FBExpression.cs
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,5 @@
namespace QuestProModule
{
// TODO Get position for eye and face data
public static class FBExpression
{
public const int Brow_Lowerer_L = 0;
Expand Down Expand Up @@ -67,5 +66,23 @@ public static class FBExpression
public const int Upper_Lip_Raiser_L = 61;
public const int Upper_Lip_Raiser_R = 62;
public const int Max = 63;

// Above are the natural expressions tracked by the Quest Pro
// Below is the eye tracking information
public const int LeftRot_x = 64;
public const int LeftRot_y = 65;
public const int LeftRot_z = 66;
public const int LeftRot_w = 67;
public const int LeftPos_x = 68;
public const int LeftPos_y = 70; // Flipped, need to convert RHS to LHS
public const int LeftPos_z = 69;
// public const int 71 is unused
public const int RightRot_x = 72;
public const int RightRot_y = 73;
public const int RightRot_z = 74;
public const int RightRot_w = 75;
public const int RightPos_x = 76;
public const int RightPos_y = 78; // Flipped, need to convert RHS to LHS
public const int RightPos_z = 77;
}
}
7 changes: 5 additions & 2 deletions Interface/IQuestProModule.cs
View file
Original file line number Diff line number Diff line change
@@ -1,8 +1,11 @@
namespace QuestProModule
using System.Threading.Tasks;
using NeosModLoader;

namespace QuestProModule
{
public interface IQuestProModule
{
public bool Initialize();
public Task<bool> Initialize(string ipaddress);

public void Update();

Expand Down
Loading

0 comments on commit 48adb48

Please sign in to comment.