104 lines
3.5 KiB
TypeScript
104 lines
3.5 KiB
TypeScript
import {
|
|
DEFAULT_PREDICTION_OPTIONS,
|
|
type HistoryPoint,
|
|
type PredictionOptions,
|
|
type Rotation,
|
|
} from "../types.js";
|
|
|
|
export class RotationPredictor {
|
|
private history: HistoryPoint<Rotation>[] = [];
|
|
private options: PredictionOptions;
|
|
private lastPrediction: Rotation | null = null;
|
|
private lastPredictionError = 0;
|
|
|
|
constructor(options: PredictionOptions = {}) {
|
|
this.options = { ...DEFAULT_PREDICTION_OPTIONS, ...options };
|
|
}
|
|
|
|
addRotation(rotation: Rotation, timestamp: number): void {
|
|
this.history.push({ value: { ...rotation }, timestamp });
|
|
const historySize =
|
|
this.options.historySize || DEFAULT_PREDICTION_OPTIONS.historySize;
|
|
if (this.history.length > historySize) {
|
|
this.history.shift();
|
|
}
|
|
if (this.lastPrediction && this.history.length >= 2) {
|
|
this.lastPredictionError = Math.sqrt(
|
|
this.angleDifference(this.lastPrediction.yaw, rotation.yaw) **
|
|
2 +
|
|
this.angleDifference(
|
|
this.lastPrediction.pitch,
|
|
rotation.pitch
|
|
) **
|
|
2
|
|
);
|
|
}
|
|
}
|
|
|
|
private angleDifference(a: number, b: number): number {
|
|
let diff = ((b - a + 180) % 360) - 180;
|
|
if (diff < -180) diff += 360;
|
|
return diff;
|
|
}
|
|
|
|
predictRotation(deltaTime: number): Rotation | null {
|
|
if (this.history.length < 2) return null;
|
|
const maxTime =
|
|
this.options.maxPredictionTime ||
|
|
DEFAULT_PREDICTION_OPTIONS.maxPredictionTime;
|
|
const limitedDeltaTime = Math.min(deltaTime, maxTime);
|
|
const dt = limitedDeltaTime / 1000;
|
|
const prediction = this.linearPredictRotation(dt);
|
|
this.lastPrediction = { ...prediction };
|
|
return prediction;
|
|
}
|
|
|
|
private linearPredictRotation(dt: number): Rotation {
|
|
const lastItem = this.history[this.history.length - 1];
|
|
if (!lastItem)
|
|
throw new Error("History item required for rotation prediction");
|
|
const last = lastItem.value;
|
|
const angularVelocity = this.calculateAngularVelocity();
|
|
return {
|
|
yaw: this.normalizeAngle(last.yaw + angularVelocity.yaw * dt),
|
|
pitch: this.clampPitch(last.pitch + angularVelocity.pitch * dt),
|
|
};
|
|
}
|
|
|
|
private calculateAngularVelocity(): Rotation {
|
|
if (this.history.length < 2) return { yaw: 0, pitch: 0 };
|
|
const last = this.history[this.history.length - 1];
|
|
const prev = this.history[this.history.length - 2];
|
|
if (!last || !prev) return { yaw: 0, pitch: 0 };
|
|
const dt = (last.timestamp - prev.timestamp) / 1000;
|
|
if (dt <= 0) return { yaw: 0, pitch: 0 };
|
|
return {
|
|
yaw: this.angleDifference(prev.value.yaw, last.value.yaw) / dt,
|
|
pitch:
|
|
this.angleDifference(prev.value.pitch, last.value.pitch) / dt,
|
|
};
|
|
}
|
|
|
|
private normalizeAngle(angle: number): number {
|
|
return ((angle + 180) % 360) - 180;
|
|
}
|
|
|
|
private clampPitch(pitch: number): number {
|
|
return Math.max(-90, Math.min(90, pitch));
|
|
}
|
|
|
|
getPredictionError(): number {
|
|
return this.lastPredictionError;
|
|
}
|
|
|
|
clearHistory(): void {
|
|
this.history = [];
|
|
this.lastPrediction = null;
|
|
this.lastPredictionError = 0;
|
|
}
|
|
|
|
setOptions(options: PredictionOptions): void {
|
|
this.options = { ...this.options, ...options };
|
|
}
|
|
}
|