Working version of train following spline path
This commit is contained in:
Emma
158
physics/follower.ts
Normal file
158
physics/follower.ts
Normal file
@@ -0,0 +1,158 @@
|
||||
import { Constants } from "../math/constants.ts";
|
||||
import { map } from "../math/lerp.ts";
|
||||
import { ComplexPath, PathSegment } from "../math/path.ts";
|
||||
import { Vector } from "../math/vector.ts";
|
||||
import { Mover } from "./mover.ts";
|
||||
|
||||
export class
|
||||
Follower extends Mover {
|
||||
debug = true;
|
||||
|
||||
follow(toFollow: ComplexPath | PathSegment) {
|
||||
if (toFollow instanceof ComplexPath) {
|
||||
const predict = this.velocity.copy();
|
||||
predict.normalize();
|
||||
predict.mult(25);
|
||||
const predictpos = Vector.add(this.position, predict)
|
||||
|
||||
if (this.ctx)
|
||||
Mover.edges(predict, this.ctx.canvas.width, this.ctx.canvas.height)
|
||||
let normal = null;
|
||||
let target = null;
|
||||
let worldRecord = 1000000;
|
||||
|
||||
for (let i = 0; i < toFollow.points.length; i++) {
|
||||
// Look at a line segment
|
||||
let a = toFollow.points[i];
|
||||
let b = toFollow.points[(i + 1) % toFollow.points.length]; // Note Path has to wraparound
|
||||
|
||||
// Get the normal point to that line
|
||||
let normalPoint = getNormalPoint(predictpos, a, b);
|
||||
|
||||
// Check if normal is on line segment
|
||||
let dir = Vector.sub(b, a);
|
||||
// If it's not within the line segment, consider the normal to just be the end of the line segment (point b)
|
||||
//if (da + db > line.mag()+1) {
|
||||
if (
|
||||
normalPoint.x < Math.min(a.x, b.x) ||
|
||||
normalPoint.x > Math.max(a.x, b.x) ||
|
||||
normalPoint.y < Math.min(a.y, b.y) ||
|
||||
normalPoint.y > Math.max(a.y, b.y)
|
||||
) {
|
||||
normalPoint = b.copy();
|
||||
// If we're at the end we really want the next line segment for looking ahead
|
||||
a = toFollow.points[(i + 1) % toFollow.points.length];
|
||||
b = toFollow.points[(i + 2) % toFollow.points.length]; // Path wraps around
|
||||
dir = Vector.sub(b, a);
|
||||
}
|
||||
|
||||
// How far away are we from the path?
|
||||
const d = Vector.dist(predictpos, normalPoint);
|
||||
// Did we beat the worldRecord and find the closest line segment?
|
||||
if (d < worldRecord) {
|
||||
worldRecord = d;
|
||||
normal = normalPoint;
|
||||
|
||||
// Look at the direction of the line segment so we can seek a little bit ahead of the normal
|
||||
dir.normalize();
|
||||
// This is an oversimplification
|
||||
// Should be based on distance to path & velocity
|
||||
dir.mult(25);
|
||||
target = normal.copy();
|
||||
target.add(dir);
|
||||
}
|
||||
|
||||
if (worldRecord > toFollow.radius) {
|
||||
return this.seek(target!);
|
||||
}
|
||||
}
|
||||
|
||||
if (this.debug && this.ctx) {
|
||||
// Draw predicted future position
|
||||
this.ctx.strokeStyle = 'red';
|
||||
this.ctx.fillStyle = 'pink';
|
||||
this.ctx.beginPath();
|
||||
this.ctx.moveTo(this.position.x, this.position.y)
|
||||
this.ctx.lineTo(predictpos.x, predictpos.y);
|
||||
this.ctx.stroke();
|
||||
|
||||
this.ctx.beginPath();
|
||||
this.ctx.arc(predictpos.x, predictpos.y, 4, 0, Constants.TWO_PI);
|
||||
this.ctx.fill();
|
||||
this.ctx.stroke();
|
||||
|
||||
// Draw normal position
|
||||
this.ctx.beginPath();
|
||||
this.ctx.arc(normal!.x, normal!.y, 4, 0, Constants.TWO_PI);
|
||||
this.ctx.fill();
|
||||
this.ctx.stroke();
|
||||
|
||||
// Draw actual target (red if steering towards it)
|
||||
this.ctx.beginPath();
|
||||
this.ctx.moveTo(predictpos.x, predictpos.y)
|
||||
this.ctx.lineTo(target!.x, target!.y);
|
||||
this.ctx.stroke();
|
||||
|
||||
// if (worldRecord > toFollow.radius) fill(255, 0, 0);
|
||||
// noStroke();
|
||||
|
||||
this.ctx.beginPath();
|
||||
this.ctx.arc(target!.x, target!.y, 8, 0, Constants.TWO_PI);
|
||||
this.ctx.fill();
|
||||
this.ctx.stroke();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
seek(target: Vector, strength: number = 1) {
|
||||
const desired = Vector.sub(target, this.position);
|
||||
desired.normalize();
|
||||
desired.mult(this.maxSpeed);
|
||||
|
||||
const steer = Vector.sub(desired, this.velocity);
|
||||
steer.limit(this.maxForce);
|
||||
|
||||
this.applyForce(steer.mult(strength));
|
||||
}
|
||||
|
||||
link(target: Mover) {
|
||||
// const desired = target.velocity.copy();
|
||||
// desired.normalize();
|
||||
// desired.mult(-distance);
|
||||
|
||||
// const predicted = Vector.add(target.position, desired);
|
||||
this.position = target.trailingPoint;
|
||||
|
||||
// const lastVel = this.velocity.copy();
|
||||
this.seek(target.trailingPoint);
|
||||
// this.velocity = target.velocity;
|
||||
}
|
||||
|
||||
arrive(target: Vector) {
|
||||
// const predicted = Vector.add(this.position, this.velocity.copy().normalize().mult(25));
|
||||
const desired = Vector.sub(target, this.position);
|
||||
const d = desired.mag();
|
||||
let speed = this.maxSpeed;
|
||||
if (d < 10) {
|
||||
speed = map(d, 0, 100, 0, this.maxSpeed);
|
||||
}
|
||||
desired.setMag(speed);
|
||||
|
||||
const steer = Vector.sub(desired, this.velocity);
|
||||
steer.limit(this.maxForce);
|
||||
|
||||
this.applyForce(steer);
|
||||
}
|
||||
}
|
||||
|
||||
function getNormalPoint(p: Vector, a: Vector, b: Vector) {
|
||||
// Vector from a to p
|
||||
const ap = Vector.sub(p, a);
|
||||
// Vector from a to b
|
||||
const ab = Vector.sub(b, a);
|
||||
ab.normalize(); // Normalize the line
|
||||
// Project vector "diff" onto line by using the dot product
|
||||
ab.mult(ap.dot(ab));
|
||||
const normalPoint = Vector.add(a, ab);
|
||||
return normalPoint;
|
||||
}
|
||||
112
physics/mover.ts
Normal file
112
physics/mover.ts
Normal file
@@ -0,0 +1,112 @@
|
||||
import { Vector } from "../math/vector.ts";
|
||||
|
||||
export class Mover {
|
||||
position: Vector;
|
||||
velocity: Vector;
|
||||
acceleration: Vector;
|
||||
maxSpeed: number;
|
||||
maxForce: number;
|
||||
_trailingPoint: number;
|
||||
protected _leadingPoint: number;
|
||||
|
||||
get trailingPoint() {
|
||||
const desired = this.velocity.copy();
|
||||
desired.normalize();
|
||||
desired.mult(-this._trailingPoint);
|
||||
|
||||
return Vector.add(this.position, desired);
|
||||
}
|
||||
get leadingPoint() {
|
||||
const desired = this.velocity.copy();
|
||||
desired.normalize();
|
||||
desired.mult(this._leadingPoint);
|
||||
|
||||
return Vector.add(this.position, desired);
|
||||
}
|
||||
|
||||
ctx?: CanvasRenderingContext2D;
|
||||
|
||||
boundingBox: {
|
||||
pos: Vector;
|
||||
size: Vector;
|
||||
}
|
||||
|
||||
constructor();
|
||||
constructor(random: boolean);
|
||||
constructor(pos?: Vector, vel?: Vector, acc?: Vector);
|
||||
constructor(posOrRandom?: Vector | boolean, vel?: Vector, acc?: Vector) {
|
||||
if (typeof posOrRandom === 'boolean' && posOrRandom) {
|
||||
this.position = Vector.random2D(new Vector());
|
||||
this.velocity = Vector.random2D(new Vector());
|
||||
this.acceleration = new Vector()
|
||||
} else {
|
||||
this.position = posOrRandom || new Vector();
|
||||
this.velocity = vel || new Vector();
|
||||
this.acceleration = acc || new Vector()
|
||||
}
|
||||
this.boundingBox = {
|
||||
size: new Vector(20, 10),
|
||||
pos: new Vector(this.position.x - 10, this.position.y - 5)
|
||||
}
|
||||
|
||||
this.maxSpeed = 3;
|
||||
this.maxForce = .3;
|
||||
|
||||
this._trailingPoint = 0;
|
||||
this._leadingPoint = 0;
|
||||
|
||||
this.init();
|
||||
}
|
||||
|
||||
init() {
|
||||
//
|
||||
}
|
||||
|
||||
move() {
|
||||
this.velocity.limit(this.maxSpeed);
|
||||
this.acceleration.limit(this.maxForce);
|
||||
this.velocity.add(this.acceleration);
|
||||
this.position.add(this.velocity);
|
||||
this.edges();
|
||||
this.draw();
|
||||
}
|
||||
|
||||
edges() {
|
||||
if (!this.ctx) return;
|
||||
|
||||
if (this.position.x > this.ctx.canvas.width) this.position.x = 0;
|
||||
if (this.position.y > this.ctx.canvas.height) this.position.y = 0;
|
||||
if (this.position.x < 0) this.position.x = this.ctx.canvas.width;
|
||||
if (this.position.y < 0) this.position.y = this.ctx.canvas.height;
|
||||
}
|
||||
|
||||
draw() {
|
||||
if (!this.ctx) return;
|
||||
|
||||
this.ctx.fillStyle = 'white'
|
||||
this.ctx.save();
|
||||
this.ctx.translate(this.position.x, this.position.y);
|
||||
this.ctx.rotate(this.velocity.heading() || 0);
|
||||
this.ctx.translate(-this.position.x, -this.position.y);
|
||||
// this.ctx.rotate(Math.PI)
|
||||
// this.ctx.rotate(.5);
|
||||
this.ctx.translate(-(this.boundingBox.size.x / 2), -(this.boundingBox.size.y / 2));
|
||||
this.ctx.fillRect(this.position.x, this.position.y, this.boundingBox.size.x, this.boundingBox.size.y);
|
||||
this.ctx.restore();
|
||||
}
|
||||
|
||||
setContext(ctx: CanvasRenderingContext2D) {
|
||||
this.ctx = ctx;
|
||||
}
|
||||
|
||||
applyForce(force: Vector) {
|
||||
this.acceleration.add(force);
|
||||
}
|
||||
|
||||
static edges(point: Vector, width: number, height: number) {
|
||||
if (point.x > width) point.x = 0;
|
||||
if (point.y > height) point.y = 0;
|
||||
if (point.x < 0) point.x = width;
|
||||
if (point.y < 0) point.y = height;
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user