Spaces:
Sleeping
Sleeping
File size: 4,071 Bytes
90cbf22 |
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 |
import { Path, PathComponent, Point, Vector, packPathComponent, queryPath } from './types';
export function distance(p0: Point, p1: Point): number {
const dx = p0.x - p1.x;
const dy = p0.y - p1.y;
return Math.sqrt(dx * dx + dy * dy);
}
export function pointsEqual(p0: Point, p1: Point): boolean {
return p0.x == p1.x && p0.y == p1.y;
}
export function manhattanDistance(p0: Point, p1: Point) {
return Math.abs(p0.x - p1.x) + Math.abs(p0.y - p1.y);
}
export function pathOverlaps(path: Path, time: number): boolean {
if (path.length < 2) {
throw new Error(`Invalid path: ${JSON.stringify(path)}`);
}
const start = queryPath(path, 0);
const end = queryPath(path, path.length - 1);
return start.t <= time && time <= end.t;
}
export function pathPosition(
path: Path,
time: number,
): { position: Point; facing: Vector; velocity: number } {
if (path.length < 2) {
throw new Error(`Invalid path: ${JSON.stringify(path)}`);
}
const first = queryPath(path, 0);
if (time < first.t) {
return { position: first.position, facing: first.facing, velocity: 0 };
}
const last = queryPath(path, path.length - 1);
if (last.t < time) {
return { position: last.position, facing: last.facing, velocity: 0 };
}
for (let i = 0; i < path.length - 1; i++) {
const segmentStart = queryPath(path, i);
const segmentEnd = queryPath(path, i + 1);
if (segmentStart.t <= time && time <= segmentEnd.t) {
const interp = (time - segmentStart.t) / (segmentEnd.t - segmentStart.t);
return {
position: {
x: segmentStart.position.x + interp * (segmentEnd.position.x - segmentStart.position.x),
y: segmentStart.position.y + interp * (segmentEnd.position.y - segmentStart.position.y),
},
facing: segmentStart.facing,
velocity:
distance(segmentStart.position, segmentEnd.position) / (segmentEnd.t - segmentStart.t),
};
}
}
throw new Error(`Timestamp checks not exhaustive?`);
}
export const EPSILON = 0.0001;
export function vector(p0: Point, p1: Point): Vector {
const dx = p1.x - p0.x;
const dy = p1.y - p0.y;
return { dx, dy };
}
export function vectorLength(vector: Vector): number {
return Math.sqrt(vector.dx * vector.dx + vector.dy * vector.dy);
}
export function normalize(vector: Vector): Vector | null {
const len = vectorLength(vector);
if (len < EPSILON) {
return null;
}
const { dx, dy } = vector;
return {
dx: dx / len,
dy: dy / len,
};
}
export function orientationDegrees(vector: Vector): number {
if (Math.sqrt(vector.dx * vector.dx + vector.dy * vector.dy) < EPSILON) {
throw new Error(`Can't compute the orientation of too small vector ${JSON.stringify(vector)}`);
}
const twoPi = 2 * Math.PI;
const radians = (Math.atan2(vector.dy, vector.dx) + twoPi) % twoPi;
return (radians / twoPi) * 360;
}
export function compressPath(densePath: PathComponent[]): Path {
const packed = densePath.map(packPathComponent);
if (densePath.length <= 2) {
return densePath.map(packPathComponent);
}
const out = [packPathComponent(densePath[0])];
let last = densePath[0];
let candidate;
for (const point of densePath.slice(1)) {
if (!candidate) {
candidate = point;
continue;
}
// We can skip `candidate` if it interpolates cleanly between
// `last` and `point`.
const { position, facing } = pathPosition(
[packPathComponent(last), packPathComponent(point)],
candidate.t,
);
const positionCloseEnough = distance(position, candidate.position) < EPSILON;
const facingDifference = {
dx: facing.dx - candidate.facing.dx,
dy: facing.dy - candidate.facing.dy,
};
const facingCloseEnough = vectorLength(facingDifference) < EPSILON;
if (positionCloseEnough && facingCloseEnough) {
candidate = point;
continue;
}
out.push(packPathComponent(candidate));
last = candidate;
candidate = point;
}
if (candidate) {
out.push(packPathComponent(candidate));
}
return out;
}
|