Add a Navigate app (requires a Mapbox API token)

This commit is contained in:
James Babcock committed 2026-07-25 23:32:01 -07:00
1 parent 3c2f53ed75
commit bde70ac380
16 files changed
+2096 -4

No files matched your search

+184
View File
@@ -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);
}
}
+237
View File
@@ -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));
}