Add OSC haptics support (#3)

* Implement a basic OSC haptics system

* Fix OSC address

* Implement bHaptics

* Add config to toggle bHaptics support

* Add toggle for AXHaptics support

* Cache bundle address bytes

* Add new AXHaptics & make it modular

* Legacy AXHaptics was never used

* Haptic config & add proximity haptics

* Simplify config + proximity calculation

AxSlime/AxSlime.csproj

@@ -10,10 +10,11 @@
   </PropertyGroup>
 
   <ItemGroup>
-    <PackageReference Include="CSharpier.MsBuild" Version="0.27.2">
+    <PackageReference Include="CSharpier.MsBuild" Version="0.27.3">
       <PrivateAssets>all</PrivateAssets>
       <IncludeAssets>runtime; build; native; contentfiles; analyzers; buildtransitive</IncludeAssets>
     </PackageReference>
+    <PackageReference Include="LucHeart.CoreOSC" Version="1.2.1" />
   </ItemGroup>
 
 </Project>

AxSlime/Axis/AxisRawData.cs

@@ -2,7 +2,7 @@
 
 namespace AxSlime.Axis
 {
-    public enum NodeBinding
+    public enum NodeBinding : byte
     {
         RightThigh,
         RightCalf,

AxSlime/Config/AxSlimeConfig.cs

@@ -8,13 +8,27 @@ public record AxSlimeConfig
         public static readonly AxSlimeConfig Default = new();
 
         [JsonPropertyName("config_version")]
-        public int ConfigVersion { get; set; } = 0;
+        public int ConfigVersion { get; set; } = 1;
 
         [JsonPropertyName("slimevr_endpoint")]
         public string SlimeVrEndPointStr { get; set; } = "127.0.0.1:6969";
 
+        [JsonPropertyName("osc_enabled")]
+        public bool OscEnabled { get; set; } = false;
+
+        [JsonPropertyName("osc_receive_endpoint")]
+        public string OscReceiveEndPointStr { get; set; } = "127.0.0.1:9001";
+
+        [JsonPropertyName("haptic_vibration")]
+        public HapticsConfig Haptics { get; set; } = new();
+
+        // Utilities
+
         [JsonIgnore]
         public IPEndPoint SlimeVrEndPoint => IPEndPoint.Parse(SlimeVrEndPointStr);
+
+        [JsonIgnore]
+        public IPEndPoint OscReceiveEndPoint => IPEndPoint.Parse(OscReceiveEndPointStr);
     }
 
     [JsonSerializable(typeof(AxSlimeConfig))]

AxSlime/Config/HapticsConfig.cs

@@ -0,0 +1,57 @@
+using System.Text.Json.Serialization;
+
+namespace AxSlime.Config
+{
+    public record class HapticsConfig
+    {
+        public static readonly HapticsConfig Default = new();
+
+        [JsonPropertyName("enable_touch")]
+        public bool EnableTouch { get; set; } = true;
+
+        [JsonPropertyName("touch_intensity")]
+        public float TouchIntensity { get; set; } = 1f;
+
+        [JsonPropertyName("touch_duration_s")]
+        public float TouchDurationS { get; set; } = 1f;
+
+        [JsonPropertyName("enable_proximity")]
+        public bool EnableProx { get; set; } = true;
+
+        [JsonPropertyName("proximity_threshold")]
+        public float ProxThreshold { get; set; } = 0f;
+
+        [JsonPropertyName("proximity_min_intensity")]
+        public float ProxMinIntensity { get; set; } = 0.25f;
+
+        [JsonPropertyName("proximity_max_intensity")]
+        public float ProxMaxIntensity { get; set; } = 1f;
+
+        [JsonPropertyName("proximity_duration_s")]
+        public float ProxDurationS { get; set; } = 0.1f;
+
+        [JsonPropertyName("linear_proximity")]
+        public bool LinearProx { get; set; } = false;
+
+        [JsonPropertyName("enable_axhaptics_support")]
+        public bool EnableAxHaptics { get; set; } = true;
+
+        [JsonPropertyName("enable_bhaptics_support")]
+        public bool EnableBHaptics { get; set; } = true;
+
+        // Utilities
+
+        [JsonIgnore]
+        public float ProxIntensityRange => ProxMaxIntensity - ProxMinIntensity;
+
+        public float CalcIntensity(float proximity)
+        {
+            var scaledProx = LinearProx ? proximity : proximity * proximity;
+            return float.Clamp(
+                ProxMinIntensity + (scaledProx * ProxIntensityRange),
+                ProxMinIntensity,
+                ProxMaxIntensity
+            );
+        }
+    }
+}

AxSlime/Osc/AxHaptics.cs

@@ -0,0 +1,57 @@
+using AxSlime.Axis;
+using AxSlime.Config;
+using LucHeart.CoreOSC;
+
+namespace AxSlime.Osc
+{
+    public class AxHaptics : HapticsSource
+    {
+        public static readonly string AxHapticsPrefix = "VRCOSC/AXHaptics/";
+        public static readonly string BinaryPrefix = "Touched";
+        public static readonly string AnalogPrefix = "Proximity";
+
+        private static readonly Dictionary<string, NodeBinding> _nameToNode =
+            Enum.GetValues<NodeBinding>().ToDictionary(v => Enum.GetName(v)!);
+
+        private readonly AxSlimeConfig _config;
+
+        public AxHaptics(AxSlimeConfig config)
+        {
+            _config = config;
+        }
+
+        public HapticEvent[] ComputeHaptics(string parameter, OscMessage message)
+        {
+            var axHaptics = parameter[AxHapticsPrefix.Length..];
+            if (_config.Haptics.EnableTouch && axHaptics.StartsWith(BinaryPrefix))
+            {
+                var trigger = message.Arguments[0] as bool?;
+                if (trigger != true)
+                    return [];
+
+                if (_nameToNode.TryGetValue(axHaptics[BinaryPrefix.Length..], out var nodeVal))
+                    return [new HapticEvent(nodeVal)];
+            }
+            else if (_config.Haptics.EnableProx && axHaptics.StartsWith(AnalogPrefix))
+            {
+                var proximity = message.Arguments[0] as float? ?? -1f;
+                if (proximity <= _config.Haptics.ProxThreshold)
+                    return [];
+
+                var intensity = _config.Haptics.CalcIntensity(proximity);
+                if (
+                    intensity > 0f
+                    && _nameToNode.TryGetValue(axHaptics[AnalogPrefix.Length..], out var nodeVal)
+                )
+                    return [new HapticEvent(nodeVal, intensity, _config.Haptics.ProxDurationS)];
+            }
+
+            return [];
+        }
+
+        public bool IsSource(string parameter, OscMessage message)
+        {
+            return _config.Haptics.EnableAxHaptics && parameter.StartsWith(AxHapticsPrefix);
+        }
+    }
+}

AxSlime/Osc/HapticEvent.cs

@@ -0,0 +1,18 @@
+using AxSlime.Axis;
+
+namespace AxSlime.Osc
+{
+    public readonly record struct HapticEvent
+    {
+        public readonly NodeBinding Node;
+        public readonly float? Intensity;
+        public readonly float? Duration;
+
+        public HapticEvent(NodeBinding node, float? intensity = null, float? duration = null)
+        {
+            Node = node;
+            Intensity = intensity;
+            Duration = duration;
+        }
+    }
+}

AxSlime/Osc/HapticsSource.cs

@@ -0,0 +1,10 @@
+using LucHeart.CoreOSC;
+
+namespace AxSlime.Osc
+{
+    public interface HapticsSource
+    {
+        public HapticEvent[] ComputeHaptics(string parameter, OscMessage message);
+        public bool IsSource(string parameter, OscMessage message);
+    }
+}

AxSlime/Osc/OscHandler.cs

@@ -0,0 +1,132 @@
+using System.Net.Sockets;
+using System.Text;
+using AxSlime.Axis;
+using AxSlime.Config;
+using LucHeart.CoreOSC;
+
+namespace AxSlime.Osc
+{
+    public class OscHandler : IDisposable
+    {
+        public static readonly string BundleAddress = "#bundle\0";
+        public static readonly byte[] BundleAddressBytes = Encoding.ASCII.GetBytes(BundleAddress);
+        public static readonly string AvatarParamPrefix = "/avatar/parameters/";
+
+        private readonly AxSlimeConfig _config;
+        private readonly AxisCommander _axisCommander;
+        private readonly UdpClient _oscClient;
+
+        private readonly CancellationTokenSource _cancelTokenSource = new();
+        private readonly Task _oscReceiveTask;
+
+        private readonly AxHaptics _axHaptics;
+        private readonly bHaptics _bHaptics;
+
+        private readonly HapticsSource[] _hapticsSources;
+
+        public OscHandler(AxSlimeConfig config, AxisCommander axisCommander)
+        {
+            _config = config;
+            _axisCommander = axisCommander;
+            _oscClient = new UdpClient(config.OscReceiveEndPoint);
+            _oscReceiveTask = OscReceiveTask(_cancelTokenSource.Token);
+
+            _axHaptics = new(config);
+            _bHaptics = new(config);
+
+            _hapticsSources = [_axHaptics, _bHaptics];
+        }
+
+        private static bool IsBundle(ReadOnlySpan<byte> buffer)
+        {
+            return buffer.Length > 16 && buffer[..8].SequenceEqual(BundleAddressBytes);
+        }
+
+        private async Task OscReceiveTask(CancellationToken cancelToken = default)
+        {
+            while (!cancelToken.IsCancellationRequested)
+            {
+                try
+                {
+                    var packet = await _oscClient.ReceiveAsync(cancelToken);
+                    if (IsBundle(packet.Buffer))
+                    {
+                        var bundle = OscBundle.ParseBundle(packet.Buffer);
+                        if (bundle.Timestamp > DateTime.Now)
+                        {
+                            // Wait for the specified timestamp
+                            _ = Task.Run(
+                                async () =>
+                                {
+                                    await Task.Delay(bundle.Timestamp - DateTime.Now, cancelToken);
+                                    OnOscBundle(bundle);
+                                },
+                                cancelToken
+                            );
+                        }
+                        else
+                        {
+                            OnOscBundle(bundle);
+                        }
+                    }
+                    else
+                    {
+                        OnOscMessage(OscMessage.ParseMessage(packet.Buffer));
+                    }
+                }
+                catch (OperationCanceledException) { }
+                catch (Exception e)
+                {
+                    Console.Error.WriteLine(e);
+                }
+            }
+        }
+
+        private void OnOscBundle(OscBundle bundle)
+        {
+            foreach (var message in bundle.Messages)
+            {
+                OnOscMessage(message);
+            }
+        }
+
+        private void OnOscMessage(OscMessage message)
+        {
+            if (message.Arguments.Length <= 0)
+                return;
+
+            foreach (var @event in ComputeEvents(message))
+            {
+                _axisCommander.SetNodeVibration(
+                    (byte)@event.Node,
+                    @event.Intensity ?? _config.Haptics.TouchIntensity,
+                    @event.Duration ?? _config.Haptics.TouchDurationS
+                );
+            }
+        }
+
+        private HapticEvent[] ComputeEvents(OscMessage message)
+        {
+            if (message.Address.Length <= AvatarParamPrefix.Length)
+                return [];
+
+            var param = message.Address[AvatarParamPrefix.Length..];
+            foreach (var source in _hapticsSources)
+            {
+                if (source.IsSource(param, message))
+                    return source.ComputeHaptics(param, message);
+            }
+
+            return [];
+        }
+
+        public void Dispose()
+        {
+            _cancelTokenSource.Cancel();
+            _oscReceiveTask.Wait();
+            _cancelTokenSource.Dispose();
+            _oscClient.Dispose();
+            GC.SuppressFinalize(this);
+        }
+    }
+}

AxSlime/Osc/bHaptics.cs

@@ -0,0 +1,66 @@
+using AxSlime.Axis;
+using AxSlime.Config;
+using LucHeart.CoreOSC;
+
+namespace AxSlime.Osc
+{
+    public class bHaptics : HapticsSource
+    {
+        public static readonly string bHapticsPrefix = "bHapticsOSC_";
+
+        private static readonly Dictionary<string, NodeBinding[]> _mappings =
+            new()
+            {
+                { "Vest_Front", [NodeBinding.Chest, NodeBinding.Hips] },
+                { "Vest_Back", [NodeBinding.Chest, NodeBinding.Hips] },
+                { "Arm_Left", [NodeBinding.LeftUpperArm, NodeBinding.LeftForeArm] },
+                { "Arm_Right", [NodeBinding.RightUpperArm, NodeBinding.RightForeArm] },
+                {
+                    "Foot_Left",
+                    [NodeBinding.LeftFoot, NodeBinding.LeftCalf, NodeBinding.LeftThigh]
+                },
+                {
+                    "Foot_Right",
+                    [NodeBinding.RightFoot, NodeBinding.RightCalf, NodeBinding.RightThigh]
+                },
+                { "Hand_Left", [NodeBinding.LeftHand] },
+                { "Hand_Right", [NodeBinding.RightHand] },
+                { "Head", [NodeBinding.Head] },
+            };
+
+        private static readonly Dictionary<string, HapticEvent[]> _eventMap = _mappings
+            .Select(m => (m.Key, m.Value.Select(n => new HapticEvent(n)).ToArray()))
+            .ToDictionary();
+
+        private readonly AxSlimeConfig _config;
+
+        public bHaptics(AxSlimeConfig config)
+        {
+            _config = config;
+        }
+
+        public HapticEvent[] ComputeHaptics(string parameter, OscMessage message)
+        {
+            if (!_config.Haptics.EnableTouch)
+                return [];
+
+            var trigger = message.Arguments[0] as bool?;
+            if (trigger != true)
+                return [];
+
+            var bHaptics = parameter[bHapticsPrefix.Length..];
+            foreach (var binding in _eventMap)
+            {
+                if (bHaptics.StartsWith(binding.Key))
+                    return binding.Value;
+            }
+
+            return [];
+        }
+
+        public bool IsSource(string parameter, OscMessage message)
+        {
+            return _config.Haptics.EnableBHaptics && parameter.StartsWith(bHapticsPrefix);
+        }
+    }
+}