@wemap/geo 1.0.5 → 2.7.1

package/src/coordinates/WGS84.js

@@ -1,8 +1,9 @@
 import {
-    Utils, Vector3, Quaternion
+    Utils, Vector3, Quaternion, rad2deg
 } from '@wemap/maths';
 import Constants from '../Constants';
 import isNumber from 'lodash.isnumber';
+import isFinite from 'lodash.isfinite';
 import Level from './Level';
 
 /**
@@ -11,14 +12,18 @@ import Level from './Level';
  *
  * Basic geo methods are directly accessibles from here:
  * distanceTo, bearingTo, toEcef...
+ *
+ * /!\ This class has been adapted to use earth as a sphere and not as an ellipsoid
+ * /!\ So, this class does not stricly represent WGS84 coordinates anymore
+ * /!\ This modifications have been made for computational improvements.
  */
 class WGS84 {
 
     constructor(lat, lng, alt, level) {
-        this._lat = lat;
-        this._lng = lng;
-        this._alt = alt;
-        this._level = level;
+        this.lat = lat;
+        this.lng = lng;
+        this.alt = alt;
+        this.level = level;
         this._ecef = null;
     }
 
@@ -39,22 +44,44 @@ class WGS84 {
     }
 
     set lat(lat) {
+        if (isNumber(lat) && Math.abs(lat) <= 90) {
+            this._lat = lat;
+        } else {
+            throw new Error('lat argument is not in [-90; 90]');
+        }
         this._ecef = null;
-        this._lat = lat;
     }
 
     set lng(lng) {
+        if (isNumber(lng) && Math.abs(lng) <= 180) {
+            this._lng = lng;
+        } else {
+            throw new Error('lng argument is not in [-180; 180]');
+        }
         this._ecef = null;
-        this._lng = lng;
     }
 
     set alt(alt) {
+        if (isNumber(alt) && isFinite(alt)) {
+            this._alt = alt;
+        } else {
+            if (typeof alt !== 'undefined' && alt !== null) {
+                throw new Error('alt argument is not a finite number');
+            }
+            this._alt = null;
+        }
         this._ecef = null;
-        this._alt = alt;
     }
 
     set level(level) {
-        this._level = level;
+        if (level instanceof Level) {
+            this._level = level;
+        } else {
+            if (typeof level !== 'undefined' && level !== null) {
+                throw new Error('level argument is not a Level object');
+            }
+            this._level = null;
+        }
     }
 
     clone() {
@@ -65,92 +92,104 @@ class WGS84 {
         return output;
     }
 
-    static equalsTo(pos1, pos2) {
+    /**
+     * Compares two WGS84
+     * @param {WGS84} pos1 position 1
+     * @param {WGS84} pos2 position 2
+     * @param {Number} eps latitude and longitude epsilon in degrees (default: 1e-8 [~1mm at lat=0])
+     * @param {Number} epsAlt altitude epsilon in meters (default: 1e-3 [= 1mm])
+     */
+    static equalsTo(pos1, pos2, eps = Constants.EPS_DEG_MM, epsAlt = Constants.EPS_MM) {
+
+        // Handle null comparison
+        if (pos1 === null && pos1 === pos2) {
+            return true;
+        }
+
         if (!(pos1 instanceof WGS84) || !(pos2 instanceof WGS84)) {
             return false;
         }
-        return pos2.lat === pos1.lat
-            && pos2.lng === pos1.lng
-            && Level.equalsTo(pos2.level, pos1.level);
+
+        return Math.abs(pos2.lat - pos1.lat) < eps
+            && Math.abs(pos2.lng - pos1.lng) < eps
+            && (pos1.alt === pos2.alt
+                || pos1.alt !== null && pos2.alt !== null
+                && Math.abs(pos2.alt - pos1.alt) < epsAlt)
+            && Level.equalsTo(pos1.level, pos2.level);
     }
 
     equalsTo(other) {
         return WGS84.equalsTo(this, other);
     }
 
-    toString() {
-        let str = '[' + this._lat.toFixed(7) + ', ' + this._lng.toFixed(7);
-        if (this._alt) {
-            str += ', ' + this._alt.toFixed(2);
-        }
-        if (this._level) {
-            str += ', [' + this._level.toString() + ']';
-        }
-        str += ']';
-        return str;
-    }
-
-    destinationPoint(distance, bearing, elevation = 0) {
+    destinationPoint(distance, bearing, elevation) {
         const newPoint = this.clone();
         newPoint.move(distance, bearing, elevation);
         return newPoint;
     }
 
     // Source: http://www.movable-type.co.uk/scripts/latlong.html#destPoint
-    move(distance, bearing, elevation = 0) {
-
-        const d = distance;
-        const R = Constants.R_MAJOR;
-
-        const φ1 = Utils.deg2rad(this.lat);
-        const λ1 = Utils.deg2rad(this.lng);
-
-        const φ2 = Math.asin(Math.sin(φ1) * Math.cos(d / R) + Math.cos(φ1) * Math.sin(d / R) * Math.cos(bearing));
-        const λ2 = λ1 + Math.atan2(Math.sin(bearing) * Math.sin(d / R) * Math.cos(φ1), Math.cos(d / R) - Math.sin(φ1) * Math.sin(φ2));
-
-        const altitude = (this.alt || 0) + elevation;
+    move(distance, bearing, elevation) {
+
+        const dR = distance / Constants.R_MAJOR;
+        const cosDr = Math.cos(dR);
+        const sinDr = Math.sin(dR);
+
+        const phi1 = Utils.deg2rad(this.lat);
+        const lambda1 = Utils.deg2rad(this.lng);
+
+        const phi2 = Math.asin(
+            Math.sin(phi1) * cosDr
+            + Math.cos(phi1) * sinDr * Math.cos(bearing)
+        );
+        const lambda2 = lambda1 + Math.atan2(
+            Math.sin(bearing) * sinDr * Math.cos(phi1),
+            cosDr - Math.sin(phi1) * Math.sin(phi2)
+        );
+
+        this.lat = Utils.rad2deg(phi2);
+        this.lng = Utils.rad2deg(lambda2);
+
+        if (isNumber(elevation) && isFinite(elevation)) {
+            if (this.alt === null) {
+                throw new Error('Point altitude is not defined');
+            }
+            this.alt += elevation;
+        }
 
-        this.lat = Utils.rad2deg(φ2);
-        this.lng = Utils.rad2deg(λ2);
-        this.alt = altitude;
 
         return this;
     }
 
     /**
      * Returns a distance between two points in meters
-     * @param  {Number} location2 latitude / longitude point2
+     * @param  {WGS84} location2 latitude / longitude point
      * @return {Number} distance in meters
      */
     distanceTo(location2) {
-        var cosn, dist, dlat, dlng, angle, tangy, tangx,
-            dlatsin, dlngsin, lat1cos, lat2cos, lat2rad, lat1rad;
-
         const lat1 = this.lat;
         const lng1 = this.lng;
 
         const lat2 = location2.lat;
         const lng2 = location2.lng;
 
-        dlat = Utils.deg2rad(lat2 - lat1);
-        dlng = Utils.deg2rad(lng2 - lng1);
+        const dlat = Utils.deg2rad(lat2 - lat1);
+        const dlng = Utils.deg2rad(lng2 - lng1);
 
-        dlngsin = Math.sin(dlng / 2);
-        dlatsin = Math.sin(dlat / 2);
-        lat1rad = Utils.deg2rad(lat1);
-        lat1cos = Math.cos(lat1rad);
-        lat2rad = Utils.deg2rad(lat2);
-        lat2cos = Math.cos(lat2rad);
-        angle = (dlatsin * dlatsin + lat1cos * lat2cos * dlngsin * dlngsin);
+        const dlngsin = Math.sin(dlng / 2);
+        const dlatsin = Math.sin(dlat / 2);
+        const lat1rad = Utils.deg2rad(lat1);
+        const lat1cos = Math.cos(lat1rad);
+        const lat2rad = Utils.deg2rad(lat2);
+        const lat2cos = Math.cos(lat2rad);
+        const angle = dlatsin * dlatsin + lat1cos * lat2cos * dlngsin * dlngsin;
 
         // arctangent
-        tangy = Math.sqrt(angle);
-        tangx = Math.sqrt(1 - angle);
-        cosn = (2 * Math.atan2(tangy, tangx));
+        const tangy = Math.sqrt(angle);
+        const tangx = Math.sqrt(1 - angle);
+        const cosn = 2 * Math.atan2(tangy, tangx);
 
-        // distance in km
-        dist = (Constants.EARTH_RADIUS_KM * 1000 * cosn);
-        return dist;
+        return Constants.R_MAJOR * cosn;
     }
 
     static distanceBetween(point1, point2) {
@@ -166,24 +205,31 @@ class WGS84 {
             Math.cos(lat1) * Math.sin(lat2) - Math.sin(lat1) * Math.cos(lat2) * Math.cos(diffLng));
     }
 
+    static bearingTo(point1, point2) {
+        return point1.bearingTo(point2);
+    }
+
+
+    /**
+     * ECEF Transformations
+     * Here we used a light version of ECEF considering earth
+     * as a sphere instead of an ellipse
+     */
 
-    getEnuToEcefRotation() {
+    get enuToEcefRotation() {
         const rot1 = Quaternion.fromAxisAngle([0, 0, 1], Math.PI / 2 + Utils.deg2rad(this.lng));
         const rot2 = Quaternion.fromAxisAngle([1, 0, 0], Math.PI / 2 - Utils.deg2rad(this.lat));
         return Quaternion.multiply(rot1, rot2);
     }
 
-    getEcefToEnuRotation() {
+    get ecefToEnuRotation() {
         const rot1 = Quaternion.fromAxisAngle([1, 0, 0], Utils.deg2rad(this.lat) - Math.PI / 2);
         const rot2 = Quaternion.fromAxisAngle([0, 0, 1], -Utils.deg2rad(this.lng) - Math.PI / 2);
         return Quaternion.multiply(rot1, rot2);
     }
 
-    getUpVector() {
-        return Vector3.normalize(this.ecef);
-    }
-
-
+    // https://gist.github.com/klucar/1536194
+    // Adapted for spherical formula
     get ecef() {
 
         if (!this._ecef) {
@@ -191,11 +237,9 @@ class WGS84 {
             const lng = Utils.deg2rad(this.lng);
             const alt = this.alt || 0;
 
-            const N = Constants.R_MAJOR / Math.sqrt(1 - Constants.ECCENTRICITY_2 * Math.pow(Math.sin(lat), 2));
-
-            const x = (N + alt) * Math.cos(lat) * Math.cos(lng);
-            const y = (N + alt) * Math.cos(lat) * Math.sin(lng);
-            const z = ((1 - Constants.ECCENTRICITY_2) * N + alt) * Math.sin(lat);
+            const x = (Constants.R_MAJOR + alt) * Math.cos(lat) * Math.cos(lng);
+            const y = (Constants.R_MAJOR + alt) * Math.cos(lat) * Math.sin(lng);
+            const z = (Constants.R_MAJOR + alt) * Math.sin(lat);
 
             this._ecef = [x, y, z];
         }
@@ -209,50 +253,49 @@ class WGS84 {
         const y = ecef[1];
         const z = ecef[2];
 
-        const b = Math.sqrt(Constants.R_MAJOR_2 * (1 - Constants.ECCENTRICITY_2));
-        const bsq = Math.pow(b, 2);
-        const ep = Math.sqrt((Constants.R_MAJOR_2 - bsq) / bsq);
-        const p = Math.sqrt(Math.pow(x, 2) + Math.pow(y, 2));
-        const th = Math.atan2(Constants.R_MAJOR * z, b * p);
+        const p = Math.sqrt(x ** 2 + y ** 2);
 
         let lng = Math.atan2(y, x);
-        const lat = Math.atan2((z + Math.pow(ep, 2) * b * Math.pow(Math.sin(th), 3)), (p - Constants.ECCENTRICITY_2 * Constants.R_MAJOR * Math.pow(Math.cos(th), 3)));
-        const N = Constants.R_MAJOR / (Math.sqrt(1 - Constants.ECCENTRICITY_2 * Math.pow(Math.sin(lat), 2)));
-        const alt = p / Math.cos(lat) - N;
+        const lat = Math.atan2(z, p);
+        const alt = p / Math.cos(lat) - Constants.R_MAJOR;
 
         lng = lng % (2 * Math.PI);
 
-        const newPoint = new WGS84(Utils.rad2deg(lat), Utils.rad2deg(lng), Utils.rad2deg(alt));
+        const newPoint = new WGS84(rad2deg(lat), rad2deg(lng), alt);
         newPoint._ecef = ecef;
         return newPoint;
     }
 
+
     // https://stackoverflow.com/questions/1299567/how-to-calculate-distance-from-a-point-to-a-line-segment-on-a-sphere
     // Adapted to ECEF
-    getProjection(edge) {
-
-        const p1 = edge.node1.coords;
-        const p2 = edge.node2.coords;
+    getSegmentProjection(p1, p2) {
 
         const a = Vector3.normalize(p1.ecef);
         const b = Vector3.normalize(p2.ecef);
         const c = Vector3.normalize(this.ecef);
 
         const G = Vector3.cross(a, b);
+        if (Vector3.norm(G) === 0) {
+            return null;
+        }
+
         const F = Vector3.cross(c, G);
         const t = Vector3.normalize(Vector3.cross(G, F));
 
-        const posECEF = Vector3.multiplyScalar(t, this.getEarthRadiusAtPosition());
+        const posECEF = Vector3.multiplyScalar(t, Constants.R_MAJOR);
         const poseWGS84 = WGS84.fromECEF(posECEF);
 
         // poseWGS84.alt is not 0 here due to the ECEF transformation residual.
         // So if p1.alt and p2.alt are defined we take the middle elevation between p1 and p2.
         // Otherwise we remove alt from projection because the residual has no sense.
         let alt;
-        if (isNumber(p1.alt) && isNumber(p2.alt)) {
+        if (p1.alt !== null && p2.alt !== null) {
+            // This formula is maybe not the best one.
             alt = (p1.alt + p2.alt) / 2;
         }
-        const projection = new WGS84(poseWGS84.lat, poseWGS84.lng, alt);
+        const projection = new WGS84(poseWGS84.lat, poseWGS84.lng,
+            alt, Level.intersect(p1.level, p2.level));
 
         if (Math.abs((p1.distanceTo(p2) - p1.distanceTo(projection) - p2.distanceTo(projection))) > 1e-6) {
             return null;
@@ -261,67 +304,40 @@ class WGS84 {
         return projection;
     }
 
+    /**
+     * Input / Output
+     */
 
-    getEarthRadiusAtPosition() {
-        const latR = Utils.deg2rad(this.lat);
-        const cos2 = Math.cos(latR) * Math.cos(latR);
-        const sin2 = Math.sin(latR) * Math.sin(latR);
-        return Math.sqrt((Constants.R_MAJOR_4 * cos2 + Constants.R_MINOR_4 * sin2) / (Constants.R_MAJOR_2 * cos2 + Constants.R_MINOR_2 * sin2));
+    toString() {
+        let str = '[' + this._lat.toFixed(7) + ', ' + this._lng.toFixed(7);
+        if (this._alt !== null) {
+            str += ', ' + this._alt.toFixed(2);
+        }
+        if (this._level !== null) {
+            str += ', [' + this._level.toString() + ']';
+        }
+        str += ']';
+        return str;
     }
 
-    toMessage() {
+    toJson() {
         const output = {
             lat: this.lat,
             lng: this.lng
         };
-        if (isNumber(this.alt)) {
+        if (this.alt !== null) {
             output.alt = this.alt;
         }
-        if (this.level) {
-            output.level = this.level;
+        if (this.level !== null) {
+            output.level = this.level.toString();
         }
         return output;
     }
 
-    static fromMessage(message) {
-        return new WGS84(message.lat, message.lng, message.alt, Level.fromString(message.level));
+    static fromJson(json) {
+        return new WGS84(json.lat, json.lng, json.alt, Level.fromString(json.level));
     }
 
-    static sampleRoute(route, stepSize = 0.7, maxLength = Number.MAX_VALUE) {
-
-        let p1, p2 = null;
-        let bearing, distance;
-
-        const sampledRoute = [];
-        let reportedDistance = 0;
-        let totalDistance = 0;
-
-        for (let i = 0; i < route.length - 1; i++) {
-
-            p1 = route[i];
-            p2 = route[i + 1];
-
-            bearing = p1.bearingTo(p2);
-            distance = p1.distanceTo(p2);
-
-            if (distance > 0) {
-                let ratio = reportedDistance / distance;
-                while (ratio < 1 && totalDistance <= maxLength) {
-                    const newPoint = p1.destinationPoint(ratio * distance, bearing);
-                    newPoint.bearing = bearing;
-                    sampledRoute.push(newPoint);
-                    ratio += stepSize / distance;
-                    totalDistance += stepSize;
-                }
-                reportedDistance = (ratio - 1) * distance;
-            }
-        }
-        if (p2) {
-            sampledRoute.push(p2);
-        }
-
-        return sampledRoute;
-    }
 }
 
 export default WGS84;