@wemap/providers 4.0.0 → 4.0.3

package/src/smoothers/AttitudeSmoother.js

@@ -1,26 +1,35 @@
 /* eslint-disable max-statements */
 import { Attitude } from '@wemap/geo';
-import { Quaternion } from '@wemap/maths';
+import { deg2rad, diffAngle, Quaternion } from '@wemap/maths';
 
 class AttitudeSmoother {
 
-    // Generated attitude by second
-    static DEFAULT_FREQUENCY = 60;
+    /** @type {number} in radians/s */
+    static ROTATION_SPEED_JUMP_THRESHOLD = deg2rad(90);
 
-    // In radians/s
-    static MAX_ROTATE_SPEED = 0.5;
+    /** @type {number} in radians/s */
+    static ROTATION_SPEED_CONVERGENCE = deg2rad(10);
 
-    /** @type {Attitude[]} */
-    queue;
+    /** @type {number} in radians/s */
+    static HIGH_JUMP_THRESHOLD = deg2rad(20);
+
+    /** @type {number} in radians/s */
+    static ROTATION_SPEED_HIGH_JUMP_CONVERGENCE = deg2rad(100);
+
+    /** @type {number} in radians */
+    static PITCH_UNCERTAINITY_HEADING_THRESHOLD = deg2rad(5);
+
+    /** @type {Function} */
+    _callback;
+
+    /** @type {object?} */
+    _smoothing = null;
 
     /**
      * @param {Function} callback
-     * @param {Number} frequency in Hz
      */
-    constructor(callback, frequency = this.constructor.DEFAULT_FREQUENCY) {
-        this.callback = callback;
-        this.queue = [];
-        this.frequency = frequency;
+    constructor(callback) {
+        this._callback = callback;
     }
 
     /**
@@ -36,65 +45,91 @@ class AttitudeSmoother {
             throw new Error('newAttitude does not have time property');
         }
 
-        let previousAttitude;
-        if (this.queue.length !== 0) {
-            previousAttitude = this.queue[0];
-            this.queue = [];
-        } else {
-            previousAttitude = this.previousAttitude;
+        const { _previousAttitude: previousAttitude } = this;
+        this._previousAttitude = newAttitude;
+
+        if (!previousAttitude) {
+            this._callback(newAttitude);
+            return;
         }
 
-        if (previousAttitude) {
-            const previousQuat = previousAttitude.quaternion;
-            const nextQuat = newAttitude.quaternion;
-            const quatAngle = Quaternion.distance(previousQuat, nextQuat);
-
-            /** max rotation for the next value (radians) */
-            const maxRotAtEpoch = this.constructor.MAX_ROTATE_SPEED / this.frequency;
-
-            if (quatAngle > maxRotAtEpoch) {
-                for (let i = 1; i <= Math.floor(quatAngle / maxRotAtEpoch); i++) {
-
-                    // coef is contained in [0; 1[
-                    const coef = i * maxRotAtEpoch / quatAngle;
-
-                    const smoothedAttitude = new Attitude(
-                        Quaternion.slerp(previousQuat, nextQuat, coef),
-                        newAttitude.time + i / this.frequency,
-                        Math.max(
-                            previousAttitude.accuracy + (newAttitude.accuracy - previousAttitude.accuracy) * coef,
-                            0
-                        )
-                    );
-                    this.queue.push(smoothedAttitude);
+        /*
+         * Comparison between two successive Attitude from the "feed" function
+         */
+        if (AttitudeSmoother.isJump(previousAttitude, newAttitude)) {
+
+            const fromAttitude = this._smoothing === null
+                ? previousAttitude
+                : this._smoothing.interpAttitude(previousAttitude);
+
+            const fromHeading = fromAttitude.heading;
+            const toHeading = newAttitude.heading;
+            const diffAngleHeading = diffAngle(toHeading, fromHeading);
+
+            const isHighJump = Math.abs(diffAngleHeading) < AttitudeSmoother.HIGH_JUMP_THRESHOLD;
+            const rotationSpeedConvergence = isHighJump
+                ? AttitudeSmoother.ROTATION_SPEED_CONVERGENCE
+                : AttitudeSmoother.ROTATION_SPEED_HIGH_JUMP_CONVERGENCE;
+
+            const fromTime = fromAttitude.time;
+            const timeToConsume = Math.abs(diffAngleHeading) / rotationSpeedConvergence;
+            const multiplier = diffAngleHeading < 0 ? -1 : 1;
+
+            this._smoothing = {
+                toTime: fromTime + timeToConsume,
+
+                /**
+                 * @param {Attitude} attitude
+                 */
+                interpAttitude: attitude => {
+                    const angle = rotationSpeedConvergence * (attitude.time - fromTime);
+                    const interpHeading = fromHeading + angle * multiplier;
+                    const offsetQuat = Quaternion.fromAxisAngle([0, 0, 1], toHeading - interpHeading);
+                    const interpQuat = Quaternion.multiply(offsetQuat, attitude.quaternion);
+                    return new Attitude(interpQuat, attitude.time, attitude.accuracy);
                 }
+            };
+        }
 
+        if (this._smoothing !== null) {
+            if (newAttitude.time >= this._smoothing.toTime) {
+                // This means that is the last epoch for smoothing
+                this._smoothing = null;
+            } else {
+                const interpAttitude = this._smoothing.interpAttitude(newAttitude);
+                this._callback(interpAttitude);
+                return;
             }
         }
 
-        this.queue.push(newAttitude.clone());
-
-        this.previousAttitude = newAttitude;
-
-        if (!this.timeoutNotify) {
-            this._notifyNext();
-        }
+        this._callback(newAttitude);
     }
 
-    _notifyNext = () => {
-        this.callback(this.queue.shift());
-        if (this.queue.length !== 0) {
-            this.timeoutNotify = setTimeout(this._notifyNext, 1e3 / this.frequency);
-        } else {
-            delete this.timeoutNotify;
+    /**
+     * @param {Attitude} previousAttitude
+     * @param {Attitude} newAttitude
+     * @returns {boolean}
+     */
+    static isJump(previousAttitude, newAttitude) {
+        const fromHeading = previousAttitude.heading;
+        const toHeading = newAttitude.heading;
+        const diffAngleHeading = diffAngle(toHeading, fromHeading);
+        const diffTime = newAttitude.time - previousAttitude.time;
+
+        /**
+         * Heading is calculated from two different formulas in function of the pitch angle.
+         * Do not consider a jump if attitude is close to the change of the methods
+         * @see MathsRotations#getHeadingFromQuaternion()
+         */
+        const [qw, qx, qy, qz] = newAttitude.quaternion;
+        const distToPitchThreshold = Math.abs(Math.asin(2 * (qw * qx + qy * qz)) - Math.PI / 4);
+        if (distToPitchThreshold < AttitudeSmoother.PITCH_UNCERTAINITY_HEADING_THRESHOLD) {
+            return false;
         }
-    }
 
-    clear() {
-        clearTimeout(this.timeoutNotify);
-        delete this.timeoutNotify;
-        this.queue = [];
+        return Math.abs(diffAngleHeading) > AttitudeSmoother.ROTATION_SPEED_JUMP_THRESHOLD * diffTime;
     }
+
 }
 
 export default AttitudeSmoother;

package/src/providers/MetaProvider.js

@@ -1,44 +0,0 @@
-import EventType from '../events/EventType.js';
-import Provider from './Provider.js';
-import ProviderEvent from '../events/ProviderEvent.js';
-
-class MetaProvider extends Provider {
-
-    _lastEvent = null;
-
-    /**
-     * Singleton pattern using reflection.
-     * @returns {MetaProvider}
-     */
-    static get instance() {
-        if (!this._instance) {
-            this._instance = Reflect.construct(this, []);
-        }
-        return this._instance;
-    }
-
-    /**
-     * @override
-     * @param {ProviderEvent} event
-     */
-    notify(event) {
-        super.notify(event);
-        this._lastEvent = event;
-    }
-
-    get lastEvent() {
-        return this._lastEvent;
-    }
-
-    /**
-     * Input data from external interface
-     * @param {ProviderEvent} event
-     * @param {EventType} eventType
-     */
-    // eslint-disable-next-line no-unused-vars
-    feed(event, eventType) {
-        throw new Error('Not implemented');
-    }
-}
-
-export default MetaProvider;

package/src/providers/attitude/absolute/AbsoluteAttitudeFromRelAttProvider.js

@@ -1,132 +0,0 @@
-import { Quaternion } from '@wemap/maths';
-import { Attitude } from '@wemap/geo';
-
-import Provider from '../../Provider.js';
-import EventType from '../../../events/EventType.js';
-import {
-    RelativeAttitude, AbsoluteAttitude, AbsoluteAttitudeFromBrowser, AbsoluteAttitudeFused
-} from '../../../Providers.js';
-
-/**
- * Absolute attitude provider gives the device attitude in East-North-Up (ENU) frame
- */
-class AbsoluteAttitudeFromRelAttProvider extends Provider {
-
-    accuracy = 0;
-
-    /**
-     * @override
-     */
-    static get pname() {
-        return 'AbsoluteAttitudeFromRelAtt';
-    }
-
-    /**
-     * @override
-     */
-    static get eventsType() {
-        return [EventType.AbsoluteAttitude];
-    }
-
-    /**
-     * @override
-     */
-    get _availability() {
-        return RelativeAttitude.availability;
-    }
-
-    /**
-     * @override
-     */
-    start() {
-
-        this.relativeAttitudeProviderId = RelativeAttitude.addEventListener(
-            events => this.onRelativeAttitudeEvent(events[0]),
-            error => this.notifyError(error)
-        );
-
-
-        this.onAbsoluteAttitudeEvent(AbsoluteAttitude.lastEvent);
-        this.absoluteAttitudeProviderId = AbsoluteAttitude.addEventListener(
-            events => this.onAbsoluteAttitudeEvent(events[0]),
-            error => this.notifyError(error),
-            false
-        );
-    }
-
-    /**
-     * @override
-     */
-    stop() {
-        RelativeAttitude.removeEventListener(this.relativeAttitudeProviderId);
-        AbsoluteAttitude.removeEventListener(this.absoluteAttitudeProviderId);
-    }
-
-
-    onRelativeAttitudeEvent(relativeAttitudeEvent) {
-
-        /**
-         * Calculate relative accuracy
-         */
-        if (this.relativeAttitudeEvent) {
-            const {
-                accuracy, time
-            } = relativeAttitudeEvent.data;
-            const diffTime = time - this.relativeAttitudeEvent.data.time;
-            this.accuracy += diffTime * accuracy;
-        }
-
-        this.relativeAttitudeEvent = relativeAttitudeEvent;
-        this.compute();
-    }
-
-
-    onAbsoluteAttitudeEvent = absoluteAttitudeEvent => {
-
-        if (!absoluteAttitudeEvent) {
-            return;
-        }
-
-        /**
-         * Use absolute attitude events only when they are not from this provider
-         */
-        if (absoluteAttitudeEvent.providersStack.includes(this.pname)
-            || absoluteAttitudeEvent.providersStack.includes(AbsoluteAttitudeFromBrowser.pname)
-            || absoluteAttitudeEvent.providersStack.includes(AbsoluteAttitudeFused.pname)) {
-            return;
-        }
-        this.absoluteAttitudeEvent = absoluteAttitudeEvent;
-        this.accuracy = 0;
-
-        // preprocess zOffset for "compute" function
-        const currentHeading = this.relativeAttitudeEvent ? this.relativeAttitudeEvent.data.heading : 0;
-        this.zOffset = Quaternion.fromAxisAngle([0, 0, 1], -absoluteAttitudeEvent.data.heading + currentHeading);
-    };
-
-
-    compute() {
-        if (!this.absoluteAttitudeEvent || !this.relativeAttitudeEvent) {
-            return;
-        }
-
-        const {
-            quaternion, time
-        } = this.relativeAttitudeEvent.data;
-
-        const absoluteAttitudeAccuracy = this.absoluteAttitudeEvent.data.accuracy;
-
-        const absoluteQuat = Quaternion.multiply(this.zOffset, quaternion);
-        const newAccuracy = Math.min(absoluteAttitudeAccuracy + this.accuracy, Math.PI);
-        const attitude = new Attitude(absoluteQuat, time, newAccuracy, this.pname);
-
-        this.notify(this.createEvent(
-            EventType.AbsoluteAttitude,
-            attitude,
-            [this.relativeAttitudeEvent, this.absoluteAttitudeEvent]
-        ));
-
-    }
-
-}
-
-export default AbsoluteAttitudeFromRelAttProvider;

package/src/providers/attitude/absolute/AbsoluteAttitudeFusedProvider.js

@@ -1,166 +0,0 @@
-import { Attitude, Edge } from '@wemap/geo';
-import {
-    Quaternion, diffAngle
-} from '@wemap/maths';
-
-import Provider from '../../Provider.js';
-import EventType from '../../../events/EventType.js';
-import { RelativeAttitude } from '../../../Providers.js';
-import ProviderState from '../../ProviderState.js';
-
-/**
- * Absolute attitude provider gives the device attitude in East-North-Up (ENU) frame
- */
-class AbsoluteAttitudeFusedProvider extends Provider {
-
-    accuracy = 0;
-    isInitialized = false;
-
-    /** @type {number[]} quaternion */
-    zOffset = null;
-
-    /**
-     * @override
-     */
-    static get pname() {
-        return 'AbsoluteAttitudeFused';
-    }
-
-    /**
-     * @override
-     */
-    static get eventsType() {
-        return [EventType.AbsoluteAttitude];
-    }
-
-    /**
-     * @override
-     */
-    get _availability() {
-        // TODO Enhance
-        return RelativeAttitude.availability;
-    }
-
-    /**
-     * @override
-     */
-    start() {
-
-        this.relativeAttitudeProviderId = RelativeAttitude.addEventListener(
-            events => this.onRelativeAttitudeEvent(events[0]),
-            error => this.notifyError(error)
-        );
-
-
-        // this.onAbsoluteAttitudeEvent(AbsoluteAttitude.lastEvent);
-        // this.absoluteAttitudeProviderId = AbsoluteAttitude.addEventListener(
-        //     events => this.onAbsoluteAttitudeEvent(events[0]),
-        //     error => this.notifyError(error),
-        //     false
-        // );
-    }
-
-    /**
-     * @override
-     */
-    stop() {
-        RelativeAttitude.removeEventListener(this.relativeAttitudeProviderId);
-        // AbsoluteAttitude.removeEventListener(this.absoluteAttitudeProviderId);
-    }
-
-    mapMatching(projection) {
-
-        if (this.state !== ProviderState.STARTED) {
-            return;
-        }
-
-        const {
-            nearestElement, origin
-        } = projection;
-        if (!(nearestElement instanceof Edge)) {
-            return;
-        }
-
-        let matchingDirection;
-        const matchingDirectionAngle1 = diffAngle(nearestElement.bearing, origin.bearing);
-        const matchingDirectionAngle2 = diffAngle(nearestElement.bearing + Math.PI, origin.bearing);
-
-        if (Math.abs(matchingDirectionAngle1) < Math.abs(matchingDirectionAngle2)) {
-            matchingDirection = nearestElement.bearing;
-        } else {
-            matchingDirection = (nearestElement.bearing + Math.PI) % (2 * Math.PI);
-        }
-
-        this.accuracy = 0;
-
-        // preprocess zOffset for "compute" function
-        const currentHeading = this.relativeAttitudeEvent ? this.relativeAttitudeEvent.data.heading : 0;
-        this.zOffset = Quaternion.fromAxisAngle([0, 0, 1], -matchingDirection + currentHeading);
-
-        this.compute();
-    }
-
-    onRelativeAttitudeEvent(relativeAttitudeEvent) {
-
-        /**
-         * Calculate relative accuracy
-         */
-        if (this.relativeAttitudeEvent) {
-            const {
-                accuracy, time
-            } = relativeAttitudeEvent.data;
-            const diffTime = time - this.relativeAttitudeEvent.data.time;
-            this.accuracy += diffTime * accuracy;
-        }
-
-        this.relativeAttitudeEvent = relativeAttitudeEvent;
-        this.compute();
-    }
-
-
-    // onAbsoluteAttitudeEvent = absoluteAttitudeEvent => {
-
-    //     if (!absoluteAttitudeEvent) {
-    //         return;
-    //     }
-
-    //     /**
-    //      * Use absolute attitude events only when they are not from this provider
-    //      */
-    //     if (absoluteAttitudeEvent.providersStack.includes(this.pname)
-    //         || absoluteAttitudeEvent.providersStack.includes(AbsoluteAttitudeFromBrowser.pname)) {
-    //         return;
-    //     }
-    //     this.absoluteAttitudeEvent = absoluteAttitudeEvent;
-    //     this.accuracy = 0;
-
-    //     // preprocess zOffset for "compute" function
-    //     const currentHeading = this.relativeAttitudeEvent ? this.relativeAttitudeEvent.data.heading : 0;
-    //     this.zOffset = Quaternion.fromAxisAngle([0, 0, 1], -absoluteAttitudeEvent.data.heading + currentHeading);
-    // };
-
-
-    compute() {
-        if (!this.zOffset || !this.relativeAttitudeEvent) {
-            return;
-        }
-
-        const {
-            quaternion, time
-        } = this.relativeAttitudeEvent.data;
-
-        const absoluteQuat = Quaternion.multiply(this.zOffset, quaternion);
-        const newAccuracy = Math.min(this.accuracy, Math.PI);
-        const attitude = new Attitude(absoluteQuat, time, newAccuracy, this.pname);
-
-        this.notify(this.createEvent(
-            EventType.AbsoluteAttitude,
-            attitude,
-            [this.relativeAttitudeEvent]
-        ));
-
-    }
-
-}
-
-export default AbsoluteAttitudeFusedProvider;