Merge pull request #107 from salmoncheeks/main

Fixed BurstCompiler compatible OneEuroFilter

Basis/Packages/com.basis.framework/Networking/Recievers/BasisNetworkReceiver.cs

@@ -9,7 +9,6 @@
 using UnityEngine;
 using static SerializableBasis;
 
-
 namespace Basis.Scripts.Networking.Recievers
 {
     [DefaultExecutionOrder(15001)]
@@ -45,12 +44,14 @@ public partial class BasisNetworkReceiver : BasisNetworkPlayer
         public double TimeInThePast;
         public bool HasAvatarInitalized;
 
-        public OneEuroFilterParallelJob oneEuroFilterJob;
-        public  float Frequency = 120f;
-        public  float MinCutoff = .05f;
+        public BasicOneEuroFilterParallelJob oneEuroFilterJob;
+        public  float MinCutoff = 0.001f;
         public  float Beta = 5f;
         public float DerivativeCutoff = 1.0f;
+
         public bool updateFilters;
+        public bool enableEuroFilter = true;
+
         /// <summary>
         /// Perform computations to interpolate and update avatar state.
         /// </summary>
@@ -112,11 +113,14 @@ public void Compute(double TimeAsDouble)
                     // Muscle interpolation job
                     musclesJob.Time = interpolationTime;
                     musclesHandle = musclesJob.Schedule(LocalAvatarSyncMessage.StoredBones, 64, AvatarHandle);
+
                     if(updateFilters)
                     {
                         ForceUpdateFilters();
                     }
-                    EuroFilterHandle = oneEuroFilterJob.Schedule(LocalAvatarSyncMessage.StoredBones,64, musclesHandle);
+
+                    oneEuroFilterJob.DeltaTime = interpolationTime;
+                    EuroFilterHandle = oneEuroFilterJob.Schedule(LocalAvatarSyncMessage.StoredBones,64,musclesHandle);
                 }
             }
         }
@@ -130,10 +134,11 @@ public void Apply(double TimeAsDouble, float DeltaTime)
                     if (HasAvatarInitalized)
                     {
                         OutputRotation = math.slerp(First.rotation, Last.rotation, interpolationTime);
+
                         // Complete the jobs and apply the results
                         EuroFilterHandle.Complete();
 
-                        ApplyPoseData(Player.BasisAvatar.Animator, OuputVectors[1], OuputVectors[0], OutputRotation, EuroValuesOutput);
+                        ApplyPoseData(Player.BasisAvatar.Animator, OuputVectors[1], OuputVectors[0], OutputRotation, enableEuroFilter ? EuroValuesOutput : musclesPreEuro);
                         PoseHandler.SetHumanPose(ref HumanPose);
 
                         RemotePlayer.RemoteBoneDriver.SimulateAndApply(DeltaTime);
@@ -217,8 +222,6 @@ public void ApplyPoseData(Animator animator, float3 Scale, float3 Position, quat
             HumanPose.bodyPosition = ScaledPosition;
             HumanPose.bodyRotation = Rotation;
 
-
-
             // Copy from job to MuscleFinalStageOutput
             Muscles.CopyTo(MuscleFinalStageOutput);
             // First, copy the first 14 elements directly
@@ -272,8 +275,8 @@ public void ReceiveAvatarChangeRequest(ServerAvatarChangeMessage ServerAvatarCha
 
             RemotePlayer.CreateAvatar(ServerAvatarChangeMessage.clientAvatarChangeMessage.loadMode, BasisLoadableBundle);
         }
-        private NativeArray<ThreadedLowPassFilter> positionFilters;
-        private NativeArray<ThreadedLowPassFilter> derivativeFilters;
+        private NativeArray<float2> positionFilters;
+        private NativeArray<float2> derivativeFilters;
         public override void Initialize()
         {
             if (!Ready)
@@ -285,15 +288,16 @@ public override void Initialize()
                 targetMuscles = new NativeArray<float>(LocalAvatarSyncMessage.StoredBones, Allocator.Persistent);
                 EuroValuesOutput = new NativeArray<float>(LocalAvatarSyncMessage.StoredBones, Allocator.Persistent);
 
-                positionFilters = new NativeArray<ThreadedLowPassFilter>(LocalAvatarSyncMessage.StoredBones, Allocator.Persistent);
-                derivativeFilters = new NativeArray<ThreadedLowPassFilter>(LocalAvatarSyncMessage.StoredBones, Allocator.Persistent);
+                positionFilters = new NativeArray<float2>(LocalAvatarSyncMessage.StoredBones, Allocator.Persistent);
+                derivativeFilters = new NativeArray<float2>(LocalAvatarSyncMessage.StoredBones, Allocator.Persistent);
 
                 musclesJob = new UpdateAvatarMusclesJob();
                 AvatarJob = new UpdateAvatarJob();
                 musclesJob.Outputmuscles = musclesPreEuro;
                 musclesJob.targetMuscles = targetMuscles;
                 AvatarJob.OutputVector = OuputVectors;
                 AvatarJob.TargetVector = TargetVectors;
+
                 ForceUpdateFilters();
 
                 RemotePlayer = (BasisRemotePlayer)Player;
@@ -311,20 +315,15 @@ public void ForceUpdateFilters()
         {
             for (int i = 0; i < LocalAvatarSyncMessage.StoredBones; i++)
             {
-                // Initialize filters for each data point
-                ThreadedLowPassFilter positionFilter = new ThreadedLowPassFilter();
-                positionFilter.Initialize(Alpha(MinCutoff));
-                positionFilters[i] = positionFilter;
-
-                ThreadedLowPassFilter derivativeFilter = new ThreadedLowPassFilter();
-                derivativeFilter.Initialize(Alpha(DerivativeCutoff));
-                derivativeFilters[i] = derivativeFilter;
+                positionFilters[i] = new float2(0,0);
+                derivativeFilters[i] = new float2(0,0);
             }
-            oneEuroFilterJob = new OneEuroFilterParallelJob
+
+            oneEuroFilterJob = new BasicOneEuroFilterParallelJob
             {
                 InputValues = musclesPreEuro,
                 OutputValues = EuroValuesOutput,
-                Frequency = Frequency,
+                DeltaTime = interpolationTime,
                 MinCutoff = MinCutoff,
                 Beta = Beta,
                 DerivativeCutoff = DerivativeCutoff,
@@ -334,7 +333,7 @@ public void ForceUpdateFilters()
         }
         private float Alpha(float cutoff)
         {
-            float te = 1.0f / Frequency;
+            float te = 1.0f / (1.0f / interpolationTime);
             float tau = 1.0f / (2.0f * Mathf.PI * cutoff);
             return 1.0f / (1.0f + tau / te);
         }

Basis/Packages/com.basis.framework/Networking/Utils/BasicOneEuroFilterParallelJob.cs

@@ -0,0 +1,66 @@
+/*
+ * BasicOneEuroFilterParallelJob.cs
+ * Author: Dario Mazzanti ([email protected]), 2016
+ *
+ * This Unity C# utility is based on the C++ implementation of the OneEuroFilter algorithm by Nicolas Roussel (http://www.lifl.fr/~casiez/1euro/OneEuroFilter.cc)
+ * More info on the 1€ filter by Géry Casiez at http://www.lifl.fr/~casiez/1euro/
+ *
+ */
+
+using Unity.Burst;
+using Unity.Collections;
+using Unity.Jobs;
+using Unity.Mathematics;
+using UnityEngine;
+
+// A job struct for processing OneEuroFilter in parallel
+[BurstCompile]
+public struct BasicOneEuroFilterParallelJob : IJobParallelFor
+{
+    [ReadOnly]
+    public NativeArray<float> InputValues;
+    public NativeArray<float> OutputValues;
+
+    public float MinCutoff;
+    public float Beta;
+    public float DerivativeCutoff;
+    public float DeltaTime;
+
+    public NativeArray<float2> PositionFilters;
+    public NativeArray<float2> DerivativeFilters;
+
+    public void Execute(int index)
+    {
+	    float frequency = 1.0f / DeltaTime;
+
+        // Estimate variation
+        float inputValue = InputValues[index];
+        float dValue = (inputValue - PositionFilters[index].x) * frequency;
+        float edValue = FilterDerivativeWithAlpha(dValue, Alpha(DerivativeCutoff, frequency), index);
+        DerivativeFilters[index] = new float2(dValue, edValue);
+
+        // Update cutoff frequency
+        float cutoff = MinCutoff + Beta * Mathf.Abs(edValue);
+
+        // Filter input value
+        OutputValues[index] = FilterPositionWithAlpha(inputValue, Alpha(cutoff, frequency), index);
+        PositionFilters[index] = new float2(inputValue, OutputValues[index]);
+    }
+
+    private float Alpha(float cutoff, float frequency)
+    {
+        float te = 1.0f / frequency;
+        float tau = 1.0f / (2.0f * Mathf.PI * cutoff);
+        return 1.0f / (1.0f + tau / te);
+    }
+
+    private float FilterPositionWithAlpha(float inputValue, float alpha, int index)
+    {
+	    return alpha * inputValue + (1.0f - alpha) * PositionFilters[index].y;
+    }
+
+    private float FilterDerivativeWithAlpha(float dValue, float alpha, int index)
+    {
+	    return alpha * dValue + (1.0f - alpha) * DerivativeFilters[index].y;
+    }
+}