diff --git a/Basis/Packages/com.basis.framework/Networking/Recievers/BasisNetworkReceiver.cs b/Basis/Packages/com.basis.framework/Networking/Recievers/BasisNetworkReceiver.cs index e7c14a7a9..52e59eb3c 100644 --- a/Basis/Packages/com.basis.framework/Networking/Recievers/BasisNetworkReceiver.cs +++ b/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; + /// /// Perform computations to interpolate and update avatar state. /// @@ -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 positionFilters; - private NativeArray derivativeFilters; + private NativeArray positionFilters; + private NativeArray derivativeFilters; public override void Initialize() { if (!Ready) @@ -285,8 +288,8 @@ public override void Initialize() targetMuscles = new NativeArray(LocalAvatarSyncMessage.StoredBones, Allocator.Persistent); EuroValuesOutput = new NativeArray(LocalAvatarSyncMessage.StoredBones, Allocator.Persistent); - positionFilters = new NativeArray(LocalAvatarSyncMessage.StoredBones, Allocator.Persistent); - derivativeFilters = new NativeArray(LocalAvatarSyncMessage.StoredBones, Allocator.Persistent); + positionFilters = new NativeArray(LocalAvatarSyncMessage.StoredBones, Allocator.Persistent); + derivativeFilters = new NativeArray(LocalAvatarSyncMessage.StoredBones, Allocator.Persistent); musclesJob = new UpdateAvatarMusclesJob(); AvatarJob = new UpdateAvatarJob(); @@ -294,6 +297,7 @@ public override void Initialize() 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); } diff --git a/Basis/Packages/com.basis.framework/Networking/Utils/BasicOneEuroFilterParallelJob.cs b/Basis/Packages/com.basis.framework/Networking/Utils/BasicOneEuroFilterParallelJob.cs new file mode 100644 index 000000000..5df7696e0 --- /dev/null +++ b/Basis/Packages/com.basis.framework/Networking/Utils/BasicOneEuroFilterParallelJob.cs @@ -0,0 +1,66 @@ +/* + * BasicOneEuroFilterParallelJob.cs + * Author: Dario Mazzanti (dario.mazzanti@iit.it), 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 InputValues; + public NativeArray OutputValues; + + public float MinCutoff; + public float Beta; + public float DerivativeCutoff; + public float DeltaTime; + + public NativeArray PositionFilters; + public NativeArray 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; + } +} diff --git a/Basis/Packages/com.basis.framework/Networking/Utils/OneEuroFilter.cs.meta b/Basis/Packages/com.basis.framework/Networking/Utils/BasicOneEuroFilterParallelJob.cs.meta similarity index 100% rename from Basis/Packages/com.basis.framework/Networking/Utils/OneEuroFilter.cs.meta rename to Basis/Packages/com.basis.framework/Networking/Utils/BasicOneEuroFilterParallelJob.cs.meta diff --git a/Basis/Packages/com.basis.framework/Networking/Utils/OneEuroFilter.cs b/Basis/Packages/com.basis.framework/Networking/Utils/OneEuroFilter.cs deleted file mode 100644 index b79859ed8..000000000 --- a/Basis/Packages/com.basis.framework/Networking/Utils/OneEuroFilter.cs +++ /dev/null @@ -1,245 +0,0 @@ -/* - * OneEuroFilter.cs - * Author: Dario Mazzanti (dario.mazzanti@iit.it), 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; - -class LowPassFilter -{ - float y, a, s; - bool initialized; - - public void setAlpha(float _alpha) - { - if (_alpha<=0.0f || _alpha>1.0f) - { - Debug.LogError("alpha should be in (0.0., 1.0]"); - return; - } - a = _alpha; - } - - public LowPassFilter(float _alpha, float _initval=0.0f) - { - y = s = _initval; - setAlpha(_alpha); - initialized = false; - } - - public float Filter(float _value) - { - float result; - if (initialized) - result = a*_value + (1.0f-a)*s; - else - { - result = _value; - initialized = true; - } - y = _value; - s = result; - return result; - } - - public float filterWithAlpha(float _value, float _alpha) - { - setAlpha(_alpha); - return Filter(_value); - } - - public bool hasLastRawValue() - { - return initialized; - } - - public float lastRawValue() - { - return y; - } - -}; - -// ----------------------------------------------------------------- - -public class OneEuroFilter -{ - float freq; - float mincutoff; - float beta; - float dcutoff; - LowPassFilter x; - LowPassFilter dx; - float lasttime; - - // currValue contains the latest value which have been succesfully filtered - // prevValue contains the previous filtered value - public float currValue {get; protected set;} - public float prevValue {get; protected set;} - - float alpha(float _cutoff) - { - float te = 1.0f/freq; - float tau = 1.0f/(2.0f*Mathf.PI*_cutoff); - return 1.0f/(1.0f + tau/te); - } - - void setFrequency(float _f) - { - if (_f<=0.0f) - { - Debug.LogError("freq should be > 0"); - return; - } - freq = _f; - } - - void setMinCutoff(float _mc) - { - if (_mc<=0.0f) - { - Debug.LogError("mincutoff should be > 0"); - return; - } - mincutoff = _mc; - } - - void setBeta(float _b) - { - beta = _b; - } - - void setDerivateCutoff(float _dc) - { - if (_dc<=0.0f) - { - Debug.LogError("dcutoff should be > 0"); - return; - } - dcutoff = _dc; - } - - public OneEuroFilter(float _freq, float _mincutoff=1.0f, float _beta=0.0f, float _dcutoff=1.0f) - { - setFrequency(_freq); - setMinCutoff(_mincutoff); - setBeta(_beta); - setDerivateCutoff(_dcutoff); - x = new LowPassFilter(alpha(mincutoff)); - dx = new LowPassFilter(alpha(dcutoff)); - lasttime = -1.0f; - - currValue = 0.0f; - prevValue = currValue; - } - - public void UpdateParams(float _freq, float _mincutoff = 1.0f, float _beta = 0.0f, float _dcutoff = 1.0f) - { - setFrequency(_freq); - setMinCutoff(_mincutoff); - setBeta(_beta); - setDerivateCutoff(_dcutoff); - x.setAlpha(alpha(mincutoff)); - dx.setAlpha(alpha(dcutoff)); - } - - public float Filter(float value, float timestamp = -1.0f) - { - prevValue = currValue; - - // update the sampling frequency based on timestamps - if (lasttime != -1.0f && timestamp != -1.0f) - { - freq = 1.0f / (timestamp - lasttime); - } - lasttime = timestamp; - // estimate the current variation per second - float dvalue = x.hasLastRawValue() ? (value - x.lastRawValue())*freq : 0.0f; // FIXME: 0.0 or value? - float edvalue = dx.filterWithAlpha(dvalue, alpha(dcutoff)); - // use it to update the cutoff frequency - float cutoff = mincutoff + beta*Mathf.Abs(edvalue); - // filter the given value - currValue = x.filterWithAlpha(value, alpha(cutoff)); - - return currValue; - } -} -// A struct for the LowPassFilter -[BurstCompile] -public struct ThreadedLowPassFilter -{ - public float Alpha; - public float LastValue; - public bool Initialized; - - public void Initialize(float alpha, float initialValue = 0.0f) - { - Alpha = alpha; - LastValue = initialValue; - Initialized = false; - } - - public float Filter(float value) - { - if (!Initialized) - { - Initialized = true; - LastValue = value; - } - else - { - LastValue = Alpha * value + (1.0f - Alpha) * LastValue; - } - return LastValue; - } - - public float FilterWithAlpha(float value, float alpha) - { - Alpha = alpha; - return Filter(value); - } -} - -// A job struct for processing OneEuroFilter in parallel -[BurstCompile] -public struct OneEuroFilterParallelJob : IJobParallelFor -{ - [ReadOnly] public NativeArray InputValues; - [WriteOnly] public NativeArray OutputValues; - - public float Frequency; - public float MinCutoff; - public float Beta; - public float DerivativeCutoff; - public NativeArray PositionFilters; - public NativeArray DerivativeFilters; - - public void Execute(int index) - { - // Estimate variation - float inputValue = InputValues[index]; - float dValue = PositionFilters[index].Initialized ? (inputValue - PositionFilters[index].LastValue) * Frequency: 0.0f; - float edValue = DerivativeFilters[index].FilterWithAlpha(dValue, Alpha(DerivativeCutoff, Frequency)); - - // Update cutoff frequency - float cutoff = MinCutoff + Beta * Mathf.Abs(edValue); - - // Filter input value - OutputValues[index] = PositionFilters[index].FilterWithAlpha(inputValue, Alpha(cutoff, Frequency)); - } - - private float Alpha(float cutoff, float freq) - { - float te = 1.0f / freq; - float tau = 1.0f / (2.0f * Mathf.PI * cutoff); - return 1.0f / (1.0f + tau / te); - } -}