Skip to content

erp5
erp5
Commit c29f7b4d authored by Roque's avatar Roque Committed by Kazuhiko Shiozaki
Browse files
Options

erp5_officejs_drone_capture_flag: refactor map coordinates 

- drop map reduction
- fix random seed issue (drop relation with #drones)
- use min-max lat-lon map parameters for init
parent 2feb6f45
Show whitespace changes
Inline Side-by-side
Showing with 222 additions and 168 deletions
bt5/erp5_officejs_drone_capture_flag/PathTemplateItem/web_page_module/drone_capture_flag_fixedwingdrone_js.js
View file @ c29f7b4d
......@@ -93,7 +93,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) {
......@@ -117,18 +117,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);
......@@ -181,50 +171,99 @@ 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,20 +391,16 @@ var FixedWingDroneAPI = /** @class */ (function () {
if (isNaN(lat) || isNaN(lon) || isNaN(z)) {
throw new Error('Target coordinates must be numbers');
}
var point = this._mapManager.toLocalCoordinates(lat, lon,
this._map_dict.map_size),
position = this._mapManager.normalize(point.x, point.y, this._map_dict);
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;
}
return {
x: position[0],
y: position[1],
z: z
};
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.getDroneViewInfo = function (drone) {
var context = this, result = { "obstacles": [], "drones": [] }, distance,
......@@ -455,10 +486,17 @@ 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;
......
bt5/erp5_officejs_drone_capture_flag/PathTemplateItem/web_page_module/drone_capture_flag_fixedwingdrone_js.xml
View file @ c29f7b4d
......@@ -246,7 +246,7 @@
</item>
<item>
<key> <string>serial</string> </key>
<value> <string>1010.5034.64121.33006</string> </value>
<value> <string>1010.22181.14558.58982</string> </value>
</item>
<item>
<key> <string>state</string> </key>
......@@ -266,7 +266,7 @@
</tuple>
<state>
<tuple>
<float>1690565260.64</float>
<float>1691593350.04</float>
<string>UTC</string>
</tuple>
</state>
......
bt5/erp5_officejs_drone_capture_flag/PathTemplateItem/web_page_module/drone_capture_flag_logic_js.js
View file @ c29f7b4d
......@@ -408,70 +408,15 @@ var DroneManager = /** @class */ (function () {
var MapManager = /** @class */ (function () {
"use strict";
//random geo-point:
var MIN_LAT = 45.64,
MIN_LON = 14.253,
EPSILON = 9.9,
var EPSILON = 9.9,
START_Z = 15,
R = 6371e3;
function calculateMapInfo(map, map_dict) {
var min_lat = map_dict.min_lat || MIN_LAT,
min_lon = map_dict.min_lon || MIN_LON,
offset = map.latLonOffset(min_lat, min_lon, map_dict.map_size),
max_lat = offset[0],
max_lon = offset[1],
starting_point = map_dict.map_size / 2 * -0.75,
local_min = map.toLocalCoordinates(min_lat, min_lon, map_dict.map_size),
local_max = map.toLocalCoordinates(max_lat, max_lon, map_dict.map_size);
map.map_info = {
"depth": map_dict.map_size,
"width": map_dict.map_size,
"map_size": map_dict.map_size,
"min_lat": min_lat,
"min_lon": min_lon,
"max_lat": max_lat,
"max_lon": max_lon,
"min_x": local_min.x,
"min_y": local_min.y,
"max_x": local_max.x,
"max_y": local_max.y,
"height": map_dict.height,
"start_AMSL": map_dict.start_AMSL,
"flag_list": map_dict.flag_list,
"geo_flag_list": [],
"flag_distance_epsilon": map_dict.flag_distance_epsilon || EPSILON,
"obstacle_list": map_dict.obstacle_list,
"geo_obstacle_list": [],
"initial_position": {
"x": 0,
"y": starting_point,
"z": START_Z
}
};
map_dict.flag_list.forEach(function (flag_info, index) {
map.map_info.geo_flag_list.push(map.convertToGeoCoordinates(
flag_info.position.x,
flag_info.position.y,
flag_info.position.z
));
});
map_dict.obstacle_list.forEach(function (obstacle_info, index) {
var geo_obstacle = {};
Object.assign(geo_obstacle, obstacle_info);
geo_obstacle.position = map.convertToGeoCoordinates(
obstacle_info.position.x,
obstacle_info.position.y,
obstacle_info.position.z
);
map.map_info.geo_obstacle_list.push(geo_obstacle);
});
}
//** CONSTRUCTOR
function MapManager(scene) {
var _this = this, max_sky, skybox, skyboxMat, largeGroundMat, flag_material,
largeGroundBottom, width, depth, terrain, max, flag_a, flag_b, mast, flag,
count = 0, new_obstacle;
calculateMapInfo(_this, GAMEPARAMETERS.map);
this.setMapInfo(GAMEPARAMETERS.map, GAMEPARAMETERS.initialPosition);
max = _this.map_info.width;
if (_this.map_info.depth > max) {
max = _this.map_info.depth;
......@@ -612,22 +557,74 @@ var MapManager = /** @class */ (function () {
_this._flag_list.push(flag);
});
}
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)),
starting_point = map_size / 2 * -0.75;
console.log("max_width:",max_width);
console.log("max_height:",max_height);
console.log("map_size:",map_size);
this.map_info = {
"depth": map_size,
"height": map_dict.height,
"width": map_size,
"map_size": map_size,
"start_AMSL": map_dict.start_AMSL,
"flag_list": map_dict.flag_list,
"geo_flag_list": [],
"flag_distance_epsilon": map_dict.flag_distance_epsilon || EPSILON,
"obstacle_list": map_dict.obstacle_list,
"geo_obstacle_list": [],
"initial_position": {
"x": 0,
"y": starting_point,
"z": START_Z
}
};
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);
var map = this;
map_dict.flag_list.forEach(function (flag_info, index) {
map.map_info.geo_flag_list.push(map.convertToGeoCoordinates(
flag_info.position.x,
flag_info.position.y,
flag_info.position.z
));
});
map_dict.obstacle_list.forEach(function (obstacle_info, index) {
var geo_obstacle = {};
Object.assign(geo_obstacle, obstacle_info);
geo_obstacle.position = map.convertToGeoCoordinates(
obstacle_info.position.x,
obstacle_info.position.y,
obstacle_info.position.z
);
map.map_info.geo_obstacle_list.push(geo_obstacle);
});
};
MapManager.prototype.getMapInfo = function () {
return this.map_info;
};
MapManager.prototype.latLonOffset = function (lat, lon, offset_in_mt) {
MapManager.prototype.longitudToX = function (lon) {
return (this.map_info.map_size / 360.0) * (180 + lon);
};
MapManager.prototype.latitudeToY = function (lat) {
return (this.map_info.map_size / 180.0) * (90 - lat);
};
//TODO refactor latLonOffset, should be the reverse of lat-lon distance
//then map_size can be used as parameter (get max lat-lon from map_size)
/*MapManager.prototype.latLonOffset = function (lat, lon, offset_in_mt) {
var R = 6371e3, //Earth radius
lat_offset = offset_in_mt / R,
lon_offset = offset_in_mt / (R * Math.cos(Math.PI * lat / 180));
return [lat + lat_offset * 180 / Math.PI,
lon + lon_offset * 180 / Math.PI];
};
MapManager.prototype.toLocalCoordinates = function (lat, lon, map_size) {
return {
"x": (map_size / 360.0) * (180 + lon),
"y": (map_size / 180.0) * (90 - lat)
};
};
};*/
MapManager.prototype.latLonDistance = function (c1, c2) {
var q1 = c1[0] * Math.PI / 180,
q2 = c2[0] * Math.PI / 180,
......@@ -639,24 +636,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) {
var map_dict = this.map_info,
lon = x + map_dict.width / 2,
lat = y + map_dict.depth / 2;
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,
......@@ -676,7 +679,7 @@ var GameManager = /** @class */ (function () {
"use strict";
// *** CONSTRUCTOR ***
function GameManager(canvas, game_parameters_json) {
var drone, header_list, drone_count;
var drone, header_list, drone_count, i;
this._canvas = canvas;
this._canvas_width = canvas.width;
this._canvas_height = canvas.height;
......@@ -737,7 +740,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++) {
console_log += args[i] + "\n";
}
};
......
bt5/erp5_officejs_drone_capture_flag/PathTemplateItem/web_page_module/drone_capture_flag_logic_js.xml
View file @ c29f7b4d
......@@ -246,7 +246,7 @@
</item>
<item>
<key> <string>serial</string> </key>
<value> <string>1010.5086.1058.34440</string> </value>
<value> <string>1010.22324.3760.41318</string> </value>
</item>
<item>
<key> <string>state</string> </key>
......@@ -266,7 +266,7 @@
</tuple>
<state>
<tuple>
<float>1690567567.2</float>
<float>1691601869.17</float>
<string>UTC</string>
</tuple>
</state>
......
bt5/erp5_officejs_drone_capture_flag/PathTemplateItem/web_page_module/ojs_drone_capture_flag_script_page_js.js
View file @ c29f7b4d
......@@ -6,7 +6,16 @@
//Drone default values - TODO: get them from the drone API
var SIMULATION_SPEED = 10,
SIMULATION_TIME = 270,
MAP_SIZE = 600,
//HARDCODED map size: it is defined by min-max lat-lon distance
//this is done by the map manager (latLonDistance)
//but map_size is needed here for map randomization (location of objects)
//TODO refactor: or randomization is moved to map manager (seed as param)
//or randomization is done here but with geo-coordinates (not meters)
MAP_SIZE = 902,
min_lat = 45.6475,
max_lat = 45.65,
min_lon = 14.265,
max_lon = 14.2766,
map_height = 700,
start_AMSL = 595,
DEFAULT_SPEED = 16,
......@@ -291,7 +300,7 @@
"hidden": 0,
"type": "FloatField"
},
"my_map_size": {
/*"my_map_size": {
"description": "",
"title": "Map size",
"default": MAP_SIZE,
......@@ -301,7 +310,7 @@
"key": "map_size",
"hidden": 0,
"type": "FloatField"
},
},*/
"my_start_AMSL": {
"description": "",
"title": "Start AMSL",
......@@ -380,7 +389,7 @@
group_list: [[
"left",
[["my_simulation_speed"], ["my_simulation_time"], ["my_number_of_drones"],
["my_map_size"], ["my_map_height"],// ["my_flag_weight"],
/*["my_map_size"], */["my_map_height"],// ["my_flag_weight"],
["my_start_AMSL"], ["my_map_seed"]]
], [
"right",
......@@ -407,6 +416,7 @@
var gadget = this, i,
fragment = gadget.element.querySelector('.simulator_div'),
game_parameters_json, map_json;
options.map_size = MAP_SIZE;
DRONE_LIST = [];
fragment = domsugar(gadget.element.querySelector('.simulator_div'),
[domsugar('div')]).firstElementChild;
......@@ -429,8 +439,7 @@
var seed_value = options.map_seed,
random_seed = new Math.seedrandom(seed_value), i,
n_enemies = randomIntFromInterval(5, 10, random_seed),
n_flags = randomIntFromInterval(Math.floor(DRONE_LIST.length / 2),
DRONE_LIST.length, random_seed),
n_flags = randomIntFromInterval(5, 10, random_seed),
n_obstacles = randomIntFromInterval(5, 15, random_seed),
flag_list = [], obstacle_list = [], enemy_list = [], random_position,
obstacles_types = ["box"/*, "sphere"*/, "cylinder"], type,
......@@ -511,6 +520,10 @@
"map_size": parseFloat(options.map_size),
"height": parseInt(options.map_height, 10),
"start_AMSL": parseFloat(options.start_AMSL),
"min_lat": parseFloat(min_lat),
"max_lat": parseFloat(max_lat),
"min_lon": parseFloat(min_lon),
"max_lon": parseFloat(max_lon),
"flag_list": [],
"obstacle_list" : [],
"drones": {
......
bt5/erp5_officejs_drone_capture_flag/PathTemplateItem/web_page_module/ojs_drone_capture_flag_script_page_js.xml
View file @ c29f7b4d
......@@ -246,7 +246,7 @@
</item>
<item>
<key> <string>serial</string> </key>
<value> <string>1009.57725.14056.1911</string> </value>
<value> <string>1010.22334.20227.40840</string> </value>
</item>
<item>
<key> <string>state</string> </key>
......@@ -266,7 +266,7 @@
</tuple>
<state>
<tuple>
<float>1689793877.59</float>
<float>1691602475.06</float>
<string>UTC</string>
</tuple>
</state>
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment
GitLab Nexedi Edition | About GitLab | About Nexedi | 沪ICP备2021021310号-2 | 沪ICP备2021021310号-7