forked from Blendman974/faceclaw
Add a Navigate app (requires a Mapbox API token)
This commit is contained in:
1 parent
3c2f53ed75
commit
bde70ac380
16 files changed
+2096
-4
No files matched your search
@@ -0,0 +1,184 @@
|
||||
/**
|
||||
* Maneuver arrow glyphs for turn-by-turn guidance, drawn with GrayImage
|
||||
* primitives from the Directions maneuver type + modifier (no image assets).
|
||||
*/
|
||||
import { GrayImage } from "../graphics/image";
|
||||
|
||||
/**
|
||||
* Draw the glyph for a maneuver into a size x size box at (x, y).
|
||||
* Roundabout exit numbers are the caller's job (drawn as text next to it).
|
||||
*/
|
||||
export function drawManeuverGlyph(
|
||||
image: GrayImage,
|
||||
x: number,
|
||||
y: number,
|
||||
size: number,
|
||||
maneuverType: string,
|
||||
modifier: string,
|
||||
value = 245,
|
||||
): void {
|
||||
const type = maneuverType.toLowerCase();
|
||||
if (type === "arrive") {
|
||||
drawArriveFlag(image, x, y, size, value);
|
||||
return;
|
||||
}
|
||||
if (type === "roundabout" || type === "rotary" || type === "roundabout turn" || type === "exit roundabout" || type === "exit rotary") {
|
||||
drawRoundabout(image, x, y, size, value);
|
||||
return;
|
||||
}
|
||||
drawArrow(image, x, y, size, modifier.toLowerCase(), value);
|
||||
}
|
||||
|
||||
/** Normalized path points (0..1 box coords, y down) for each arrow shape. */
|
||||
function arrowPath(modifier: string): Array<[number, number]> {
|
||||
switch (modifier) {
|
||||
case "uturn":
|
||||
return [
|
||||
[0.68, 0.92],
|
||||
[0.68, 0.42],
|
||||
[0.62, 0.28],
|
||||
[0.5, 0.22],
|
||||
[0.38, 0.28],
|
||||
[0.32, 0.42],
|
||||
[0.32, 0.88],
|
||||
];
|
||||
case "sharp left":
|
||||
return [
|
||||
[0.6, 0.92],
|
||||
[0.6, 0.42],
|
||||
[0.2, 0.72],
|
||||
];
|
||||
case "left":
|
||||
return [
|
||||
[0.62, 0.92],
|
||||
[0.62, 0.42],
|
||||
[0.14, 0.42],
|
||||
];
|
||||
case "slight left":
|
||||
return [
|
||||
[0.56, 0.92],
|
||||
[0.56, 0.5],
|
||||
[0.32, 0.16],
|
||||
];
|
||||
case "sharp right":
|
||||
return [
|
||||
[0.4, 0.92],
|
||||
[0.4, 0.42],
|
||||
[0.8, 0.72],
|
||||
];
|
||||
case "right":
|
||||
return [
|
||||
[0.38, 0.92],
|
||||
[0.38, 0.42],
|
||||
[0.86, 0.42],
|
||||
];
|
||||
case "slight right":
|
||||
return [
|
||||
[0.44, 0.92],
|
||||
[0.44, 0.5],
|
||||
[0.68, 0.16],
|
||||
];
|
||||
case "straight":
|
||||
default:
|
||||
return [
|
||||
[0.5, 0.92],
|
||||
[0.5, 0.12],
|
||||
];
|
||||
}
|
||||
}
|
||||
|
||||
function drawArrow(image: GrayImage, x: number, y: number, size: number, modifier: string, value: number): void {
|
||||
const path = arrowPath(modifier).map(([px, py]): [number, number] => [x + px * size, y + py * size]);
|
||||
const thickness = Math.max(2, Math.round(size / 14));
|
||||
for (let i = 1; i < path.length; i++) {
|
||||
drawThickLine(image, path[i - 1]!, path[i]!, thickness, value);
|
||||
}
|
||||
const tip = path[path.length - 1]!;
|
||||
const prev = path[path.length - 2]!;
|
||||
drawArrowHead(image, prev, tip, Math.max(5, size * 0.22), thickness, value);
|
||||
}
|
||||
|
||||
function drawRoundabout(image: GrayImage, x: number, y: number, size: number, value: number): void {
|
||||
const cx = x + size * 0.5;
|
||||
const cy = y + size * 0.58;
|
||||
const radius = size * 0.24;
|
||||
const thickness = Math.max(2, Math.round(size / 14));
|
||||
// Circle from line segments.
|
||||
const segments = 20;
|
||||
let last: [number, number] | null = null;
|
||||
for (let i = 0; i <= segments; i++) {
|
||||
const angle = (i / segments) * Math.PI * 2;
|
||||
const point: [number, number] = [cx + radius * Math.cos(angle), cy + radius * Math.sin(angle)];
|
||||
if (last) drawThickLine(image, last, point, thickness, value);
|
||||
last = point;
|
||||
}
|
||||
// Exit arrow upward out of the circle.
|
||||
const from: [number, number] = [cx, cy - radius];
|
||||
const to: [number, number] = [cx, y + size * 0.08];
|
||||
drawThickLine(image, from, to, thickness, value);
|
||||
drawArrowHead(image, from, to, Math.max(5, size * 0.2), thickness, value);
|
||||
}
|
||||
|
||||
function drawArriveFlag(image: GrayImage, x: number, y: number, size: number, value: number): void {
|
||||
const poleX = Math.round(x + size * 0.36);
|
||||
const top = Math.round(y + size * 0.12);
|
||||
const bottom = Math.round(y + size * 0.92);
|
||||
const thickness = Math.max(2, Math.round(size / 16));
|
||||
drawThickLine(image, [poleX, top], [poleX, bottom], thickness, value);
|
||||
// Flag: two stacked rows of alternating blocks reads as checkered at 4bpp.
|
||||
const flagWidth = Math.round(size * 0.42);
|
||||
const rowHeight = Math.max(3, Math.round(size * 0.11));
|
||||
const blocks = 3;
|
||||
const blockWidth = Math.max(3, Math.round(flagWidth / blocks));
|
||||
for (let row = 0; row < 2; row++) {
|
||||
for (let block = 0; block < blocks; block++) {
|
||||
const bright = (row + block) % 2 === 0;
|
||||
image.fillRect(
|
||||
poleX + thickness + block * blockWidth,
|
||||
top + row * rowHeight,
|
||||
blockWidth,
|
||||
rowHeight,
|
||||
bright ? value : Math.round(value * 0.35),
|
||||
);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
function drawArrowHead(
|
||||
image: GrayImage,
|
||||
from: [number, number],
|
||||
tip: [number, number],
|
||||
headLength: number,
|
||||
thickness: number,
|
||||
value: number,
|
||||
): void {
|
||||
const angle = Math.atan2(tip[1] - from[1], tip[0] - from[0]);
|
||||
const spread = Math.PI / 5.5;
|
||||
for (const side of [-1, 1]) {
|
||||
const barbAngle = angle + Math.PI + side * spread;
|
||||
const barb: [number, number] = [
|
||||
tip[0] + headLength * Math.cos(barbAngle),
|
||||
tip[1] + headLength * Math.sin(barbAngle),
|
||||
];
|
||||
drawThickLine(image, tip, barb, thickness, value);
|
||||
}
|
||||
}
|
||||
|
||||
function drawThickLine(
|
||||
image: GrayImage,
|
||||
from: [number, number],
|
||||
to: [number, number],
|
||||
thickness: number,
|
||||
value: number,
|
||||
): void {
|
||||
// Stamp a small square along the line; plenty at glyph scale.
|
||||
const dx = to[0] - from[0];
|
||||
const dy = to[1] - from[1];
|
||||
const steps = Math.max(1, Math.ceil(Math.max(Math.abs(dx), Math.abs(dy))));
|
||||
const half = Math.floor(thickness / 2);
|
||||
for (let i = 0; i <= steps; i++) {
|
||||
const px = Math.round(from[0] + (dx * i) / steps);
|
||||
const py = Math.round(from[1] + (dy * i) / steps);
|
||||
image.fillRect(px - half, py - half, thickness, thickness, value);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,237 @@
|
||||
/**
|
||||
* Client-side route following: snap GPS fixes to the route polyline, track
|
||||
* progress toward the next maneuver, and detect off-route/arrival. Pure
|
||||
* geometry over the Directions response — no network, no Android.
|
||||
*/
|
||||
import type { Route } from "../native/mapbox";
|
||||
|
||||
const EARTH_RADIUS_M = 6_371_000;
|
||||
/** Perpendicular distance that counts as off the route. */
|
||||
const OFF_ROUTE_THRESHOLD_M = 50;
|
||||
/** Consecutive off-route fixes before declaring a reroute is needed. */
|
||||
const OFF_ROUTE_FIX_COUNT = 3;
|
||||
/** Within this many meters of the destination counts as arrived. */
|
||||
const ARRIVAL_THRESHOLD_M = 35;
|
||||
/** How far behind the last known progress the snap search may look. */
|
||||
const SNAP_BACKTRACK_SEGMENTS = 8;
|
||||
/** How far ahead of the last known progress the snap search may look. */
|
||||
const SNAP_LOOKAHEAD_SEGMENTS = 120;
|
||||
|
||||
export type RouteProgress = {
|
||||
/** Snapped position [lon, lat]. */
|
||||
snapped: [number, number];
|
||||
/** Meters traveled along the route. */
|
||||
alongMeters: number;
|
||||
/** Perpendicular meters from the fix to the route. */
|
||||
crossTrackMeters: number;
|
||||
/** Index of the upcoming maneuver in route.steps (its instruction is what to show). */
|
||||
nextStepIndex: number;
|
||||
/** Meters until the upcoming maneuver. */
|
||||
metersToNextManeuver: number;
|
||||
remainingMeters: number;
|
||||
remainingSec: number;
|
||||
offRoute: boolean;
|
||||
arrived: boolean;
|
||||
};
|
||||
|
||||
export class RouteFollower {
|
||||
readonly route: Route;
|
||||
/** Cumulative meters from the origin to each geometry vertex. */
|
||||
private readonly vertexMeters: number[];
|
||||
/** Meters along the route at which each step's maneuver occurs. */
|
||||
private readonly stepStartMeters: number[];
|
||||
private readonly totalMeters: number;
|
||||
private lastSegmentIndex = 0;
|
||||
private offRouteStreak = 0;
|
||||
|
||||
constructor(route: Route) {
|
||||
this.route = route;
|
||||
this.vertexMeters = new Array(route.coordinates.length);
|
||||
let along = 0;
|
||||
this.vertexMeters[0] = 0;
|
||||
for (let i = 1; i < route.coordinates.length; i++) {
|
||||
along += haversineMeters(route.coordinates[i - 1]!, route.coordinates[i]!);
|
||||
this.vertexMeters[i] = along;
|
||||
}
|
||||
this.totalMeters = along;
|
||||
|
||||
// Each step's maneuver happens where the previous step's travel ends, so
|
||||
// the maneuver point of step k sits at the sum of steps 0..k-1 distances.
|
||||
// (Slight drift vs the geometry is fine at guidance granularity.)
|
||||
this.stepStartMeters = new Array(route.steps.length);
|
||||
let stepAlong = 0;
|
||||
for (let i = 0; i < route.steps.length; i++) {
|
||||
this.stepStartMeters[i] = Math.min(stepAlong, this.totalMeters);
|
||||
stepAlong += route.steps[i]!.distanceMeters;
|
||||
}
|
||||
}
|
||||
|
||||
/** Ingest a GPS fix and return current guidance state. */
|
||||
update(longitude: number, latitude: number): RouteProgress {
|
||||
const fix: [number, number] = [longitude, latitude];
|
||||
const snap = this.snapToRoute(fix);
|
||||
this.lastSegmentIndex = snap.segmentIndex;
|
||||
|
||||
if (snap.crossTrackMeters > OFF_ROUTE_THRESHOLD_M) {
|
||||
this.offRouteStreak++;
|
||||
} else {
|
||||
this.offRouteStreak = 0;
|
||||
}
|
||||
|
||||
const remainingMeters = Math.max(0, this.totalMeters - snap.alongMeters);
|
||||
const arrived =
|
||||
remainingMeters < ARRIVAL_THRESHOLD_M &&
|
||||
haversineMeters(fix, this.route.coordinates[this.route.coordinates.length - 1]!) <
|
||||
ARRIVAL_THRESHOLD_M * 2;
|
||||
|
||||
// The next maneuver is the first one still ahead of us. Step 0 is
|
||||
// "depart", which is behind us as soon as we move; arrival is last.
|
||||
let nextStepIndex = this.route.steps.length - 1;
|
||||
for (let i = 1; i < this.route.steps.length; i++) {
|
||||
if (this.stepStartMeters[i]! > snap.alongMeters + 5) {
|
||||
nextStepIndex = i;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
return {
|
||||
snapped: snap.snapped,
|
||||
alongMeters: snap.alongMeters,
|
||||
crossTrackMeters: snap.crossTrackMeters,
|
||||
nextStepIndex,
|
||||
metersToNextManeuver: Math.max(0, this.stepStartMeters[nextStepIndex]! - snap.alongMeters),
|
||||
remainingMeters,
|
||||
remainingSec: this.remainingSeconds(snap.alongMeters, nextStepIndex),
|
||||
offRoute: this.offRouteStreak >= OFF_ROUTE_FIX_COUNT,
|
||||
arrived,
|
||||
};
|
||||
}
|
||||
|
||||
totalRouteMeters(): number {
|
||||
return this.totalMeters;
|
||||
}
|
||||
|
||||
/**
|
||||
* The route geometry from just behind the current position to aheadMeters
|
||||
* past it — the slice worth drawing on a follow-mode map.
|
||||
*/
|
||||
routeSliceAround(alongMeters: number, behindMeters: number, aheadMeters: number): Array<[number, number]> {
|
||||
const from = Math.max(0, alongMeters - behindMeters);
|
||||
const to = Math.min(this.totalMeters, alongMeters + aheadMeters);
|
||||
const slice: Array<[number, number]> = [];
|
||||
for (let i = 0; i < this.route.coordinates.length; i++) {
|
||||
const at = this.vertexMeters[i]!;
|
||||
if (at < from) continue;
|
||||
if (at > to) break;
|
||||
slice.push(this.route.coordinates[i]!);
|
||||
}
|
||||
return slice.length >= 2 ? slice : this.route.coordinates.slice(0, 2);
|
||||
}
|
||||
|
||||
/**
|
||||
* Nearest point on the polyline, biased to search near the last known
|
||||
* segment first so parallel roads and overlaps resolve forward along the
|
||||
* route. Falls back to a whole-route search when the local window misses.
|
||||
*/
|
||||
private snapToRoute(fix: [number, number]): {
|
||||
snapped: [number, number];
|
||||
alongMeters: number;
|
||||
crossTrackMeters: number;
|
||||
segmentIndex: number;
|
||||
} {
|
||||
const windowStart = Math.max(0, this.lastSegmentIndex - SNAP_BACKTRACK_SEGMENTS);
|
||||
const windowEnd = Math.min(
|
||||
this.route.coordinates.length - 2,
|
||||
this.lastSegmentIndex + SNAP_LOOKAHEAD_SEGMENTS,
|
||||
);
|
||||
const local = this.nearestOnSegments(fix, windowStart, windowEnd);
|
||||
if (local && local.crossTrackMeters <= OFF_ROUTE_THRESHOLD_M * 2) {
|
||||
return local;
|
||||
}
|
||||
const global = this.nearestOnSegments(fix, 0, this.route.coordinates.length - 2);
|
||||
return global ?? local ?? {
|
||||
snapped: this.route.coordinates[0]!,
|
||||
alongMeters: 0,
|
||||
crossTrackMeters: Number.POSITIVE_INFINITY,
|
||||
segmentIndex: 0,
|
||||
};
|
||||
}
|
||||
|
||||
private nearestOnSegments(
|
||||
fix: [number, number],
|
||||
firstSegment: number,
|
||||
lastSegment: number,
|
||||
): { snapped: [number, number]; alongMeters: number; crossTrackMeters: number; segmentIndex: number } | null {
|
||||
let best: { snapped: [number, number]; alongMeters: number; crossTrackMeters: number; segmentIndex: number } | null =
|
||||
null;
|
||||
// Planar approximation in a locally scaled lon/lat frame; fine for
|
||||
// segment-scale distances.
|
||||
const cosLat = Math.cos((fix[1] * Math.PI) / 180);
|
||||
for (let i = firstSegment; i <= lastSegment; i++) {
|
||||
const a = this.route.coordinates[i]!;
|
||||
const b = this.route.coordinates[i + 1]!;
|
||||
const ax = (a[0] - fix[0]) * cosLat;
|
||||
const ay = a[1] - fix[1];
|
||||
const bx = (b[0] - fix[0]) * cosLat;
|
||||
const by = b[1] - fix[1];
|
||||
const dx = bx - ax;
|
||||
const dy = by - ay;
|
||||
const lengthSq = dx * dx + dy * dy;
|
||||
const t = lengthSq > 0 ? clamp01(-(ax * dx + ay * dy) / lengthSq) : 0;
|
||||
const px = ax + t * dx;
|
||||
const py = ay + t * dy;
|
||||
const distanceMeters = Math.sqrt(px * px + py * py) * ((Math.PI / 180) * EARTH_RADIUS_M);
|
||||
if (best === null || distanceMeters < best.crossTrackMeters) {
|
||||
const segmentLength = this.vertexMeters[i + 1]! - this.vertexMeters[i]!;
|
||||
best = {
|
||||
snapped: [a[0] + t * (b[0] - a[0]), a[1] + t * (b[1] - a[1])],
|
||||
alongMeters: this.vertexMeters[i]! + t * segmentLength,
|
||||
crossTrackMeters: distanceMeters,
|
||||
segmentIndex: i,
|
||||
};
|
||||
}
|
||||
}
|
||||
return best;
|
||||
}
|
||||
|
||||
private remainingSeconds(alongMeters: number, nextStepIndex: number): number {
|
||||
// Whole steps still ahead, plus a proportional share of the step we're in.
|
||||
let seconds = 0;
|
||||
for (let i = nextStepIndex; i < this.route.steps.length; i++) {
|
||||
seconds += this.route.steps[i]!.durationSec;
|
||||
}
|
||||
const currentStep = this.route.steps[nextStepIndex - 1];
|
||||
if (currentStep && currentStep.distanceMeters > 0) {
|
||||
const stepStart = this.stepStartMeters[nextStepIndex - 1]!;
|
||||
const fractionRemaining = clamp01(1 - (alongMeters - stepStart) / currentStep.distanceMeters);
|
||||
seconds += currentStep.durationSec * fractionRemaining;
|
||||
}
|
||||
return seconds;
|
||||
}
|
||||
}
|
||||
|
||||
export function haversineMeters(a: [number, number], b: [number, number]): number {
|
||||
const dLat = ((b[1] - a[1]) * Math.PI) / 180;
|
||||
const dLon = ((b[0] - a[0]) * Math.PI) / 180;
|
||||
const lat1 = (a[1] * Math.PI) / 180;
|
||||
const lat2 = (b[1] * Math.PI) / 180;
|
||||
const h =
|
||||
Math.sin(dLat / 2) * Math.sin(dLat / 2) +
|
||||
Math.cos(lat1) * Math.cos(lat2) * Math.sin(dLon / 2) * Math.sin(dLon / 2);
|
||||
return 2 * EARTH_RADIUS_M * Math.asin(Math.min(1, Math.sqrt(h)));
|
||||
}
|
||||
|
||||
/** Initial bearing (degrees clockwise from north) from a to b. */
|
||||
export function bearingDegrees(a: [number, number], b: [number, number]): number {
|
||||
const lat1 = (a[1] * Math.PI) / 180;
|
||||
const lat2 = (b[1] * Math.PI) / 180;
|
||||
const dLon = ((b[0] - a[0]) * Math.PI) / 180;
|
||||
const y = Math.sin(dLon) * Math.cos(lat2);
|
||||
const x = Math.cos(lat1) * Math.sin(lat2) - Math.sin(lat1) * Math.cos(lat2) * Math.cos(dLon);
|
||||
const degrees = (Math.atan2(y, x) * 180) / Math.PI;
|
||||
return (degrees + 360) % 360;
|
||||
}
|
||||
|
||||
function clamp01(value: number): number {
|
||||
return Math.max(0, Math.min(1, value));
|
||||
}
|
||||
Reference in new issue
Block a user