OneEuroFilter feature

package.json

@@ -1,6 +1,6 @@
 {
   "name": "@webarkit/arnft-threejs",
-  "version": "0.4.0",
+  "version": "0.4.5",
   "description": "The ARnft rendering engine, based on Three.js",
   "main": "dist/ARnftThreejs.js",
   "types": "types/index.d.ts",
@@ -32,21 +32,21 @@
   },
   "homepage": "https://github.com/webarkit/ARnft-threejs#readme",
   "devDependencies": {
-    "@babel/core": "^7.15.8",
-    "@babel/plugin-transform-runtime": "^7.15.8",
-    "@babel/preset-env": "^7.15.8",
+    "@babel/core": "^7.16.0",
+    "@babel/plugin-transform-runtime": "^7.16.4",
+    "@babel/preset-env": "^7.16.4",
     "@types/node": "^15.6.0",
-    "@types/three": "^0.133.0",
+    "@types/three": "^0.134.0",
     "babel-loader": "^8.2.2",
     "prettier": "^2.4.1",
     "ts-loader": "^9.2.6",
-    "typedoc": "^0.22.5",
+    "typedoc": "^0.22.9",
     "typescript": "^4.2.4",
-    "webpack": "^5.58.1",
-    "webpack-cli": "^4.9.0"
+    "webpack": "^5.64.1",
+    "webpack-cli": "^4.9.1"
   },
   "dependencies": {
-    "@babel/runtime": "^7.15.4",
-    "three": "^0.133.0"
+    "@babel/runtime": "^7.16.3",
+    "three": "^0.134.0"
   }
 }

src/SceneRendererTJS.ts

@@ -80,7 +80,7 @@ export default class SceneRendererTJS {
         } else {
             this.camera = new THREE.Camera();
         }
-        this.version = "0.4.0";
+        this.version = "0.4.5";
         console.log("ARnftThreejs version: ", this.version);
     }
 

src/filters/ARnftFilter.ts

@@ -0,0 +1,100 @@
+import { OneEuroFilterVector3 } from "./OneEuroFilter";
+import { DelayableSignalFilter } from "./DelayableSignalFilter";
+import { Euler, Matrix4, Quaternion, Vector3 } from "three";
+
+export class ARnftFilter {
+    private delayExitCheck: DelayableSignalFilter;
+
+    private delayEnterCheck: DelayableSignalFilter;
+
+    private _hasFound: boolean = false;
+
+    // private _interpolationFactor: number = 15;
+
+    private _lastTranslation: Vector3;
+
+    private _frameDrops: number = 0;
+
+    private _deltaAccuracy: number = 10;
+
+    private _positionFilter: OneEuroFilterVector3;
+
+    private _rotationFilter: OneEuroFilterVector3;
+
+    public filterFrequency: number = 30.0;
+    public filterMinCutoff: number = 1.0;
+    public filterBeta: number = 0.0;
+    public filterDcutoff: number = 1.0;
+
+    constructor() {
+        this.delayEnterCheck = new DelayableSignalFilter(2);
+        this.delayExitCheck = new DelayableSignalFilter(0);
+
+        this._positionFilter = new OneEuroFilterVector3(this.filterFrequency);
+        this._rotationFilter = new OneEuroFilterVector3(this.filterFrequency * 2);
+    }
+
+    public update(world: any): Vector3[] {
+        let pos: Vector3 = new Vector3();
+        let rotationVec: Vector3 = new Vector3();
+        let scale: Vector3 = new Vector3();
+        if (!world) {
+            this._hasFound = false;
+            this._frameDrops = 0;
+        } else {
+            let matrixW: Matrix4 = new Matrix4();
+            let worldMatrix: Matrix4 = matrixW.fromArray(this.getArrayMatrix(world));
+            if (!this._hasFound) {
+                this._hasFound = true;
+                let vecTrans: Vector3 = new Vector3();
+                this._lastTranslation = vecTrans.setFromMatrixPosition(worldMatrix);
+            } else {
+                let vecTrans: Vector3 = new Vector3();
+                let _currentTranslation: Vector3 = vecTrans.setFromMatrixPosition(worldMatrix);
+                if (Math.abs(_currentTranslation.distanceTo(this._lastTranslation)) > this._deltaAccuracy) {
+                    this._frameDrops += 1;
+                    if (this._frameDrops > 3) {
+                        this._lastTranslation = _currentTranslation;
+                    }
+                    return [pos, rotationVec, scale];
+                }
+                this._frameDrops = 0;
+                this._lastTranslation = _currentTranslation;
+            }
+            this._positionFilter.UpdateParams(
+                this.filterFrequency,
+                this.filterMinCutoff,
+                this.filterBeta,
+                this.filterDcutoff
+            );
+            this._rotationFilter.UpdateParams(
+                this.filterFrequency * 2,
+                this.filterMinCutoff,
+                this.filterBeta,
+                this.filterDcutoff
+            );
+            let matrix: Matrix4 = new Matrix4();
+
+            matrix = worldMatrix;
+
+            let rotation: Quaternion = new Quaternion();
+            let eulerRot: Euler = new Euler();
+            let position: Vector3 = new Vector3(0, 0, 0);
+
+            // or even simple decompose the worldMatrix into position, quaternion and scale with decompose
+            worldMatrix.decompose(position, rotation, scale);
+            let eRot = eulerRot.setFromQuaternion(rotation);
+            rotationVec = this._rotationFilter.Filter(eRot.toVector3());
+
+            pos = this._positionFilter.Filter(position);
+        }
+        return [pos, rotationVec, scale];
+    }
+    protected getArrayMatrix(value: any): any {
+        var array: any = [];
+        for (var key in value) {
+            array[key] = value[key]; //.toFixed(4);
+        }
+        return array;
+    }
+}

src/filters/DelayableSignalFilter.ts

@@ -0,0 +1,38 @@
+import { getTime } from "../utils/Utils";
+
+export class DelayableSignalFilter {
+    private _inDelay: boolean;
+    private _totalTime: number;
+    private _prevTime: number;
+
+    private _timeOut: number;
+
+    constructor(timeOut: number) {
+        this._timeOut = timeOut;
+        this._inDelay = false;
+    }
+
+    public Update(tick: boolean): boolean {
+        let time: number = getTime();
+
+        if (!this._inDelay) {
+            this._prevTime = time;
+            this._totalTime = 0;
+        }
+
+        this._totalTime += time - this._prevTime;
+
+        if (this._inDelay && this._totalTime > this._timeOut) {
+            this._inDelay = false;
+            return true;
+        }
+
+        if (tick) {
+            this._inDelay = true;
+            return false;
+        }
+        this._inDelay = false;
+
+        return false;
+    }
+}

src/filters/OneEuroFilter.ts

@@ -0,0 +1,188 @@
+import { Vector3 } from "three";
+
+//https://github.com/DarioMazzanti/OneEuroFilterUnity/blob/master/Assets/Scripts/OneEuroFilter.cs
+//https://github.com/DarioMazzanti/OneEuroFilterUnity/blob/master/Assets/Scripts/FilterTestVector3.cs
+//https://gist.github.com/ThorstenBux/323183bb0bc2ccb92ff23ebdf3de6408
+
+/* eslint-disable max-classes-per-file */
+class LowPassFilter {
+    y: number | null;
+
+    s: number | null;
+
+    alpha = 0;
+
+    constructor(alpha: number) {
+        this.setAlpha(alpha);
+        this.y = null;
+        this.s = null;
+    }
+
+    setAlpha(alpha: number) {
+        if (alpha <= 0 || alpha > 1.0) {
+            throw new Error();
+        }
+        this.alpha = alpha;
+    }
+
+    filter(value: number, timestamp: number, alpha: number) {
+        if (alpha) {
+            this.setAlpha(alpha);
+        }
+        let s;
+        if (!this.y) {
+            s = value;
+        } else {
+            s = this.alpha * value + (1.0 - this.alpha) * this.s!;
+        }
+        this.y = value;
+        this.s = s;
+        return s;
+    }
+
+    lastValue() {
+        return this.y;
+    }
+}
+
+export default class OneEuroFilter {
+    freq: number;
+
+    minCutOff: number;
+
+    beta: number;
+
+    dCutOff: number;
+
+    x: LowPassFilter;
+
+    dx: LowPassFilter;
+
+    lasttime: number | null;
+
+    public currValue: number;
+    public prevValue: number;
+
+    constructor(freq: number, minCutOff = 1.0, beta = 0.0, dCutOff = 1.0) {
+        if (freq <= 0 || minCutOff <= 0 || dCutOff <= 0) {
+            throw new Error();
+        }
+        this.freq = freq;
+        this.minCutOff = minCutOff;
+        this.beta = beta;
+        this.dCutOff = dCutOff;
+        this.x = new LowPassFilter(this.alpha(this.minCutOff));
+        this.dx = new LowPassFilter(this.alpha(this.dCutOff));
+        this.lasttime = null;
+
+        this.currValue = 0.0;
+        this.prevValue = this.currValue;
+    }
+
+    public alpha(cutOff: number) {
+        const te = 1.0 / this.freq;
+        const tau = 1.0 / (2 * Math.PI * cutOff);
+        return 1.0 / (1.0 + tau / te);
+    }
+
+    public UpdateParams(_freq: number, _mincutoff: number = 1.0, _beta: number = 0, _dcutoff: number = 1): void {
+        this.freq = _freq;
+        this.minCutOff = _mincutoff;
+        this.beta = _beta;
+        this.dCutOff = _dcutoff;
+        this.x.setAlpha(this.alpha(this.minCutOff));
+        this.dx.setAlpha(this.alpha(this.dCutOff));
+    }
+
+    public Filter(x: number, timestamp: number | null = null): number {
+        this.prevValue = this.currValue;
+        if (this.lasttime && timestamp) {
+            this.freq = 1.0 / (timestamp - this.lasttime);
+        }
+        this.lasttime = timestamp;
+        const prevX = this.x.lastValue();
+        const dx = !prevX ? 0.0 : (x - prevX) * this.freq;
+        const edx = this.dx.filter(dx, timestamp!, this.alpha(this.dCutOff));
+        const cutOff = this.minCutOff + this.beta * Math.abs(edx);
+        return (this.currValue = this.x.filter(x, timestamp!, this.alpha(cutOff)));
+    }
+}
+
+export class OneEuroFilterVector3 {
+    // containst the type of T
+    // the array of filters
+    private oneEuroFilters: Array<OneEuroFilter>;
+
+    private _freq: number;
+    public get freq(): number {
+        return this._freq;
+    }
+
+    private _beta: number;
+    public get beta(): number {
+        return this._beta;
+    }
+
+    private _dcutoff: number;
+    public get dcutoff(): number {
+        return this._dcutoff;
+    }
+    private _mincutoff: number;
+    public get mincutoff_1(): number {
+        return this._mincutoff;
+    }
+
+    // currValue contains the latest value which have been succesfully filtered
+    // prevValue contains the previous filtered value
+
+    private currValue: Vector3;
+    private prevValue: Vector3;
+
+    // initialization of our filter(s)
+    constructor(_freq: number, _mincutoff: number = 1, _beta: number = 0, _dcutoff: number = 1) {
+        this.currValue = new Vector3();
+        this.prevValue = new Vector3();
+
+        this._freq = _freq;
+        this._mincutoff = _mincutoff;
+        this._beta = _beta;
+        this._dcutoff = _dcutoff;
+
+        this.oneEuroFilters = [];
+        this.oneEuroFilters.push(new OneEuroFilter(_freq, _mincutoff, _beta, _dcutoff));
+        this.oneEuroFilters.push(new OneEuroFilter(_freq, _mincutoff, _beta, _dcutoff));
+        this.oneEuroFilters.push(new OneEuroFilter(_freq, _mincutoff, _beta, _dcutoff));
+    }
+
+    // updates the filter parameters
+    public UpdateParams(_freq: number, _mincutoff: number = 1.0, _beta: number = 0, _dcutoff: number = 1): void {
+        this._freq = _freq;
+        this._mincutoff = _mincutoff;
+        this._beta = _beta;
+        this._dcutoff = _dcutoff;
+
+        for (let i: number = 0; i < this.oneEuroFilters.length; i++)
+            this.oneEuroFilters[i].UpdateParams(this._freq, this._mincutoff, this._beta, this._dcutoff);
+    }
+
+    // filters the provided _value and returns the result.
+    // Note: a timestamp can also be provided - will override filter frequency.
+    public Filter(_value: Vector3, timestamp: number = -1.0): Vector3 {
+        this.prevValue = this.currValue;
+
+        //let output: number[] = Vector3.ZeroReadOnly.asArray(); // Babylon code...
+        let out: Vector3 = new Vector3();
+        let output: number[] = out.toArray();
+
+        //let input: number[] = _value.asArray(); // Babylon code
+        let input: number[] = _value.toArray();
+
+        this.oneEuroFilters.forEach((filters, idx) => {
+            output[idx] = filters.Filter(input[idx], timestamp);
+        });
+
+        let arr: Vector3 = new Vector3();
+
+        return (this.currValue = arr.fromArray(output));
+    }
+}