erp5_officejs_drone_simulator: fix drone position update, speed and coordinates conversion

See merge request !1806

bt5/erp5_officejs_drone_simulator/PathTemplateItem/web_page_module/drone_simulator_dronelogfollower_js.js

@@ -61,16 +61,14 @@ var DroneLogAPI = /** @class */ (function () {
       timestamp = parseInt(splitted_log_entry[0], 10);
       lat = parseFloat(splitted_log_entry[1]);
       lon = parseFloat(splitted_log_entry[2]);
-      x = this._mapManager.longitudToX(lon, map_dict.map_size);
-      y = this._mapManager.latitudeToY(lat, map_dict.map_size);
-      position = this._mapManager.normalize(x, y, map_dict);
       height = parseFloat(splitted_log_entry[4]);
       if (height < min_height) {
         height = min_height;
       }
-      converted_log_point_list.push([position[0],
-                                    position[1],
-                                    height, timestamp / time_offset]);
+      position = this._mapManager.convertToLocalCoordinates(lat, lon, height);
+      converted_log_point_list.push([position.x,
+                                    position.y,
+                                    position.z, timestamp / time_offset]);
     }
     this._flight_parameters.converted_log_point_list = converted_log_point_list;
   };

bt5/erp5_officejs_drone_simulator/PathTemplateItem/web_page_module/drone_simulator_dronelogfollower_js.xml

@@ -232,7 +232,7 @@
                   </item>
                   <item>
                       <key> <string>actor</string> </key>
-                      <value> <string>zope</string> </value>
+                      <value> <unicode>zope</unicode> </value>
                   </item>
                   <item>
                       <key> <string>comment</string> </key>
@@ -246,7 +246,7 @@
                   </item>
                   <item>
                       <key> <string>serial</string> </key>
-                      <value> <string>1009.38419.20212.62293</string> </value>
+                      <value> <string>1010.13157.33937.23381</string> </value>
                   </item>
                   <item>
                       <key> <string>state</string> </key>
@@ -266,7 +266,7 @@
                           </tuple>
                           <state>
                             <tuple>
-                              <float>1688635412.87</float>
+                              <float>1691051883.97</float>
                               <string>UTC</string>
                             </tuple>
                           </state>

bt5/erp5_officejs_drone_simulator/PathTemplateItem/web_page_module/drone_simulator_fixedwingdrone_js.js

@@ -92,7 +92,7 @@ var FixedWingDroneAPI = /** @class */ (function () {
   ** Function called on every drone update, right before onUpdate AI script
   */
   FixedWingDroneAPI.prototype.internal_update = function (context, delta_time) {
-    var diff, newrot, orientationValue, rotStep, updateSpeed;
+    var diff, newrot, orientationValue, rotStep;
 
     //TODO rotation
     if (context._rotationTarget) {
@@ -116,18 +116,8 @@ var FixedWingDroneAPI = /** @class */ (function () {
     }
 
     this._updateSpeed(context, delta_time);
-    this._updateDirection(context, delta_time);
+    this._updatePosition(context, delta_time);
 
-    updateSpeed = context._speed * delta_time / 1000;
-    if (context._direction.x !== 0 ||
-        context._direction.y !== 0 ||
-        context._direction.z !== 0) {
-      context._controlMesh.position.addInPlace(new BABYLON.Vector3(
-        context._direction.x * updateSpeed,
-        context._direction.y * updateSpeed,
-        context._direction.z * updateSpeed
-      ));
-    }
     //TODO rotation
     orientationValue = context._maxOrientation *
       (context._speed / context._maxSpeed);
@@ -180,51 +170,100 @@ var FixedWingDroneAPI = /** @class */ (function () {
     }
   };
 
-  FixedWingDroneAPI.prototype._updateDirection = function (drone, delta_time) {
-    var horizontalCoeff, newX, newY, newZ, tangentYaw;
+  FixedWingDroneAPI.prototype._updatePosition = function (drone, delta_time) {
+    var R = 6371e3,
+      currentGeoCoordinates = this._mapManager.convertToGeoCoordinates(
+        drone.position.x,
+        drone.position.y,
+        drone.position.z
+      ),
+      targetCoordinates = this._mapManager.convertToGeoCoordinates(
+        drone._targetCoordinates.x,
+        drone._targetCoordinates.y,
+        drone._targetCoordinates.z
+      ),
+      bearing = this._computeBearing(
+        currentGeoCoordinates.x,
+        currentGeoCoordinates.y,
+        targetCoordinates.x,
+        targetCoordinates.y
+      ),
+      currentCosLat,
+      currentLatRad,
+      distance,
+      distanceCos,
+      distanceSin,
+      currentSinLat,
+      currentLonRad,
+      groundSpeed,
+      newCoordinates,
+      newLatRad,
+      newLonRad,
+      newYaw,
+      newYawRad,
+      verticalSpeed,
+      yawToDirection;
 
     if (this._loiter_mode
         && Math.sqrt(
           Math.pow(drone._targetCoordinates.x - drone.position.x, 2)
             + Math.pow(drone._targetCoordinates.y - drone.position.y, 2)
         ) <= this._loiter_radius) {
-      tangentYaw = this._computeBearing(
-        drone.position.x,
-        drone.position.y,
-        drone._targetCoordinates.x,
-        drone._targetCoordinates.y
-      ) - 90;
-      // trigonometric circle is east oriented, yaw angle is clockwise
-      tangentYaw = this._toRad(-tangentYaw + 90);
-      newX = Math.cos(tangentYaw);
-      newZ = Math.sin(tangentYaw);
+      newYaw = bearing - 90;
     } else {
-      [newX, newZ] = this._getNewYaw(drone, delta_time);
+      newYaw = this._getNewYaw(drone, bearing, delta_time);
     }
-    newY = this._getNewAltitude(drone);
+    newYawRad = this._toRad(newYaw);
+
+    currentLatRad = this._toRad(currentGeoCoordinates.x);
+    currentCosLat = Math.cos(currentLatRad);
+    currentSinLat = Math.sin(currentLatRad);
+    currentLonRad = this._toRad(currentGeoCoordinates.y);
+
+    verticalSpeed = this._getVerticalSpeed(drone);
+    groundSpeed = Math.sqrt(
+      Math.pow(drone.getAirSpeed(), 2) - Math.pow(verticalSpeed, 2)
+    );
+
+    distance = (groundSpeed * delta_time / 1000) / R;
+    distanceCos = Math.cos(distance);
+    distanceSin = Math.sin(distance);
 
-    horizontalCoeff = Math.sqrt(
-      (
-        Math.pow(drone.getAirSpeed(), 2) - Math.pow(newY, 2)
-      ) / (
-        Math.pow(newX, 2) + Math.pow(newZ, 2)
-      )
+    newLatRad = Math.asin(
+      currentSinLat * distanceCos
+        + currentCosLat * distanceSin * Math.cos(newYawRad)
     );
-    newX *= horizontalCoeff;
-    newZ *= horizontalCoeff;
+    newLonRad = currentLonRad + Math.atan2(
+      Math.sin(newYawRad) * distanceSin * currentCosLat,
+      distanceCos - currentSinLat * Math.sin(newLatRad)
+    );
+
+    newCoordinates = this._mapManager.convertToLocalCoordinates(
+      this._toDeg(newLatRad),
+      this._toDeg(newLonRad),
+      drone.position.z
+    );
+
     // swap y and z axis so z axis represents altitude
-    drone.setDirection(newX, newZ, newY);
+    drone._controlMesh.position.addInPlace(new BABYLON.Vector3(
+      Math.abs(newCoordinates.x - drone.position.x)
+        * (newCoordinates.x < drone.position.x ? -1 : 1),
+      verticalSpeed * delta_time / 1000,
+      Math.abs(newCoordinates.y - drone.position.y)
+        * (newCoordinates.y < drone.position.y ? -1 : 1)
+    ));
+    yawToDirection = this._toRad(-newYaw + 90);
+    drone.setDirection(
+      groundSpeed * Math.cos(yawToDirection),
+      groundSpeed * Math.sin(yawToDirection),
+      verticalSpeed
+    );
   };
 
-  FixedWingDroneAPI.prototype._getNewYaw = function (drone, delta_time) {
+  FixedWingDroneAPI.prototype._getNewYaw =
+    function (drone, bearing, delta_time) {
       // swap y and z axis so z axis represents altitude
-    var bearing = this._computeBearing(
-        drone.position.x,
-        drone.position.y,
-        drone._targetCoordinates.x,
-        drone._targetCoordinates.y
-      ),
-      yaw = drone.getYaw(),
+      var yaw = drone.getYaw(),
         yawDiff = this._computeYawDiff(yaw, bearing),
         yawUpdate = this.getYawVelocity(drone) * delta_time / 1000;
 
@@ -233,14 +272,10 @@ var FixedWingDroneAPI = /** @class */ (function () {
       } else if (yawDiff < 0) {
         yawUpdate *= -1;
       }
-    yaw += yawUpdate;
-
-    // trigonometric circle is east oriented, yaw angle is clockwise
-    yaw = this._toRad(-yaw + 90);
-    return [Math.cos(yaw), Math.sin(yaw)];
+      return yaw + yawUpdate;
     };
 
-  FixedWingDroneAPI.prototype._getNewAltitude = function (drone) {
+  FixedWingDroneAPI.prototype._getVerticalSpeed = function (drone) {
     // swap y and z axis so z axis represents altitude
     var altitudeDiff = drone._targetCoordinates.z - drone.position.z,
       verticalSpeed;
@@ -356,24 +391,17 @@ var FixedWingDroneAPI = /** @class */ (function () {
     if (isNaN(lat) || isNaN(lon) || isNaN(z)) {
       throw new Error('Target coordinates must be numbers');
     }
-    var x = this._mapManager.longitudToX(lon, this._map_dict.map_size),
-      y = this._mapManager.latitudeToY(lat, this._map_dict.map_size),
-      position = this._mapManager.normalize(x, y, this._map_dict),
-      processed_coordinates;
-    if (z > this._map_dict.start_AMSL) {
-      z -= this._map_dict.start_AMSL;
+    var processed_coordinates =
+      this._mapManager.convertToLocalCoordinates(lat, lon, z);
+    if (processed_coordinates.z > this._map_dict.start_AMSL) {
+      processed_coordinates.z -= this._map_dict.start_AMSL;
     }
-    processed_coordinates = {
-      x: position[0],
-      y: position[1],
-      z: z
-    };
     //this._last_altitude_point_reached = -1;
     //this.takeoff_path = [];
     return processed_coordinates;
   };
   FixedWingDroneAPI.prototype.getCurrentPosition = function (x, y, z) {
-    return this._mapManager.convertToGeoCoordinates(x, y, z, this._map_dict);
+    return this._mapManager.convertToGeoCoordinates(x, y, z);
   };
   FixedWingDroneAPI.prototype.getDroneAI = function () {
     return null;
@@ -419,11 +447,19 @@ var FixedWingDroneAPI = /** @class */ (function () {
   };
   FixedWingDroneAPI.prototype.getYaw = function (drone) {
     var direction = drone.worldDirection;
-    return this._computeBearing(0, 0, direction.x, direction.z);
-  };
-  FixedWingDroneAPI.prototype._computeBearing = function (x1, z1, x2, z2) {
-    return this._toDeg(Math.atan2(x2 - x1, z2 - z1));
+    return this._toDeg(Math.atan2(direction.x, direction.z));
+  };
+  FixedWingDroneAPI.prototype._computeBearing =
+      function (lat1, lon1, lat2, lon2) {
+      var dLon = this._toRad(lon2 - lon1),
+        lat1Rad = this._toRad(lat1),
+        lat2Rad = this._toRad(lat2),
+        x = Math.cos(lat2Rad) * Math.sin(dLon),
+        y = Math.cos(lat1Rad) * Math.sin(lat2Rad)
+          - Math.sin(lat1Rad) * Math.cos(lat2Rad) * Math.cos(dLon);
+      return this._toDeg(Math.atan2(x, y));
     };
+
   FixedWingDroneAPI.prototype._computeYawDiff = function (yaw1, yaw2) {
     var diff = yaw2 - yaw1;
     diff += (diff > 180) ? -360 : (diff < -180) ? 360 : 0;

bt5/erp5_officejs_drone_simulator/PathTemplateItem/web_page_module/drone_simulator_fixedwingdrone_js.xml

@@ -240,7 +240,7 @@
                   </item>
                   <item>
                       <key> <string>serial</string> </key>
-                      <value> <string>1009.59163.16294.47701</string> </value>
+                      <value> <string>1010.13155.61424.54442</string> </value>
                   </item>
                   <item>
                       <key> <string>state</string> </key>
@@ -260,7 +260,7 @@
                           </tuple>
                           <state>
                             <tuple>
-                              <float>1690395852.2</float>
+                              <float>1691051762.91</float>
                               <string>UTC</string>
                             </tuple>
                           </state>

bt5/erp5_officejs_drone_simulator/PathTemplateItem/web_page_module/drone_simulator_logic_js.js

@@ -367,41 +367,11 @@ var DroneManager = /** @class */ (function () {
 
 var MapManager = /** @class */ (function () {
   "use strict";
-  function calculateMapInfo(map, map_dict, initial_position) {
-    var max_width = map.latLonDistance([map_dict.min_lat, map_dict.min_lon],
-                                       [map_dict.min_lat, map_dict.max_lon]),
-      max_height = map.latLonDistance([map_dict.min_lat, map_dict.min_lon],
-                                      [map_dict.max_lat, map_dict.min_lon]),
-      map_size = Math.ceil(Math.max(max_width, max_height)) * 0.6,
-      map_info = {
-        "depth": map_size,
-        "height": map_dict.height,
-        "width": map_size,
-        "map_size": map_size,
-        "min_x": map.longitudToX(map_dict.min_lon, map_size),
-        "min_y": map.latitudeToY(map_dict.min_lat, map_size),
-        "max_x": map.longitudToX(map_dict.max_lon, map_size),
-        "max_y": map.latitudeToY(map_dict.max_lat, map_size),
-        "start_AMSL": map_dict.start_AMSL
-      },
-      position = map.normalize(
-        map.longitudToX(initial_position.longitude, map_size),
-        map.latitudeToY(initial_position.latitude, map_size),
-        map_info
-      );
-    map_info.initial_position = {
-      "x": position[0],
-      "y": position[1],
-      "z": initial_position.z
-    };
-    return map_info;
-  }
   //** CONSTRUCTOR
   function MapManager(scene) {
     var _this = this, max_sky, skybox, skyboxMat, largeGroundMat,
       largeGroundBottom, width, depth, terrain, max;
-    _this.map_info = calculateMapInfo(_this, GAMEPARAMETERS.map,
-                                      GAMEPARAMETERS.initialPosition);
+    this.setMapInfo(GAMEPARAMETERS.map, GAMEPARAMETERS.initialPosition);
     max = _this.map_info.width;
     if (_this.map_info.depth > max) {
       max = _this.map_info.depth;
@@ -446,14 +416,37 @@ var MapManager = /** @class */ (function () {
     terrain.scaling = new BABYLON.Vector3(depth / 50000, depth / 50000,
                                           width / 50000);
   }
+  MapManager.prototype.setMapInfo = function (map_dict, initial_position) {
+    var max_width = this.latLonDistance([map_dict.min_lat, map_dict.min_lon],
+                                       [map_dict.min_lat, map_dict.max_lon]),
+      max_height = this.latLonDistance([map_dict.min_lat, map_dict.min_lon],
+                                      [map_dict.max_lat, map_dict.min_lon]),
+      map_size = Math.ceil(Math.max(max_width, max_height)) * 0.6;
+    this.map_info = {
+      "depth": map_size,
+      "height": map_dict.height,
+      "width": map_size,
+      "map_size": map_size,
+      "start_AMSL": map_dict.start_AMSL
+    };
+    this.map_info.min_x = this.longitudToX(map_dict.min_lon);
+    this.map_info.min_y = this.latitudeToY(map_dict.min_lat);
+    this.map_info.max_x = this.longitudToX(map_dict.max_lon);
+    this.map_info.max_y = this.latitudeToY(map_dict.max_lat);
+    this.map_info.initial_position = this.convertToLocalCoordinates(
+      initial_position.latitude,
+      initial_position.longitude,
+      initial_position.z
+    );
+  };
   MapManager.prototype.getMapInfo = function () {
     return this.map_info;
   };
-  MapManager.prototype.longitudToX = function (lon, map_size) {
-    return (map_size / 360.0) * (180 + lon);
+  MapManager.prototype.longitudToX = function (lon) {
+    return (this.map_info.map_size / 360.0) * (180 + lon);
   };
-  MapManager.prototype.latitudeToY = function (lat, map_size) {
-    return (map_size / 180.0) * (90 - lat);
+  MapManager.prototype.latitudeToY = function (lat) {
+    return (this.map_info.map_size / 180.0) * (90 - lat);
   };
   MapManager.prototype.latLonDistance = function (c1, c2) {
     var R = 6371e3,
@@ -467,23 +460,30 @@ var MapManager = /** @class */ (function () {
       c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1 - a));
     return R * c;
   };
-  MapManager.prototype.normalize = function (x, y, map_dict) {
-    var n_x = (x - map_dict.min_x) / (map_dict.max_x - map_dict.min_x),
-      n_y = (y - map_dict.min_y) / (map_dict.max_y - map_dict.min_y);
-    return [n_x * 1000 - map_dict.width / 2,
-            n_y * 1000 - map_dict.depth / 2];
+  MapManager.prototype.convertToLocalCoordinates =
+    function (latitude, longitude, altitude) {
+      var map_info = this.map_info,
+        x = this.longitudToX(longitude),
+        y = this.latitudeToY(latitude);
+      return {
+        x: ((x - map_info.min_x) / (map_info.max_x - map_info.min_x))
+          * 1000 - map_info.width / 2,
+        y: ((y - map_info.min_y) / (map_info.max_y - map_info.min_y))
+          * 1000 - map_info.depth / 2,
+        z: altitude
+      };
     };
-  MapManager.prototype.convertToGeoCoordinates = function (x, y, z, map_dict) {
-    var lon = x + map_dict.width / 2,
-      lat = y + map_dict.depth / 2;
+  MapManager.prototype.convertToGeoCoordinates = function (x, y, z) {
+    var lon = x + this.map_info.width / 2,
+      lat = y + this.map_info.depth / 2;
     lon = lon / 1000;
-    lon = lon * (map_dict.max_x - map_dict.min_x) +
-      map_dict.min_x;
-    lon = lon / (map_dict.width / 360.0) - 180;
+    lon = lon * (this.map_info.max_x - this.map_info.min_x) +
+      this.map_info.min_x;
+    lon = lon / (this.map_info.map_size / 360.0) - 180;
     lat = lat / 1000;
-    lat = lat * (map_dict.max_y - map_dict.min_y) +
-      map_dict.min_y;
-    lat = 90 - lat / (map_dict.depth / 180.0);
+    lat = lat * (this.map_info.max_y - this.map_info.min_y) +
+      this.map_info.min_y;
+    lat = 90 - lat / (this.map_info.map_size / 180.0);
     return {
       x: lat,
       y: lon,
@@ -503,7 +503,7 @@ var GameManager = /** @class */ (function () {
   "use strict";
   // *** CONSTRUCTOR ***
   function GameManager(canvas, game_parameters_json) {
-    var drone, header_list;
+    var drone, header_list, i;
     this._canvas = canvas;
     this._canvas_width = canvas.width;
     this._canvas_height = canvas.height;
@@ -555,7 +555,7 @@ var GameManager = /** @class */ (function () {
       console.log = function () {
         baseLogFunction.apply(console, arguments);
         var args = Array.prototype.slice.call(arguments);
-        for (var i = 0;i < args.length;i++) {
+        for (i = 0; i < args.length; i += 1) {
           console_log += args[i] + "\n";
         }
       };
@@ -706,8 +706,7 @@ var GameManager = /** @class */ (function () {
                 geo_coordinates = map_manager.convertToGeoCoordinates(
                   drone_position.x,
                   drone_position.y,
-                  drone_position.z,
-                  map_info
+                  drone_position.z
                 );
                 game_manager._flight_log[index].push([
                   game_manager._game_duration, geo_coordinates.x,

bt5/erp5_officejs_drone_simulator/PathTemplateItem/web_page_module/drone_simulator_logic_js.xml

@@ -240,7 +240,7 @@
                   </item>
                   <item>
                       <key> <string>serial</string> </key>
-                      <value> <string>1010.3592.54606.53538</string> </value>
+                      <value> <string>1010.13183.29224.8874</string> </value>
                   </item>
                   <item>
                       <key> <string>state</string> </key>
@@ -260,7 +260,7 @@
                           </tuple>
                           <state>
                             <tuple>
-                              <float>1690478004.4</float>
+                              <float>1691053915.45</float>
                               <string>UTC</string>
                             </tuple>
                           </state>