Working version of train following spline path
This commit is contained in:
Emma
3
math/constants.ts
Normal file
3
math/constants.ts
Normal file
@@ -0,0 +1,3 @@
|
||||
export const Constants = {
|
||||
TWO_PI: Math.PI * 2
|
||||
}
|
||||
8
math/lerp.ts
Normal file
8
math/lerp.ts
Normal file
@@ -0,0 +1,8 @@
|
||||
import { Vector } from "./vector.ts";
|
||||
|
||||
export const lerp = (a: number, b: number, t: number) => {
|
||||
return (a*t) + (b*(1-t));
|
||||
}
|
||||
|
||||
export const map = (value: number, x1: number, y1: number, x2: number, y2: number) =>
|
||||
(value - x1) * (y2 - x2) / (y1 - x1) + x2;
|
||||
172
math/path.ts
Normal file
172
math/path.ts
Normal file
@@ -0,0 +1,172 @@
|
||||
import { Vector } from "./vector.ts";
|
||||
|
||||
export class ComplexPath {
|
||||
|
||||
points: Vector[] = [];
|
||||
|
||||
radius = 50;
|
||||
|
||||
ctx?: CanvasRenderingContext2D;
|
||||
|
||||
constructor(points?: Vector[]) {
|
||||
points && (this.points = points);
|
||||
}
|
||||
|
||||
setContext(ctx: CanvasRenderingContext2D) {
|
||||
this.ctx = ctx;
|
||||
}
|
||||
|
||||
draw() {
|
||||
if (!this.ctx || !this.points.length) return;
|
||||
const ctx = this.ctx;
|
||||
|
||||
ctx.save();
|
||||
ctx.lineWidth = 2;
|
||||
ctx.strokeStyle = 'white';
|
||||
ctx.setLineDash([21, 6])
|
||||
|
||||
let last = this.points[this.points.length - 1]
|
||||
|
||||
for (const point of this.points) {
|
||||
ctx.beginPath();
|
||||
ctx.moveTo(last.x, last.y);
|
||||
ctx.lineTo(point.x, point.y);
|
||||
ctx.stroke();
|
||||
last = point;
|
||||
}
|
||||
ctx.restore();
|
||||
}
|
||||
}
|
||||
|
||||
export class PathSegment {
|
||||
points: [Vector, Vector, Vector, Vector]
|
||||
ctx?: CanvasRenderingContext2D;
|
||||
|
||||
constructor(points: [Vector, Vector, Vector, Vector]) {
|
||||
this.points = points;
|
||||
}
|
||||
|
||||
setContext(ctx: CanvasRenderingContext2D) {
|
||||
this.ctx = ctx;
|
||||
}
|
||||
|
||||
draw() {
|
||||
if (!this.ctx) return;
|
||||
const ctx = this.ctx;
|
||||
|
||||
ctx.save();
|
||||
ctx.beginPath();
|
||||
ctx.moveTo(this.points[0].x, this.points[0].y);
|
||||
|
||||
ctx.bezierCurveTo(
|
||||
this.points[1].x,
|
||||
this.points[1].y,
|
||||
this.points[2].x,
|
||||
this.points[2].y,
|
||||
this.points[3].x,
|
||||
this.points[3].y,
|
||||
);
|
||||
|
||||
ctx.strokeStyle = '#ffffff50';
|
||||
ctx.lineWidth = 2;
|
||||
ctx.stroke();
|
||||
ctx.restore();
|
||||
}
|
||||
|
||||
getPointAtT(t: number) {
|
||||
const [a, b, c, d] = this.points;
|
||||
const res = a.copy();
|
||||
|
||||
res.add(Vector.add(a.copy().mult(-3), b.copy().mult(3)).mult(t))
|
||||
res.add(Vector.add(Vector.add(a.copy().mult(3), b.copy().mult(-6)), c.copy().mult(3)).mult(Math.pow(t, 2)));
|
||||
res.add(Vector.add(Vector.add(a.copy().mult(-1), b.copy().mult(3)), Vector.add(c.copy().mult(-3), d.copy())).mult(Math.pow(t, 3)));
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
getClosestPoint(v: Vector): [Vector, number, number] {
|
||||
const samples = 25;
|
||||
const resolution = 1 / samples;
|
||||
let closest = this.points[0];
|
||||
let closestDistance = this.points[0].dist(v);
|
||||
let closestT = 0;
|
||||
|
||||
for (let i = 0; i < samples; i++) {
|
||||
const point = this.getPointAtT(i * resolution);
|
||||
const distance = v.dist(point);
|
||||
if (distance < closestDistance) {
|
||||
closest = point;
|
||||
closestDistance = distance;
|
||||
closestT = i * resolution;
|
||||
}
|
||||
}
|
||||
|
||||
return [closest, closestDistance, closestT];
|
||||
}
|
||||
|
||||
getPointsWithinRadius(v: Vector, r: number) {
|
||||
const points: [number, PathSegment][] = [];
|
||||
const samples = 25;
|
||||
const resolution = 1 / samples;
|
||||
|
||||
for (let i = 0; i < samples; i++) {
|
||||
const point = this.getPointAtT(i * resolution);
|
||||
const distance = v.dist(point);
|
||||
if (distance < r) {
|
||||
points.push([i * resolution,this]);
|
||||
}
|
||||
}
|
||||
return points
|
||||
}
|
||||
|
||||
tangent(t: number) {
|
||||
// dP(t) / dt = -3(1-t)^2 * P0 + 3(1-t)^2 * P1 - 6t(1-t) * P1 - 3t^2 * P2 + 6t(1-t) * P2 + 3t^2 * P3
|
||||
const [a, b, c, d] = this.points;
|
||||
|
||||
const res = Vector.sub(b, a).mult(3 * Math.pow(1 - t, 2));
|
||||
res.add(Vector.add(Vector.sub(c, b).mult(6 * (1 - t) * t), Vector.sub(d, c).mult(3 * Math.pow(t, 2))));
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
doesIntersectCircle(x: number, y: number, r: number) {
|
||||
const v = new Vector(x, y);
|
||||
const samples = 25;
|
||||
const resolution = 1 / samples;
|
||||
|
||||
let distance = Infinity;
|
||||
let t;
|
||||
|
||||
for (let i = 0; i < samples; i++) {
|
||||
if (i !== samples - 1) {
|
||||
const a = this.getPointAtT(i * resolution);
|
||||
const b = this.getPointAtT((i + 1) * resolution);
|
||||
const ac = Vector.sub(v, a);
|
||||
const ab = Vector.sub(b, a);
|
||||
|
||||
const d = Vector.add(Vector.vectorProjection(ac, ab), a);
|
||||
const ad = Vector.sub(d, a);
|
||||
|
||||
const k = Math.abs(ab.x) > Math.abs(ab.y) ? ad.x / ab.x : ad.y / ab.y;
|
||||
|
||||
let dist;
|
||||
if (k <= 0.0) {
|
||||
dist = Vector.hypot2(v, a)
|
||||
} else if (k >= 1.0) {
|
||||
dist = Vector.hypot2(v, b)
|
||||
}
|
||||
|
||||
dist = Vector.hypot2(v, d)
|
||||
|
||||
if (dist < distance) {
|
||||
distance = dist;
|
||||
t = i * resolution;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (distance < r) return t;
|
||||
|
||||
return false;
|
||||
}
|
||||
}
|
||||
273
math/vector.ts
Normal file
273
math/vector.ts
Normal file
@@ -0,0 +1,273 @@
|
||||
import { Constants } from "./constants.ts";
|
||||
|
||||
export class Vector {
|
||||
x: number;
|
||||
y: number;
|
||||
z: number;
|
||||
|
||||
constructor(x = 0, y = 0, z = 0) {
|
||||
this.x = x;
|
||||
this.y = y;
|
||||
this.z = z;
|
||||
}
|
||||
|
||||
set(x: number, y: number, z?: number): void;
|
||||
set(v: Vector): void;
|
||||
set(v: [number, number, number]): void;
|
||||
set(v: Vector | [number, number, number] | number, y?: number, z?: number) {
|
||||
if (arguments.length === 1 && typeof v !== "number") {
|
||||
this.set((v as Vector).x || (v as Array<number>)[0] || 0,
|
||||
(v as Vector).y || (v as Array<number>)[1] || 0,
|
||||
(v as Vector).z || (v as Array<number>)[2] || 0);
|
||||
} else {
|
||||
this.x = v as number;
|
||||
this.y = y || 0;
|
||||
this.z = z || 0;
|
||||
}
|
||||
}
|
||||
get() {
|
||||
return new Vector(this.x, this.y, this.z);
|
||||
}
|
||||
mag() {
|
||||
const x = this.x,
|
||||
y = this.y,
|
||||
z = this.z;
|
||||
return Math.sqrt(x * x + y * y + z * z);
|
||||
}
|
||||
magSq() {
|
||||
const x = this.x,
|
||||
y = this.y,
|
||||
z = this.z;
|
||||
return (x * x + y * y + z * z);
|
||||
}
|
||||
setMag(len: number): void;
|
||||
setMag(v: Vector, len: number): Vector
|
||||
setMag(v_or_len: Vector | number, len?: number) {
|
||||
if (len === undefined) {
|
||||
len = v_or_len as number;
|
||||
this.normalize();
|
||||
this.mult(len);
|
||||
} else {
|
||||
const v = v_or_len as Vector;
|
||||
v.normalize();
|
||||
v.mult(len);
|
||||
return v;
|
||||
}
|
||||
}
|
||||
add(x: number, y: number, z: number): void;
|
||||
add(x: number, y: number): void;
|
||||
add(v: Vector): void;
|
||||
add(v: Vector | number, y?: number, z?: number) {
|
||||
if (arguments.length === 1 && typeof v !== 'number') {
|
||||
this.x += v.x;
|
||||
this.y += v.y;
|
||||
this.z += v.z;
|
||||
} else if (arguments.length === 2) {
|
||||
// 2D Vector
|
||||
this.x += v as number;
|
||||
this.y += y ?? 0;
|
||||
} else {
|
||||
this.x += v as number;
|
||||
this.y += y ?? 0;
|
||||
this.z += z ?? 0;
|
||||
}
|
||||
}
|
||||
sub(x: number, y: number, z: number): void;
|
||||
sub(x: number, y: number): void;
|
||||
sub(v: Vector): void;
|
||||
sub(v: Vector | number, y?: number, z?: number) {
|
||||
if (arguments.length === 1 && typeof v !== 'number') {
|
||||
this.x -= v.x;
|
||||
this.y -= v.y;
|
||||
this.z -= v.z;
|
||||
} else if (arguments.length === 2) {
|
||||
// 2D Vector
|
||||
this.x -= v as number;
|
||||
this.y -= y ?? 0;
|
||||
} else {
|
||||
this.x -= v as number;
|
||||
this.y -= y ?? 0;
|
||||
this.z -= z ?? 0;
|
||||
}
|
||||
}
|
||||
mult(v: number | Vector) {
|
||||
if (typeof v === 'number') {
|
||||
this.x *= v;
|
||||
this.y *= v;
|
||||
this.z *= v;
|
||||
} else {
|
||||
this.x *= v.x;
|
||||
this.y *= v.y;
|
||||
this.z *= v.z;
|
||||
}
|
||||
return this;
|
||||
}
|
||||
div(v: number | Vector) {
|
||||
if (typeof v === 'number') {
|
||||
this.x /= v;
|
||||
this.y /= v;
|
||||
this.z /= v;
|
||||
} else {
|
||||
this.x /= v.x;
|
||||
this.y /= v.y;
|
||||
this.z /= v.z;
|
||||
}
|
||||
}
|
||||
rotate(angle: number) {
|
||||
const prev_x = this.x;
|
||||
const c = Math.cos(angle);
|
||||
const s = Math.sin(angle);
|
||||
this.x = c * this.x - s * this.y;
|
||||
this.y = s * prev_x + c * this.y;
|
||||
}
|
||||
dist(v: Vector) {
|
||||
const dx = this.x - v.x,
|
||||
dy = this.y - v.y,
|
||||
dz = this.z - v.z;
|
||||
return Math.sqrt(dx * dx + dy * dy + dz * dz);
|
||||
}
|
||||
dot(x: number, y: number, z: number): number;
|
||||
dot(v: Vector): number;
|
||||
dot(v: Vector | number, y?: number, z?: number) {
|
||||
if (arguments.length === 1 && typeof v !== 'number') {
|
||||
return (this.x * v.x + this.y * v.y + this.z * v.z);
|
||||
}
|
||||
return (this.x * (v as number) + this.y * y! + this.z * z!);
|
||||
}
|
||||
cross(v: Vector) {
|
||||
const x = this.x,
|
||||
y = this.y,
|
||||
z = this.z;
|
||||
return new Vector(y * v.z - v.y * z,
|
||||
z * v.x - v.z * x,
|
||||
x * v.y - v.x * y);
|
||||
}
|
||||
lerp(x: number, y: number, z: number): void;
|
||||
lerp(v: Vector, amt: number): void;
|
||||
lerp(v_or_x: Vector | number, amt_or_y: number, z?: number, amt?: number) {
|
||||
const lerp_val = (start: number, stop: number, amt: number) => {
|
||||
return start + (stop - start) * amt;
|
||||
};
|
||||
let x, y: number;
|
||||
if (arguments.length === 2 && typeof v_or_x !== 'number') {
|
||||
// given vector and amt
|
||||
amt = amt_or_y;
|
||||
x = v_or_x.x;
|
||||
y = v_or_x.y;
|
||||
z = v_or_x.z;
|
||||
} else {
|
||||
// given x, y, z and amt
|
||||
x = v_or_x as number;
|
||||
y = amt_or_y;
|
||||
}
|
||||
this.x = lerp_val(this.x, x, amt!);
|
||||
this.y = lerp_val(this.y, y, amt!);
|
||||
this.z = lerp_val(this.z, z!, amt!);
|
||||
}
|
||||
normalize() {
|
||||
const m = this.mag();
|
||||
if (m > 0) {
|
||||
this.div(m);
|
||||
}
|
||||
return this;
|
||||
}
|
||||
limit(high: number) {
|
||||
if (this.mag() > high) {
|
||||
this.normalize();
|
||||
this.mult(high);
|
||||
}
|
||||
}
|
||||
heading() {
|
||||
return (-Math.atan2(-this.y, this.x));
|
||||
}
|
||||
heading2D() {
|
||||
return this.heading();
|
||||
}
|
||||
toString() {
|
||||
return "[" + this.x + ", " + this.y + ", " + this.z + "]";
|
||||
}
|
||||
array() {
|
||||
return [this.x, this.y, this.z];
|
||||
}
|
||||
|
||||
copy() {
|
||||
return new Vector(this.x, this.y, this.z);
|
||||
}
|
||||
|
||||
drawDot(ctx: CanvasRenderingContext2D) {
|
||||
// ctx.fillStyle = 'red'
|
||||
ctx.beginPath();
|
||||
ctx.arc(this.x, this.y, 2, 0, Constants.TWO_PI);
|
||||
ctx.fill();
|
||||
}
|
||||
|
||||
static fromAngle(angle: number, v?: Vector) {
|
||||
if (v === undefined || v === null) {
|
||||
v = new Vector();
|
||||
}
|
||||
v.x = Math.cos(angle);
|
||||
v.y = Math.sin(angle);
|
||||
return v;
|
||||
}
|
||||
|
||||
static random2D(v?: Vector) {
|
||||
return Vector.fromAngle(Math.random() * (Math.PI * 2), v);
|
||||
}
|
||||
|
||||
static random3D(v: Vector) {
|
||||
const angle = Math.random() * Constants.TWO_PI;
|
||||
const vz = Math.random() * 2 - 1;
|
||||
const mult = Math.sqrt(1 - vz * vz);
|
||||
const vx = mult * Math.cos(angle);
|
||||
const vy = mult * Math.sin(angle);
|
||||
if (v === undefined || v === null) {
|
||||
v = new Vector(vx, vy, vz);
|
||||
} else {
|
||||
v.set(vx, vy, vz);
|
||||
}
|
||||
return v;
|
||||
}
|
||||
|
||||
static dist(v1: Vector, v2: Vector) {
|
||||
return v1.dist(v2);
|
||||
}
|
||||
|
||||
static dot(v1: Vector, v2: Vector) {
|
||||
return v1.dot(v2);
|
||||
}
|
||||
|
||||
static cross(v1: Vector, v2: Vector) {
|
||||
return v1.cross(v2);
|
||||
}
|
||||
|
||||
static add(v1: Vector, v2: Vector) {
|
||||
return new Vector(v1.x + v2.x, v1.y + v2.y, v1.z + v2.z);
|
||||
}
|
||||
|
||||
static sub(v1: Vector, v2: Vector) {
|
||||
return new Vector(v1.x - v2.x, v1.y - v2.y, v1.z - v2.z);
|
||||
}
|
||||
|
||||
static angleBetween(v1: Vector, v2: Vector) {
|
||||
return Math.acos(v1.dot(v2) / Math.sqrt(v1.magSq() * v2.magSq()));
|
||||
}
|
||||
|
||||
static lerp(v1: Vector, v2: Vector, amt: number) {
|
||||
// non-static lerp mutates object, but this version returns a new vector
|
||||
const retval = new Vector(v1.x, v1.y, v1.z);
|
||||
retval.lerp(v2, amt);
|
||||
return retval;
|
||||
}
|
||||
|
||||
static vectorProjection(v1: Vector, v2: Vector) {
|
||||
v2 = v2.copy();
|
||||
v2.normalize();
|
||||
const sp = v1.dot(v2);
|
||||
v2.mult(sp);
|
||||
return v2;
|
||||
}
|
||||
|
||||
static hypot2(a: Vector, b: Vector) {
|
||||
return Vector.dot(Vector.sub(a,b), Vector.sub(a,b))
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user