290 lines
12 KiB
TypeScript
290 lines
12 KiB
TypeScript
import type { TelemetryPoint } from './telemetry-export';
|
|
|
|
const numeric = (value: unknown): number | undefined => {
|
|
if (value == null || value === '') return undefined;
|
|
const n = Number(value);
|
|
return Number.isFinite(n) ? n : undefined;
|
|
};
|
|
|
|
/** Recorded samples only. Invalid GPS, explicit segments and time gaps break geometry. */
|
|
export function normalizeTelemetryPoints(samples: unknown[]): TelemetryPoint[] {
|
|
const valid: TelemetryPoint[] = [];
|
|
let sourceSegment = 0;
|
|
let lastExplicitSegment: number | undefined;
|
|
for (const sample of samples) {
|
|
if (!sample || typeof sample !== 'object') { sourceSegment++; continue; }
|
|
const p = sample as Record<string, unknown>;
|
|
const latitude = numeric(p.latitude), longitude = numeric(p.longitude);
|
|
const time = typeof p.timestamp === 'string' ? Date.parse(p.timestamp) : NaN;
|
|
if (latitude == null || longitude == null || Math.abs(latitude) > 90 || Math.abs(longitude) > 180 || !Number.isFinite(time)) {
|
|
sourceSegment++;
|
|
continue;
|
|
}
|
|
const value = (key: string) => numeric(p[key]) ?? 0;
|
|
const explicitSegment = numeric(p.segmentId);
|
|
if (valid.length && explicitSegment !== lastExplicitSegment) sourceSegment++;
|
|
lastExplicitSegment = explicitSegment;
|
|
valid.push({ timestamp: new Date(time).toISOString(), latitude, longitude,
|
|
altitude: value('altitude'), speed: value('speed'), heading: value('heading'), gpsAccuracy: value('gpsAccuracy'),
|
|
leanAngle: value('leanAngle'), pitchAngle: value('pitchAngle'), accelX: value('accelX'), accelY: value('accelY'), accelZ: value('accelZ'),
|
|
longitudinalG: value('longitudinalG'), lateralG: value('lateralG'),
|
|
segmentId: sourceSegment });
|
|
}
|
|
valid.sort((a,b) => Date.parse(a.timestamp) - Date.parse(b.timestamp));
|
|
let segment = 0;
|
|
return valid.map((point, i) => {
|
|
const previous = valid[i-1];
|
|
if (previous && (previous.segmentId !== point.segmentId || Date.parse(point.timestamp) - Date.parse(previous.timestamp) > 5000)) segment++;
|
|
return {...point, segmentId:segment};
|
|
});
|
|
}
|
|
|
|
export function areTelemetryPointsConnected(a: TelemetryPoint, b: TelemetryPoint): boolean {
|
|
const delta = Date.parse(b.timestamp) - Date.parse(a.timestamp);
|
|
return a.segmentId === b.segmentId && delta >= 0 && delta <= 5000;
|
|
}
|
|
|
|
/**
|
|
* Advances a replay cursor by a budget of virtual milliseconds, honoring the real time
|
|
* between recorded samples so the rider marker stays synchronized with the elapsed-time
|
|
* readout (which is derived from timestamps). Playback is therefore wall-clock accurate at
|
|
* 1x and correctly scaled at other speeds, even when samples are non-uniform.
|
|
*
|
|
* Disconnected transitions (segment breaks or GPS loss, per areTelemetryPointsConnected)
|
|
* are compressed to a single nominal 20 Hz step (50 ms) so playback never stalls across
|
|
* dead time instead of waiting out a multi-second gap. No coordinates are altered.
|
|
*
|
|
* Returns the new index and the leftover budget to carry into the next frame.
|
|
*/
|
|
export function advancePlaybackIndex(
|
|
points: TelemetryPoint[],
|
|
fromIndex: number,
|
|
budgetMs: number
|
|
): { index: number; remainingMs: number } {
|
|
if (points.length === 0) return { index: 0, remainingMs: 0 };
|
|
const i = Math.max(0, Math.min(fromIndex, points.length - 1));
|
|
if (!Number.isFinite(budgetMs) || budgetMs <= 0) {
|
|
return { index: i, remainingMs: 0 };
|
|
}
|
|
let budget = budgetMs;
|
|
let curr = i;
|
|
while (curr < points.length - 1) {
|
|
const connected = areTelemetryPointsConnected(points[curr], points[curr + 1]);
|
|
const rawDt = connected
|
|
? Date.parse(points[curr + 1].timestamp) - Date.parse(points[curr].timestamp)
|
|
: 50;
|
|
const dt = Number.isFinite(rawDt) ? Math.max(0, rawDt) : 50;
|
|
if (budget >= dt) {
|
|
budget -= dt;
|
|
curr++;
|
|
} else {
|
|
break;
|
|
}
|
|
}
|
|
return { index: curr, remainingMs: curr < points.length - 1 ? budget : 0 };
|
|
}
|
|
|
|
/**
|
|
* Interpolates heading degrees between two orientations using the shortest angular path,
|
|
* correctly handling the 359° to 0° (and 0° to 359°) wraparound boundary without full rotations.
|
|
* Returns a normalized angle in [0, 360).
|
|
*/
|
|
export function interpolateHeading(fromHeading: number, toHeading: number, t: number): number {
|
|
if (!Number.isFinite(fromHeading) && !Number.isFinite(toHeading)) return 0;
|
|
if (!Number.isFinite(fromHeading)) return ((toHeading % 360) + 360) % 360;
|
|
if (!Number.isFinite(toHeading)) return ((fromHeading % 360) + 360) % 360;
|
|
|
|
const from = ((fromHeading % 360) + 360) % 360;
|
|
const to = ((toHeading % 360) + 360) % 360;
|
|
const clampedT = Math.max(0, Math.min(1, Number.isFinite(t) ? t : 0));
|
|
|
|
// Shortest angular difference in [-180, 180)
|
|
const diff = ((to - from + 540) % 360) - 180;
|
|
const result = (from + clampedT * diff) % 360;
|
|
return (result + 360) % 360;
|
|
}
|
|
|
|
/**
|
|
* Calculates the interpolated geographic position (latitude, longitude) and heading at a given
|
|
* playback point index and fractional sub-sample millisecond offset.
|
|
*
|
|
* If points are disconnected (e.g. across a GPS loss gap or segment boundary), the position
|
|
* stays at the start point rather than interpolating false coordinates across dead space.
|
|
* Clamps safely to points array bounds and handles non-finite values and duplicate timestamps.
|
|
*/
|
|
export function interpolateTelemetryPosition(
|
|
points: TelemetryPoint[],
|
|
index: number,
|
|
remainingMs: number
|
|
): { latitude: number; longitude: number; heading: number } {
|
|
if (!points || points.length === 0) {
|
|
return { latitude: 0, longitude: 0, heading: 0 };
|
|
}
|
|
const i = Math.max(0, Math.min(index, points.length - 1));
|
|
const p1 = points[i];
|
|
const p1Lat = Number.isFinite(p1.latitude) ? p1.latitude : 0;
|
|
const p1Lng = Number.isFinite(p1.longitude) ? p1.longitude : 0;
|
|
const p1Heading = Number.isFinite(p1.heading) ? ((p1.heading % 360) + 360) % 360 : 0;
|
|
|
|
if (i >= points.length - 1 || !points[i + 1]) {
|
|
return {
|
|
latitude: p1Lat,
|
|
longitude: p1Lng,
|
|
heading: p1Heading,
|
|
};
|
|
}
|
|
|
|
const p2 = points[i + 1];
|
|
if (!areTelemetryPointsConnected(p1, p2)) {
|
|
return {
|
|
latitude: p1Lat,
|
|
longitude: p1Lng,
|
|
heading: p1Heading,
|
|
};
|
|
}
|
|
|
|
const dt = Date.parse(p2.timestamp) - Date.parse(p1.timestamp);
|
|
if (!Number.isFinite(dt) || dt <= 0 || !Number.isFinite(remainingMs) || remainingMs <= 0) {
|
|
return {
|
|
latitude: p1Lat,
|
|
longitude: p1Lng,
|
|
heading: p1Heading,
|
|
};
|
|
}
|
|
|
|
const t = Math.max(0, Math.min(1, remainingMs / dt));
|
|
const p2Lat = Number.isFinite(p2.latitude) ? p2.latitude : p1Lat;
|
|
const p2Lng = Number.isFinite(p2.longitude) ? p2.longitude : p1Lng;
|
|
|
|
const latitude = p1Lat + t * (p2Lat - p1Lat);
|
|
const longitude = p1Lng + t * (p2Lng - p1Lng);
|
|
const heading = interpolateHeading(p1.heading, p2.heading, t);
|
|
|
|
return {
|
|
latitude: Number.isFinite(latitude) ? latitude : p1Lat,
|
|
longitude: Number.isFinite(longitude) ? longitude : p1Lng,
|
|
heading,
|
|
};
|
|
}
|
|
|
|
/**
|
|
* Calculates a fully interpolated TelemetryPoint between two recorded samples.
|
|
* Preserves recorded metadata, interpolates numerical telemetry metrics (speed, lean, G-forces),
|
|
* and avoids interpolating across segment breaks or recording gaps.
|
|
*/
|
|
export function interpolateTelemetryPoint(
|
|
points: TelemetryPoint[],
|
|
index: number,
|
|
remainingMs: number
|
|
): TelemetryPoint {
|
|
if (!points || points.length === 0) {
|
|
return {
|
|
timestamp: new Date(0).toISOString(),
|
|
latitude: 0,
|
|
longitude: 0,
|
|
altitude: 0,
|
|
speed: 0,
|
|
heading: 0,
|
|
gpsAccuracy: 0,
|
|
leanAngle: 0,
|
|
pitchAngle: 0,
|
|
accelX: 0,
|
|
accelY: 0,
|
|
accelZ: 0,
|
|
longitudinalG: 0,
|
|
lateralG: 0,
|
|
segmentId: 0,
|
|
};
|
|
}
|
|
const i = Math.max(0, Math.min(index, points.length - 1));
|
|
const p1 = points[i];
|
|
if (i >= points.length - 1 || !points[i + 1]) return p1;
|
|
|
|
const p2 = points[i + 1];
|
|
if (!areTelemetryPointsConnected(p1, p2)) return p1;
|
|
|
|
const dt = Date.parse(p2.timestamp) - Date.parse(p1.timestamp);
|
|
if (!Number.isFinite(dt) || dt <= 0 || !Number.isFinite(remainingMs) || remainingMs <= 0) return p1;
|
|
|
|
const t = Math.max(0, Math.min(1, remainingMs / dt));
|
|
const t1 = Date.parse(p1.timestamp);
|
|
const interpolatedTime = Number.isFinite(t1) ? new Date(t1 + t * dt).toISOString() : p1.timestamp;
|
|
|
|
const lerp = (v1: number | undefined, v2: number | undefined): number => {
|
|
const a = Number.isFinite(v1) ? (v1 as number) : 0;
|
|
const b = Number.isFinite(v2) ? (v2 as number) : 0;
|
|
return a + t * (b - a);
|
|
};
|
|
|
|
return {
|
|
timestamp: interpolatedTime,
|
|
latitude: lerp(p1.latitude, p2.latitude),
|
|
longitude: lerp(p1.longitude, p2.longitude),
|
|
altitude: lerp(p1.altitude, p2.altitude),
|
|
speed: lerp(p1.speed, p2.speed),
|
|
heading: interpolateHeading(p1.heading, p2.heading, t),
|
|
gpsAccuracy: lerp(p1.gpsAccuracy, p2.gpsAccuracy),
|
|
leanAngle: lerp(p1.leanAngle, p2.leanAngle),
|
|
pitchAngle: lerp(p1.pitchAngle, p2.pitchAngle),
|
|
accelX: lerp(p1.accelX, p2.accelX),
|
|
accelY: lerp(p1.accelY, p2.accelY),
|
|
accelZ: lerp(p1.accelZ, p2.accelZ),
|
|
longitudinalG: lerp(p1.longitudinalG, p2.longitudinalG),
|
|
lateralG: lerp(p1.lateralG, p2.lateralG),
|
|
segmentId: p1.segmentId,
|
|
};
|
|
}
|
|
|
|
/** Leaflet's geographic API is [latitude, longitude], never normalized screen pixels. */
|
|
export function geographicSegments(points: TelemetryPoint[]): [number, number][][] {
|
|
const segments: [number,number][][] = [];
|
|
points.forEach((point,i) => {
|
|
if (!i || !areTelemetryPointsConnected(points[i-1],point)) segments.push([]);
|
|
segments[segments.length-1].push([point.latitude,point.longitude]);
|
|
});
|
|
return segments;
|
|
}
|
|
|
|
function limitPoints(points: TelemetryPoint[], maxPoints: number): TelemetryPoint[] {
|
|
if (points.length <= maxPoints || !Number.isFinite(maxPoints)) return points;
|
|
const stride = Math.max(1, Math.ceil(points.length / Math.max(2,maxPoints)));
|
|
// Retain segment endpoints as well as regular samples; no interpolation or snapping.
|
|
return points.filter((p,i) => i === 0 || i === points.length-1 || i % stride === 0 ||
|
|
!areTelemetryPointsConnected(points[i-1],p) || !areTelemetryPointsConnected(p,points[i+1]));
|
|
}
|
|
|
|
export function parseGpxToTelemetryPoints(xml: string, maxPoints = Infinity): TelemetryPoint[] {
|
|
if (!xml || typeof xml !== 'string') return [];
|
|
const samples: unknown[] = [];
|
|
const prefix = '(?:[\\w.-]+:)?';
|
|
const segments = [...xml.matchAll(new RegExp(`<${prefix}trkseg\\b[^>]*>([\\s\\S]*?)<\\/${prefix}trkseg\\s*>`, 'gi'))];
|
|
const blocks = segments.length ? segments.map(m=>m[1]) : [xml];
|
|
blocks.forEach((block,segmentId) => {
|
|
const regex = new RegExp(`<${prefix}trkpt\\b([^>]*?)(?:\\/>|>([\\s\\S]*?)<\\/${prefix}trkpt\\s*>)`, 'gi');
|
|
for (const match of block.matchAll(regex)) {
|
|
const attr = (name: string) => new RegExp(`\\b${name}\\s*=\\s*["']([^"']+)["']`,'i').exec(match[1])?.[1];
|
|
const inner = (name: string) => new RegExp(`<${prefix}${name}\\b[^>]*>([\\s\\S]*?)<\\/${prefix}${name}\\s*>`,'i').exec(match[2] || '')?.[1].trim();
|
|
samples.push({ latitude:attr('lat'), longitude:attr('lon'), timestamp:inner('time'), altitude:inner('ele'), speed:inner('speed'),
|
|
heading:inner('heading'), leanAngle:inner('lean'), longitudinalG:inner('longitudinalG'), lateralG:inner('lateralG'), segmentId });
|
|
}
|
|
});
|
|
return limitPoints(normalizeTelemetryPoints(samples),maxPoints);
|
|
}
|
|
|
|
export function parseCsvToTelemetryPoints(csv: string, maxPoints = Infinity): TelemetryPoint[] {
|
|
if (!csv || typeof csv !== 'string') return [];
|
|
// MotorSignal's numeric export has no embedded commas; tolerate quoted scalar cells.
|
|
const rows = csv.trim().split(/\r?\n/).map(line=>line.split(',').map(cell=>cell.trim().replace(/^"|"$/g,'')));
|
|
if (rows.length < 2) return [];
|
|
const names = rows[0].map(h=>h.toLowerCase().replace(/^\uFEFF/,''));
|
|
const fields = ['timestamp','latitude','longitude','altitude','speed','heading','gpsAccuracy','leanAngle','pitchAngle','accelX','accelY','accelZ','longitudinalG','lateralG','segmentId'];
|
|
let segmentId = 0;
|
|
const samples = rows.slice(1).map(row => {
|
|
const sample: Record<string, unknown> = {segmentId};
|
|
fields.forEach(field => { const i=names.indexOf(field.toLowerCase()); if(i>=0) sample[field]=row[i]; });
|
|
if (numeric(sample.latitude) == null || numeric(sample.longitude) == null) segmentId++;
|
|
return sample;
|
|
});
|
|
return limitPoints(normalizeTelemetryPoints(samples),maxPoints);
|
|
}
|