296 lines
11 KiB
JavaScript
296 lines
11 KiB
JavaScript
|
|
let simulated = false;
|
|
|
|
class Status {
|
|
constructor(path) {
|
|
this.path = path;
|
|
this.on_path = false; // are we on the path or lost ?
|
|
this.position = null; // where we are
|
|
this.cos_direction = null; // cos of where we look at
|
|
this.sin_direction = null; // sin of where we look at
|
|
this.current_segment = null; // which segment is closest
|
|
this.highest_completed_segment = -1; // remember what we already acomplished to disambiguate nearest path when some segments are takend in both directions
|
|
this.reaching = null; // which waypoint are we reaching ?
|
|
this.distance_to_next_point = null; // how far are we from next point ?
|
|
|
|
let r = [0];
|
|
// let's do a reversed prefix computations on all distances:
|
|
// loop on all segments in reversed order
|
|
let previous_point = null;
|
|
for (let i = this.path.len - 1; i >= 0; i--) {
|
|
let point = this.path.point(i);
|
|
if (previous_point !== null) {
|
|
r.unshift(r[0] + point.distance(previous_point));
|
|
}
|
|
previous_point = point;
|
|
}
|
|
this.remaining_distances = r; // how much distance remains at start of each segment
|
|
}
|
|
update_position(new_position, direction) {
|
|
|
|
if (Bangle.isLocked() && this.position !== null && new_position.lon == this.position.lon && new_position.lat == this.position.lat) {
|
|
return;
|
|
}
|
|
|
|
this.cos_direction = Math.cos(-direction - Math.PI / 2.0);
|
|
this.sin_direction = Math.sin(-direction - Math.PI / 2.0);
|
|
this.position = new_position;
|
|
|
|
// detect segment we are on now
|
|
let next_segment = this.path.nearest_segment(this.position, Math.max(0, this.current_segment-1), Math.min(this.current_segment+2, path.len - 1));
|
|
|
|
if (this.is_lost(next_segment)) {
|
|
// it did not work, try anywhere
|
|
next_segment = this.path.nearest_segment(this.position, 0, path.len - 1);
|
|
}
|
|
// now check if we strayed away from path or back to it
|
|
let lost = this.is_lost(next_segment);
|
|
if (this.on_path == lost) {
|
|
if (lost) {
|
|
Bangle.buzz(); // we lost path
|
|
setTimeout(()=>Bangle.buzz(), 300);
|
|
} else {
|
|
Bangle.buzz(); // we found path back
|
|
}
|
|
this.on_path = !lost;
|
|
}
|
|
|
|
if (this.current_segment != next_segment) {
|
|
if (this.current_segment == next_segment - 1) {
|
|
this.highest_completed_segment = this.current_segment;
|
|
}
|
|
}
|
|
this.current_segment = next_segment;
|
|
|
|
// check if we are nearing the next point on our path and alert the user
|
|
let next_point = this.current_segment + 1;
|
|
this.distance_to_next_point = Math.ceil(this.position.distance(this.path.point(next_point)));
|
|
if (this.reaching != next_point && this.distance_to_next_point <= 20) {
|
|
this.reaching = next_point;
|
|
Bangle.buzz();
|
|
}
|
|
|
|
// re-display unless locked
|
|
if (!Bangle.isLocked()) {
|
|
this.display();
|
|
}
|
|
}
|
|
remaining_distance() {
|
|
return this.remaining_distances[this.current_segment+1] + this.position.distance(this.path.point(this.current_segment+1));
|
|
}
|
|
is_lost(segment) {
|
|
let distance_to_nearest = this.position.fake_distance_to_segment(this.path.point(segment), this.path.point(segment+1));
|
|
let meters = 6371e3 * distance_to_nearest;
|
|
return (meters > 20);
|
|
}
|
|
display() {
|
|
g.clear();
|
|
this.display_map();
|
|
this.display_stats();
|
|
Bangle.drawWidgets();
|
|
}
|
|
display_stats() {
|
|
let rounded_distance = Math.round(this.remaining_distance() / 100) / 10;
|
|
let total = Math.round(this.remaining_distances[0] / 100) / 10;
|
|
let now = new Date();
|
|
let minutes = now.getMinutes().toString();
|
|
if (minutes.length < 2) {
|
|
minutes = '0' + minutes;
|
|
}
|
|
let hours = now.getHours().toString();
|
|
g.setFont("6x8:2").drawString(hours + ":" + minutes, 0, g.getHeight() - 49);
|
|
g.drawString("d. " + rounded_distance + "/" + total, 0, g.getHeight() - 32);
|
|
g.drawString("seg." + (this.current_segment + 1) + "/" + path.len + " " + this.distance_to_next_point + "m", 0, g.getHeight() - 15);
|
|
}
|
|
display_map() {
|
|
// don't display all segments, only those neighbouring current segment
|
|
// this is most likely to be the correct display
|
|
// while lowering the cost a lot
|
|
let start = Math.max(this.current_segment - 5, 0);
|
|
let end = Math.min(this.current_segment + 6, this.path.len - 1);
|
|
let pos = this.position;
|
|
let cos = this.cos_direction;
|
|
let sin = this.sin_direction;
|
|
|
|
// segments
|
|
this.path.on_segments(function(p1, p2, i) {
|
|
if (i == this.current_segment + 1) {
|
|
g.setColor(0.0, 1.0, 0.0);
|
|
} else {
|
|
g.setColor(1.0, 0.0, 0.0);
|
|
}
|
|
let c1 = p1.coordinates(pos, cos, sin);
|
|
let c2 = p2.coordinates(pos, cos, sin);
|
|
g.drawLine(c1[0], c1[1], c2[0], c2[1]);
|
|
}, start, end);
|
|
|
|
// waypoints
|
|
for (let i = start ; i < end + 1 ; i++) {
|
|
let p = this.path.point(i);
|
|
let c = p.coordinates(pos, cos, sin);
|
|
g.setColor(g.theme.fg);
|
|
g.fillCircle(c[0], c[1], 4);
|
|
g.setColor(g.theme.bg);
|
|
g.fillCircle(c[0], c[1], 3);
|
|
}
|
|
|
|
// now display ourselves
|
|
g.setColor(g.theme.fgH);
|
|
g.fillCircle(g.getWidth() / 2, g.getHeight() / 2, 5);
|
|
}
|
|
}
|
|
|
|
class Path {
|
|
constructor(filename) {
|
|
let buffer = require("Storage").readArrayBuffer(filename);
|
|
this.points = Float64Array(buffer);
|
|
}
|
|
|
|
// execute op on all segments.
|
|
// start is index of first wanted segment
|
|
// end is 1 after index of last wanted segment
|
|
on_segments(op, start, end) {
|
|
let previous_point = null;
|
|
for (let i = start; i < end + 1; i++) {
|
|
let point = new Point(this.points[2 * i], this.points[2 * i + 1]);
|
|
if (previous_point !== null) {
|
|
op(previous_point, point, i);
|
|
}
|
|
previous_point = point;
|
|
}
|
|
}
|
|
|
|
// return point at given index
|
|
point(index) {
|
|
let lon = this.points[2 * index];
|
|
let lat = this.points[2 * index + 1];
|
|
return new Point(lon, lat);
|
|
}
|
|
|
|
// return index of segment which is nearest from point
|
|
nearest_segment(point, start, end) {
|
|
let min_index = 0;
|
|
let min_distance = Number.MAX_VALUE;
|
|
this.on_segments(function(p1, p2, i) {
|
|
let distance = point.fake_distance_to_segment(p1, p2);
|
|
if (distance <= min_distance) {
|
|
min_distance = distance;
|
|
min_index = i - 1;
|
|
}
|
|
}, start, end);
|
|
return min_index;
|
|
}
|
|
get len() {
|
|
return this.points.length / 2;
|
|
}
|
|
}
|
|
|
|
class Point {
|
|
constructor(lon, lat) {
|
|
this.lon = lon;
|
|
this.lat = lat;
|
|
}
|
|
coordinates(current_position, cos_direction, sin_direction) {
|
|
let translated = this.minus(current_position).times(20000.0);
|
|
let rotated_x = translated.lon * cos_direction - translated.lat * sin_direction;
|
|
let rotated_y = translated.lon * sin_direction + translated.lat * cos_direction;
|
|
return [
|
|
g.getWidth() / 2 - Math.round(rotated_x), // x is inverted
|
|
g.getHeight() / 2 + Math.round(rotated_y)
|
|
];
|
|
}
|
|
minus(other_point) {
|
|
let xdiff = this.lon - other_point.lon;
|
|
let ydiff = this.lat - other_point.lat;
|
|
return new Point(xdiff, ydiff);
|
|
}
|
|
plus(other_point) {
|
|
return new Point(this.lon + other_point.lon, this.lat + other_point.lat);
|
|
}
|
|
length_squared(other_point) {
|
|
let d = this.minus(other_point);
|
|
return (d.lon * d.lon + d.lat * d.lat);
|
|
}
|
|
times(scalar) {
|
|
return new Point(this.lon * scalar, this.lat * scalar);
|
|
}
|
|
dot(other_point) {
|
|
return this.lon * other_point.lon + this.lat * other_point.lat;
|
|
}
|
|
distance(other_point) {
|
|
//see https://www.movable-type.co.uk/scripts/latlong.html
|
|
const R = 6371e3; // metres
|
|
const phi1 = this.lat * Math.PI / 180;
|
|
const phi2 = other_point.lat * Math.PI / 180;
|
|
const deltaphi = (other_point.lat - this.lat) * Math.PI / 180;
|
|
const deltalambda = (other_point.lon - this.lon) * Math.PI / 180;
|
|
|
|
const a = Math.sin(deltaphi / 2) * Math.sin(deltaphi / 2) +
|
|
Math.cos(phi1) * Math.cos(phi2) *
|
|
Math.sin(deltalambda / 2) * Math.sin(deltalambda / 2);
|
|
const c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1 - a));
|
|
|
|
return R * c; // in metres
|
|
}
|
|
fake_distance(other_point) {
|
|
return Math.sqrt(this.length_squared(other_point));
|
|
}
|
|
fake_distance_to_segment(v, w) {
|
|
// from : https://stackoverflow.com/questions/849211/shortest-distance-between-a-point-and-a-line-segment
|
|
// Return minimum distance between line segment vw and point p
|
|
let l2 = v.length_squared(w); // i.e. |w-v|^2 - avoid a sqrt
|
|
if (l2 == 0.0) {
|
|
return this.distance(v); // v == w case
|
|
}
|
|
// Consider the line extending the segment, parameterized as v + t (w - v).
|
|
// We find projection of point p onto the line.
|
|
// It falls where t = [(p-v) . (w-v)] / |w-v|^2
|
|
// We clamp t from [0,1] to handle points outside the segment vw.
|
|
let t = Math.max(0, Math.min(1, (this.minus(v)).dot(w.minus(v)) / l2));
|
|
|
|
let projection = v.plus((w.minus(v)).times(t)); // Projection falls on the segment
|
|
return this.fake_distance(projection);
|
|
}
|
|
}
|
|
|
|
|
|
Bangle.loadWidgets();
|
|
let path = new Path("test.gpc");
|
|
let status = new Status(path);
|
|
|
|
function set_coordinates(data) {
|
|
let valid_coordinates = !isNaN(data.lat) && !isNaN(data.lon);
|
|
if (valid_coordinates) {
|
|
let direction = data.course * Math.PI / 180.0;
|
|
let position = new Point(data.lon, data.lat);
|
|
status.update_position(position, direction);
|
|
}
|
|
}
|
|
|
|
let fake_gps_point = 0.0;
|
|
function simulate_gps(status) {
|
|
let point_index = Math.floor(fake_gps_point);
|
|
if (point_index >= path.len) {
|
|
return;
|
|
}
|
|
let p1 = path.point(point_index);
|
|
let p2 = path.point(point_index + 1);
|
|
|
|
let alpha = fake_gps_point - point_index;
|
|
let pos = p1.times(1-alpha).plus(p2.times(alpha));
|
|
let old_pos = status.position;
|
|
|
|
fake_gps_point += 0.05; // advance simulation
|
|
let direction = Math.atan2(pos.lat - old_pos.lat, pos.lon - old_pos.lon);
|
|
status.update_position(pos, direction);
|
|
}
|
|
|
|
|
|
if (simulated) {
|
|
status.position = new Point(status.path.point(0));
|
|
setInterval(simulate_gps, 500, status);
|
|
} else {
|
|
Bangle.setGPSPower(true, "gipy");
|
|
Bangle.on('GPS', set_coordinates);
|
|
}
|